Matrix HaloRenderingThread::getOrientationMatrix(CrystalDescriptor::OrientationType orientation) { Matrix m; switch (orientation) { case CrystalDescriptor::OT_RANDOM: { /* First rotate the crystal around the Z axis:*/ Matrix zRot = createRotationMatrix(Vector3(0, 0, 1), randFloat(-3.14159265, 3.14159265)); /* Then rotate the Z axis into an uniformly distributed position*/ const Vector3 newAxis = getRandomVector(); Vector3 rotAxis = Vector3(0, 0, 1) % newAxis; Matrix axisRot = getIdentityMatrix(); if (rotAxis * rotAxis != 0) { rotAxis /= ~rotAxis; double rotAngle = acos(Vector3(0, 0, 1) * newAxis); axisRot = createRotationMatrix(rotAxis, rotAngle); } return zRot % axisRot; } break; case CrystalDescriptor::OT_PARRY: { /* For hexagonal prisms, one side face is horizontal. */ m = createRotationMatrix(Vector3(0, 1, 0), randFloat(0, 3.1415926536)); } break; case CrystalDescriptor::OT_COLUMN: { /* Main axis horizontal. */ Matrix n = createRotationMatrix(Vector3(0, 0, 1), randFloat(0, 3.1415926536)); Matrix o = createRotationMatrix(Vector3(0, 1, 0), randFloat(0, 3.1415926536)); m = n % o; } break; case CrystalDescriptor::OT_PLATE: { /* Main axis vertical. */ m = createRotationMatrix(Vector3(1, 0, 0), 3.14159265636 * 0.5) % createRotationMatrix(Vector3(0, 1, 0), randFloat(0, 3.1415926536)); } break; case CrystalDescriptor::OT_LOWITZ: { /* Rotation around a diagonal axis. The axis is horizontal.*/ m = createRotationMatrix(Vector3(1, 0, 0), 3.14159265636 * randFloat(-3.1415 * 0.5, 3.1415 * 0.5)) % createRotationMatrix(Vector3(0, 1, 0), randFloat(0, 3.1415926536)); } break; default: { sendNotifyString(wxT("Invalid orientation type."), 1); } } return m; }
//first create a translation matrix that moves cube to (0,0,0) //create rotation matrix from cube's axis, then multiply with translation matrix //create a translation matrix that moves back to orginal position, multiply that //matrix with the one above. Then multiply all vertices with final matrix void rotateCube(Cube *cube){ float translationMatrix[16]; float rotationMatrix[16]; float tempMatrix[16]; float finalMatrix[16]; createTranslationMatrix(-cube->center[0], -cube->center[1], -cube->center[2], translationMatrix); createRotationMatrix(rotationMatrix, cube->angle, cube->axis); mat4MultMat4Float(rotationMatrix, translationMatrix, tempMatrix); createTranslationMatrix(cube->center[0], cube->center[1], cube->center[2], translationMatrix); mat4MultMat4Float(translationMatrix, tempMatrix, finalMatrix); for(int i = 0; i < 8; i++){ mat4MultVec4Float(finalMatrix, cube->vertices[i], cube->vertices[i]); cube->vertices[i][3] = 1.0; } }
Matrix3D<float> getViewMatrix() const { // The viewMatrix will be used to modify the vertex coordinates in order // to transform object coordinates as viewed-by-camera (or eye) coordinates. // Standard x,y,z values are used. Obviously the z value used here will have // to be in the range near side < z < far side as defined by the frustum used // in the projection matrix (see getProjectionMatrix() above). Matrix3D<float> viewMatrix (Vector3D<float> (0.0f, 0.0f, -50.0f)); // The rotation matrix will be applied on each frame. // The vector passed here contains the Euler angle values for each axis // The empiric values used as params will create a slight but constant tilting // on the x-axis, a periodic rotation on the y-axis and no rotation on the // z-axis. Matrix3D<float> rotationMatrix = createRotationMatrix (Vector3D<float> (-0.3f, 5.0f * std::sin (getFrameCounter() * 0.01f), 0.0f)); return rotationMatrix * viewMatrix; }
int main(void) { /////////////////////////////////////////////////////////////////////////// uint32_t currentTime; systemReady = false; systemInit(); systemReady = true; evrPush(EVR_StartingMain, 0); while (1) { evrCheck(); /////////////////////////////// if (frame_50Hz) { frame_50Hz = false; currentTime = micros(); deltaTime50Hz = currentTime - previous50HzTime; previous50HzTime = currentTime; processFlightCommands(); if (eepromConfig.useMs5611 == true) { if (newTemperatureReading && newPressureReading) { d1Value = d1.value; d2Value = d2.value; calculateMs5611Temperature(); calculateMs5611PressureAltitude(); newTemperatureReading = false; newPressureReading = false; } } else { if (newTemperatureReading && newPressureReading) { uncompensatedTemperatureValue = uncompensatedTemperature.value; uncompensatedPressureValue = uncompensatedPressure.value; calculateBmp085Temperature(); calculateBmp085PressureAltitude(); newTemperatureReading = false; newPressureReading = false; } } sensors.pressureAlt50Hz = firstOrderFilter(sensors.pressureAlt50Hz, &firstOrderFilters[PRESSURE_ALT_LOWPASS]); executionTime50Hz = micros() - currentTime; } /////////////////////////////// if (frame_10Hz) { frame_10Hz = false; currentTime = micros(); deltaTime10Hz = currentTime - previous10HzTime; previous10HzTime = currentTime; sensors.mag10Hz[XAXIS] = -((float)rawMag[XAXIS].value * magScaleFactor[XAXIS] - eepromConfig.magBias[XAXIS]); sensors.mag10Hz[YAXIS] = (float)rawMag[YAXIS].value * magScaleFactor[YAXIS] - eepromConfig.magBias[YAXIS]; sensors.mag10Hz[ZAXIS] = -((float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS] - eepromConfig.magBias[ZAXIS]); newMagData = false; magDataUpdate = true; batMonTick(); cliCom(); if (eepromConfig.mavlinkEnabled == true) { mavlinkSendAttitude(); mavlinkSendVfrHud(); } executionTime10Hz = micros() - currentTime; } /////////////////////////////// if (frame_500Hz) { frame_500Hz = false; currentTime = micros(); deltaTime500Hz = currentTime - previous500HzTime; previous500HzTime = currentTime; dt500Hz = (float)deltaTime500Hz * 0.000001f; // For integrations in 500 Hz loop if (eepromConfig.useMpu6050 == true) { computeMpu6050TCBias(); sensors.accel500Hz[XAXIS] = ((float)accelData500Hz[XAXIS] - accelTCBias[XAXIS]) * MPU6050_ACCEL_SCALE_FACTOR; sensors.accel500Hz[YAXIS] = -((float)accelData500Hz[YAXIS] - accelTCBias[YAXIS]) * MPU6050_ACCEL_SCALE_FACTOR; sensors.accel500Hz[ZAXIS] = -((float)accelData500Hz[ZAXIS] - accelTCBias[ZAXIS]) * MPU6050_ACCEL_SCALE_FACTOR; sensors.gyro500Hz[ROLL ] = ((float)gyroData500Hz[ROLL ] - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * MPU6050_GYRO_SCALE_FACTOR; sensors.gyro500Hz[PITCH] = -((float)gyroData500Hz[PITCH] - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * MPU6050_GYRO_SCALE_FACTOR; sensors.gyro500Hz[YAW ] = -((float)gyroData500Hz[YAW ] - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * MPU6050_GYRO_SCALE_FACTOR; } else { sensors.accel500Hz[XAXIS] = -((float)accelData500Hz[XAXIS] - eepromConfig.accelBias[XAXIS]) * eepromConfig.accelScaleFactor[XAXIS]; sensors.accel500Hz[YAXIS] = -((float)accelData500Hz[YAXIS] - eepromConfig.accelBias[YAXIS]) * eepromConfig.accelScaleFactor[YAXIS]; sensors.accel500Hz[ZAXIS] = -((float)accelData500Hz[ZAXIS] - eepromConfig.accelBias[ZAXIS]) * eepromConfig.accelScaleFactor[ZAXIS]; // HJI sensors.accel500Hz[XAXIS] = firstOrderFilter(sensors.accel500Hz[XAXIS], &firstOrderFilters[ACCEL500HZ_X_LOWPASS]); // HJI sensors.accel500Hz[YAXIS] = firstOrderFilter(sensors.accel500Hz[YAXIS], &firstOrderFilters[ACCEL500HZ_Y_LOWPASS]); // HJI sensors.accel500Hz[ZAXIS] = firstOrderFilter(sensors.accel500Hz[ZAXIS], &firstOrderFilters[ACCEL500HZ_Z_LOWPASS]); computeMpu3050TCBias(); sensors.gyro500Hz[ROLL ] = ((float)gyroData500Hz[ROLL ] - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * MPU3050_GYRO_SCALE_FACTOR; sensors.gyro500Hz[PITCH] = -((float)gyroData500Hz[PITCH] - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * MPU3050_GYRO_SCALE_FACTOR; sensors.gyro500Hz[YAW ] = -((float)gyroData500Hz[YAW ] - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * MPU3050_GYRO_SCALE_FACTOR; } MargAHRSupdate( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW], sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], magDataUpdate, dt500Hz ); magDataUpdate = false; computeAxisCommands(dt500Hz); mixTable(); writeMotors(); if (eepromConfig.receiverType == SPEKTRUM) writeServos(); executionTime500Hz = micros() - currentTime; } /////////////////////////////// if (frame_100Hz) { frame_100Hz = false; currentTime = micros(); deltaTime100Hz = currentTime - previous100HzTime; previous100HzTime = currentTime; dt100Hz = (float)deltaTime100Hz * 0.000001f; // For integrations in 100 Hz loop sensors.accel100Hz[XAXIS] = sensors.accel500Hz[XAXIS]; // No sensor averaging so use the 500 Hz value sensors.accel100Hz[YAXIS] = sensors.accel500Hz[YAXIS]; // No sensor averaging so use the 500 Hz value sensors.accel100Hz[ZAXIS] = sensors.accel500Hz[ZAXIS]; // No sensor averaging so use the 500 Hz value // HJI sensors.accel100Hz[XAXIS] = firstOrderFilter(sensors.accel100Hz[XAXIS], &firstOrderFilters[ACCEL100HZ_X_LOWPASS]); // HJI sensors.accel100Hz[YAXIS] = firstOrderFilter(sensors.accel100Hz[YAXIS], &firstOrderFilters[ACCEL100HZ_Y_LOWPASS]); // HJI sensors.accel100Hz[ZAXIS] = firstOrderFilter(sensors.accel100Hz[ZAXIS], &firstOrderFilters[ACCEL100HZ_Z_LOWPASS]); createRotationMatrix(); bodyAccelToEarthAccel(); vertCompFilter(dt100Hz); if (armed == true) { if ( eepromConfig.activeTelemetry == 1 ) { // 500 Hz Accels telemPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS]); } if ( eepromConfig.activeTelemetry == 2 ) { // 500 Hz Gyros telemPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW ]); } if ( eepromConfig.activeTelemetry == 4 ) { // 500 Hz Attitudes telemPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ], sensors.attitude500Hz[PITCH], sensors.attitude500Hz[YAW ]); } if ( eepromConfig.activeTelemetry == 8 ) { // Vertical Variables telemPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", earthAxisAccels[ZAXIS], sensors.pressureAlt50Hz, hDotEstimate, hEstimate); } if ( eepromConfig.activeTelemetry == 16 ) { // Vertical Variables telemPortPrintF("%9.4f, %9.4f, %9.4f,%1d, %9.4f, %9.4f\n", verticalVelocityCmd, hDotEstimate, hEstimate, verticalModeState, throttleCmd, eepromConfig.PID[HDOT_PID].iTerm); } } executionTime100Hz = micros() - currentTime; } /////////////////////////////// if (frame_5Hz) { frame_5Hz = false; currentTime = micros(); deltaTime5Hz = currentTime - previous5HzTime; previous5HzTime = currentTime; if (batMonVeryLowWarning > 0) { BEEP_TOGGLE; batMonVeryLowWarning--; } executionTime5Hz = micros() - currentTime; } /////////////////////////////// if (frame_1Hz) { frame_1Hz = false; currentTime = micros(); deltaTime1Hz = currentTime - previous1HzTime; previous1HzTime = currentTime; if (execUp == false) execUpCount++; if ((execUpCount == 5) && (execUp == false)) { execUp = true; LED0_OFF; LED1_OFF; pwmEscInit(); homeData.magHeading = sensors.attitude500Hz[YAW]; } if (batMonLowWarning > 0) { BEEP_TOGGLE; batMonLowWarning--; } if (eepromConfig.mavlinkEnabled == true) { mavlinkSendHeartbeat(); mavlinkSendSysStatus(); } executionTime1Hz = micros() - currentTime; } //////////////////////////////// } /////////////////////////////////////////////////////////////////////////// }
int main(void) { /////////////////////////////////////////////////////////////////////////// uint32_t currentTime; systemInit(); systemReady = true; while (1) { /////////////////////////////// if (frame_50Hz) { frame_50Hz = false; currentTime = micros(); deltaTime50Hz = currentTime - previous50HzTime; previous50HzTime = currentTime; processFlightCommands(); if (eepromConfig.osdEnabled) { if (eepromConfig.osdDisplayAlt) displayAltitude(sensors.pressureAlt10Hz, 0.0f, DISENGAGED); if (eepromConfig.osdDisplayAH) displayArtificialHorizon(sensors.attitude500Hz[ROLL], sensors.attitude500Hz[PITCH], flightMode); if (eepromConfig.osdDisplayAtt) displayAttitude(sensors.attitude500Hz[ROLL], sensors.attitude500Hz[PITCH], flightMode); if (eepromConfig.osdDisplayHdg) displayHeading(heading.mag); } executionTime50Hz = micros() - currentTime; } /////////////////////////////// if (frame_10Hz) { frame_10Hz = false; currentTime = micros(); deltaTime10Hz = currentTime - previous10HzTime; previous10HzTime = currentTime; if (newMagData == true) { sensors.mag10Hz[XAXIS] = (float)rawMag[XAXIS].value * magScaleFactor[XAXIS] - eepromConfig.magBias[XAXIS]; sensors.mag10Hz[YAXIS] = (float)rawMag[YAXIS].value * magScaleFactor[YAXIS] - eepromConfig.magBias[YAXIS]; sensors.mag10Hz[ZAXIS] = -((float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS] - eepromConfig.magBias[ZAXIS]); newMagData = false; magDataUpdate = true; } d1Average = d1Sum / 10; d1Sum = 0; calculateTemperature(); calculatePressureAltitude(); pressureAltValid = true; switch (eepromConfig.gpsType) { /////////////////////// case NO_GPS: // No GPS installed break; /////////////////////// case MEDIATEK_3329_BINARY: // MediaTek 3329 in binary mode decodeMediaTek3329BinaryMsg(); break; /////////////////////// case MEDIATEK_3329_NMEA: // MediaTek 3329 in NMEA mode decodeNMEAsentence(); break; /////////////////////// case UBLOX: // UBLOX in binary mode decodeUbloxMsg(); break; /////////////////////// } cliCom(); rfCom(); executionTime10Hz = micros() - currentTime; } /////////////////////////////// if (frame_500Hz) { frame_500Hz = false; currentTime = micros(); deltaTime500Hz = currentTime - previous500HzTime; previous500HzTime = currentTime; TIM_Cmd(TIM10, DISABLE); timerValue = TIM_GetCounter(TIM10); TIM_SetCounter(TIM10, 0); TIM_Cmd(TIM10, ENABLE); dt500Hz = (float)timerValue * 0.0000005f; // For integrations in 500 Hz loop computeMPU6000TCBias(); /* sensorTemp1 = computeMPU6000SensorTemp(); sensorTemp2 = sensorTemp1 * sensorTemp1; sensorTemp3 = sensorTemp2 * sensorTemp1; */ sensors.accel500Hz[XAXIS] = ((float)accelSummedSamples500Hz[XAXIS] / 2.0f - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR; sensors.accel500Hz[YAXIS] = -((float)accelSummedSamples500Hz[YAXIS] / 2.0f - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR; sensors.accel500Hz[ZAXIS] = -((float)accelSummedSamples500Hz[ZAXIS] / 2.0f - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR; /* sensors.accel500Hz[XAXIS] = ((float)accelSummedSamples500Hz[XAXIS] / 2.0f + eepromConfig.accelBiasP0[XAXIS] + eepromConfig.accelBiasP1[XAXIS] * sensorTemp1 + eepromConfig.accelBiasP2[XAXIS] * sensorTemp2 + eepromConfig.accelBiasP3[XAXIS] * sensorTemp3 ) * ACCEL_SCALE_FACTOR; sensors.accel500Hz[YAXIS] = -((float)accelSummedSamples500Hz[YAXIS] / 2.0f + eepromConfig.accelBiasP0[YAXIS] + eepromConfig.accelBiasP1[YAXIS] * sensorTemp1 + eepromConfig.accelBiasP2[YAXIS] * sensorTemp2 + eepromConfig.accelBiasP3[YAXIS] * sensorTemp3 ) * ACCEL_SCALE_FACTOR; sensors.accel500Hz[ZAXIS] = -((float)accelSummedSamples500Hz[ZAXIS] / 2.0f + eepromConfig.accelBiasP0[ZAXIS] + eepromConfig.accelBiasP1[ZAXIS] * sensorTemp1 + eepromConfig.accelBiasP2[ZAXIS] * sensorTemp2 + eepromConfig.accelBiasP3[ZAXIS] * sensorTemp3 ) * ACCEL_SCALE_FACTOR; */ sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR; /* sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL ] / 2.0f + gyroBiasP0[ROLL ] + eepromConfig.gyroBiasP1[ROLL ] * sensorTemp1 + eepromConfig.gyroBiasP2[ROLL ] * sensorTemp2 + eepromConfig.gyroBiasP3[ROLL ] * sensorTemp3 ) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f + gyroBiasP0[PITCH] + eepromConfig.gyroBiasP1[PITCH] * sensorTemp1 + eepromConfig.gyroBiasP2[PITCH] * sensorTemp2 + eepromConfig.gyroBiasP3[PITCH] * sensorTemp3 ) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f + gyroBiasP0[YAW ] + eepromConfig.gyroBiasP1[YAW ] * sensorTemp1 + eepromConfig.gyroBiasP2[YAW ] * sensorTemp2 + eepromConfig.gyroBiasP3[YAW ] * sensorTemp3 ) * GYRO_SCALE_FACTOR; */ MargAHRSupdate( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW], sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], eepromConfig.accelCutoff, magDataUpdate, dt500Hz ); magDataUpdate = false; computeAxisCommands(dt500Hz); mixTable(); writeServos(); writeMotors(); executionTime500Hz = micros() - currentTime; } /////////////////////////////// if (frame_100Hz) { frame_100Hz = false; currentTime = micros(); deltaTime100Hz = currentTime - previous100HzTime; previous100HzTime = currentTime; TIM_Cmd(TIM11, DISABLE); timerValue = TIM_GetCounter(TIM11); TIM_SetCounter(TIM11, 0); TIM_Cmd(TIM11, ENABLE); dt100Hz = (float)timerValue * 0.0000005f; // For integrations in 100 Hz loop sensors.accel100Hz[XAXIS] = ((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR; sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR; sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR; createRotationMatrix(); bodyAccelToEarthAccel(); vertCompFilter(dt100Hz); if ( highSpeedTelem1Enabled == true ) { // 500 Hz Accels telemetryPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS]); } if ( highSpeedTelem2Enabled == true ) { // 500 Hz Gyros telemetryPrintF("%9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW ]); } if ( highSpeedTelem3Enabled == true ) { // Roll Rate, Roll Rate Command telemetryPrintF("%9.4f, %9.4f\n", sensors.gyro500Hz[ROLL], rxCommand[ROLL]); } if ( highSpeedTelem4Enabled == true ) { // Pitch Rate, Pitch Rate Command telemetryPrintF("%9.4f, %9.4f\n", sensors.gyro500Hz[PITCH], rxCommand[PITCH]); } if ( highSpeedTelem5Enabled == true ) { // Yaw Rate, Yaw Rate Command telemetryPrintF("%9.4f, %9.4f\n", sensors.gyro500Hz[YAW], rxCommand[YAW]); } if ( highSpeedTelem6Enabled == true ) { // 500 Hz Attitudes telemetryPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ], sensors.attitude500Hz[PITCH], sensors.attitude500Hz[YAW ]); } if ( highSpeedTelem7Enabled == true ) { // Vertical Variables telemetryPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", earthAxisAccels[ZAXIS], sensors.pressureAlt10Hz, hDotEstimate, hEstimate); } executionTime100Hz = micros() - currentTime; } /////////////////////////////// if (frame_5Hz) { frame_5Hz = false; currentTime = micros(); deltaTime5Hz = currentTime - previous5HzTime; previous5HzTime = currentTime; if (execUp == true) BLUE_LED_TOGGLE; executionTime5Hz = micros() - currentTime; } /////////////////////////////// if (frame_1Hz) { frame_1Hz = false; currentTime = micros(); deltaTime1Hz = currentTime - previous1HzTime; previous1HzTime = currentTime; if (execUp == true) GREEN_LED_TOGGLE; if (execUp == false) execUpCount++; if ((execUpCount == 5) && (execUp == false)) execUp = true; executionTime1Hz = micros() - currentTime; } //////////////////////////////// } /////////////////////////////////////////////////////////////////////////// }
wxThread::ExitCode HaloRenderingThread::Entry() { wxCommandEvent evt(wxEVT_HALO_RENDERING_THREAD_NOTIFY, wxID_ANY); setupStruct.resultValid = false; setupStruct.imageBuffer = 0; setupStruct.rayPaths = new map<RayPathId, RayPathDescriptor>; if (setupStruct.crystals.size() == 0) { evt.SetString(wxT("No crystal population set up.")); evt.SetInt(1); setupStruct.notifee->AddPendingEvent(evt); return 0; } // Cast rays on crystals and refracted rays will form a pixel. int crystalTypeIndex = 0; CrystalDescriptor *currentDescriptor = &setupStruct.crystals[0]; const int N_CRYSTAL_TYPES = setupStruct.crystals.size(); int crystalsRemaining = currentDescriptor->populationWeight; Mesh currentMesh; size_t percentStep = setupStruct.crystalCount / 100; if (percentStep < 1) percentStep = 1; Vector3 sunPos(0, 0, -100000); rotate2d(sunPos.y, sunPos.z, setupStruct.solarAltitude * 3.14159265636 / 180.0); // Two prependicular vector Vector3 prepX = sunPos % Vector3(0, 1, 0); // FIXME: Sun on the zenith Vector3 prepY = sunPos % prepX; prepX /= ~prepX; prepY /= ~prepY; setupStruct.deleteBuffer = freeBuffer; setupStruct.deleteMap = freeMap; size_t size = setupStruct.imageSize; setupStruct.imageBuffer = new uint32_t[size * size * 6]; memset(setupStruct.imageBuffer, 0, size * size * 6 * sizeof(uint32_t)); uint32_t *leftPlane = setupStruct.imageBuffer; // 0th plane uint32_t *rightPlane = setupStruct.imageBuffer + size * size; // 1st plane uint32_t *topPlane = setupStruct.imageBuffer + 2 * size * size; // 2nd plane uint32_t *bottomPlane = setupStruct.imageBuffer + 3 * size * size; // 3rd plane uint32_t *backPlane = setupStruct.imageBuffer + 4 * size * size; // 4th plane uint32_t *frontPlane = setupStruct.imageBuffer + 5 * size * size; // 5th plane double halfImageSize = size * 0.5; int maxRays = (setupStruct.maxRayCastInfoSize * 1000000) / (sizeof(RayPathDescriptor) + sizeof(RayPathId)); if (maxRays == 0) maxRays = 1; int recordRayModulus = setupStruct.crystalCount / maxRays; if (!recordRayModulus) recordRayModulus = 1; struct RefractionColor { int rgb; double refractionIndex; }; RefractionColor colors[] = { { 0x0000FF, 1.3072 }, { 0x0080FF, 1.3094 }, { 0x00FFFF, 1.31 }, { 0x00FF80, 1.3107 }, { 0x00FF00, 1.3114 }, { 0x80FF00, 1.3125 }, { 0xFFFF00, 1.3136 }, { 0xFF8000, 1.3147 }, { 0xFF0000, 1.3158 }, { 0xFF0040, 1.3172 }, { 0xFF0080, 1.32 }, }; const int N_COLORS = sizeof(colors) / sizeof(colors[0]); // Cast many rays. for (size_t i = 0; i < setupStruct.crystalCount; i++) { if (setupStruct.cancelled) { // If the user shut the rendering down... delete[] setupStruct.imageBuffer; setupStruct.imageBuffer = 0; return 0; } if (!crystalsRemaining) { // We iterate through the crystals based on their population weights. crystalTypeIndex++; if (crystalTypeIndex >= N_CRYSTAL_TYPES) crystalTypeIndex = 0; currentDescriptor = &setupStruct.crystals[crystalTypeIndex]; crystalsRemaining = currentDescriptor->populationWeight; } // Get the raw mesh currentMesh = currentDescriptor->mesh; // Rotate it according to the orientation. Matrix orientationMatrix = getOrientationMatrix(currentDescriptor->orientation); // Apply wobbliness Vector3 wobbleRotationAxis(1, 0, 0); rotate2d(wobbleRotationAxis.x, wobbleRotationAxis.z, randFloat(0, 3.1415)); double wobblinessLimit = currentDescriptor->wobbliness * 3.1415 / 180.0; Matrix wobbleMatrix = createRotationMatrix( wobbleRotationAxis, randFloatNormal( 0, wobblinessLimit ) ); Matrix transformation = orientationMatrix % wobbleMatrix; transformMesh(currentMesh, transformation); // Crystal created, now cast a ray on it. Vector3 offset = prepX * randFloat(-1, 1) + prepY * randFloat(-1, 1); vector<RayPath> rayPaths; // Compute color here double colorCode = randFloat(0, N_COLORS - 1); RefractionColor prev = colors[(int)(floor(colorCode))]; RefractionColor next = colors[(int)(ceil(colorCode))]; double kColor = colorCode - floor(colorCode); RefractionColor currentColor; currentColor.rgb = prev.rgb; currentColor.refractionIndex = (1 - kColor) * prev.refractionIndex + kColor * next.refractionIndex; // Compute real poisition of the ray (Sun is a disk) Vector3 realSunPos = sunPos; Vector3 rayRotVector; double rayRotVectorLength; do { rayRotVector = realSunPos % getRandomVector(); rayRotVectorLength = ~rayRotVector; } while (rayRotVectorLength == 0); rayRotVector /= rayRotVectorLength; realSunPos = transformVector( createRotationMatrix( rayRotVector, randFloat( 0, setupStruct.solarDiskRadius * 3.1415926536 / 180.0 ) ), realSunPos ); computeRayPathInGlassMesh(currentMesh, currentColor.refractionIndex, realSunPos + offset, -realSunPos, 0.01, rayPaths); /* Project rays on the six planes. */ for (size_t j = 0; j < rayPaths.size(); j++) { RayPath ¤t = rayPaths[j]; size_t pathLength = current.size(); Vector3 exitDir = current[pathLength - 1].first - current[pathLength - 2].first; Vector3 projectionDir = -exitDir; double xPos, yPos; // select the plane to project on. uint32_t *plane; double xm = 1, ym = 1; int planeId; if ((fabs(projectionDir.x) > fabs(projectionDir.y)) && (fabs(projectionDir.x) > fabs(projectionDir.z))) { if (projectionDir.x < 0) { plane = leftPlane; planeId = 0; xm = -1; } else { plane = rightPlane; planeId = 1; } xPos = xm * projectionDir.z / fabs(projectionDir.x) * halfImageSize + halfImageSize; yPos = -ym * projectionDir.y / fabs(projectionDir.x) * halfImageSize + halfImageSize; } if ((fabs(projectionDir.y) > fabs(projectionDir.x)) && (fabs(projectionDir.y) > fabs(projectionDir.z))) { if (projectionDir.y < 0) { plane = bottomPlane; planeId = 3; ym = -1; } else { plane = topPlane; planeId = 2; } xPos = xm * projectionDir.x / fabs(projectionDir.y) * halfImageSize + halfImageSize; yPos = -ym * projectionDir.z / fabs(projectionDir.y) * halfImageSize + halfImageSize; } if ((fabs(projectionDir.z) > fabs(projectionDir.x)) && (fabs(projectionDir.z) > fabs(projectionDir.y))) { if (projectionDir.z < 0) { plane = frontPlane; planeId = 5; } else { plane = backPlane; planeId = 4; xm = -1; } xPos = xm * projectionDir.x / fabs(projectionDir.z) * halfImageSize + halfImageSize; yPos = -ym * projectionDir.y / fabs(projectionDir.z) * halfImageSize + halfImageSize; } // Calculate the new intensity of the pixel. xPos = clampInInt(xPos, 0, size - 1); yPos = clampInInt(yPos, 0, size - 1); int prevPixel = plane[(int)(yPos) * size + (int)(xPos)]; int nextPixel = 0; double intensity = current[pathLength - 2].second * 20; for (int j = 0; j < 3; j++) { int currentSaturation = (prevPixel >> (8 * j)) & 0xFF; int toAdd = (int)(((currentColor.rgb >> (8 * j)) & 0xFF) * intensity) >> 8; int nextSaturation; if (currentSaturation + toAdd > 255) nextSaturation = 255; else nextSaturation = currentSaturation + toAdd; nextPixel += (1 << (8 * j)) * nextSaturation * setupStruct.pixelIntensity; } plane[(int)(yPos) * size + (int)(xPos)] = nextPixel; // Record the ray if needed if (!(i % recordRayModulus)) { RayPathId pixelId(planeId, (int)xPos, (int)yPos); RayPathDescriptor theDescriptor( ¤tDescriptor->mesh, transformation, realSunPos + offset, -realSunPos, intensity ); map<RayPathId, RayPathDescriptor>::iterator it = setupStruct.rayPaths->find(pixelId); if (it == setupStruct.rayPaths->end()) { // If not found, insert it as new setupStruct.rayPaths->insert( make_pair( pixelId, theDescriptor ) ); } else if (it->second.intensity < intensity) { // If found, update it if the current ray is more intense it->second = theDescriptor; } } } if (i % percentStep == 0) { sendNotifyString(wxString::Format(wxT("Casting rays: %d%%"), i / percentStep)); } crystalsRemaining--; } evt.SetString(wxT("Completed.")); evt.SetInt(1); //< 1 means the operation is finished. setupStruct.resultValid = true; setupStruct.notifee->AddPendingEvent(evt); return 0; }
int main(void) { /////////////////////////////////////////////////////////////////////////// uint32_t currentTime; #ifdef _DTIMING #define LA1_ENABLE GPIO_SetBits(GPIOA, GPIO_Pin_4) #define LA1_DISABLE GPIO_ResetBits(GPIOA, GPIO_Pin_4) #define LA4_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_5) #define LA4_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_5) #define LA2_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_2) #define LA2_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_2) #define LA3_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_3) #define LA3_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_3) GPIO_InitTypeDef GPIO_InitStructure; RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); GPIO_StructInit(&GPIO_InitStructure); // Init pins GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOA, &GPIO_InitStructure); // Init pins GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1; //GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; //GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOB, &GPIO_InitStructure); // Init pins GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_5; //GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; //GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOC, &GPIO_InitStructure); // PB0_DISABLE; LA4_DISABLE; LA2_DISABLE; LA3_DISABLE; LA1_DISABLE; #endif systemInit(); systemReady = true; evrPush(EVR_StartingMain, 0); while (1) { evrCheck(); /////////////////////////////// if (frame_50Hz) { #ifdef _DTIMING LA2_ENABLE; #endif frame_50Hz = false; currentTime = micros(); deltaTime50Hz = currentTime - previous50HzTime; previous50HzTime = currentTime; processFlightCommands(); if (newTemperatureReading && newPressureReading) { d1Value = d1.value; d2Value = d2.value; calculateTemperature(); calculatePressureAltitude(); newTemperatureReading = false; newPressureReading = false; } sensors.pressureAlt50Hz = firstOrderFilter(sensors.pressureAlt50Hz, &firstOrderFilters[PRESSURE_ALT_LOWPASS]); rssiMeasure(); updateMax7456(currentTime, 0); executionTime50Hz = micros() - currentTime; #ifdef _DTIMING LA2_DISABLE; #endif } /////////////////////////////// if (frame_10Hz) { #ifdef _DTIMING LA4_ENABLE; #endif frame_10Hz = false; currentTime = micros(); deltaTime10Hz = currentTime - previous10HzTime; previous10HzTime = currentTime; if (newMagData == true) { sensors.mag10Hz[XAXIS] = (float)rawMag[XAXIS].value * magScaleFactor[XAXIS + eepromConfig.externalHMC5883] - eepromConfig.magBias[XAXIS + eepromConfig.externalHMC5883]; sensors.mag10Hz[YAXIS] = (float)rawMag[YAXIS].value * magScaleFactor[YAXIS + eepromConfig.externalHMC5883] - eepromConfig.magBias[YAXIS + eepromConfig.externalHMC5883]; sensors.mag10Hz[ZAXIS] = -((float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS + eepromConfig.externalHMC5883] - eepromConfig.magBias[ZAXIS + eepromConfig.externalHMC5883]); newMagData = false; magDataUpdate = true; } decodeUbloxMsg(); batMonTick(); cliCom(); if (eepromConfig.mavlinkEnabled == true) { mavlinkSendAttitude(); mavlinkSendVfrHud(); } executionTime10Hz = micros() - currentTime; #ifdef _DTIMING LA4_DISABLE; #endif } /////////////////////////////// if (frame_500Hz) { #ifdef _DTIMING LA1_ENABLE; #endif frame_500Hz = false; currentTime = micros(); deltaTime500Hz = currentTime - previous500HzTime; previous500HzTime = currentTime; TIM_Cmd(TIM10, DISABLE); timerValue = TIM_GetCounter(TIM10); TIM_SetCounter(TIM10, 0); TIM_Cmd(TIM10, ENABLE); dt500Hz = (float)timerValue * 0.0000005f; // For integrations in 500 Hz loop computeMPU6000TCBias(); sensors.accel500Hz[XAXIS] = ((float)accelSummedSamples500Hz[XAXIS] * 0.5f - eepromConfig.accelBiasMPU[XAXIS] - accelTCBias[XAXIS]) * eepromConfig.accelScaleFactorMPU[XAXIS]; sensors.accel500Hz[YAXIS] = -((float)accelSummedSamples500Hz[YAXIS] * 0.5f - eepromConfig.accelBiasMPU[YAXIS] - accelTCBias[YAXIS]) * eepromConfig.accelScaleFactorMPU[YAXIS]; sensors.accel500Hz[ZAXIS] = -((float)accelSummedSamples500Hz[ZAXIS] * 0.5f - eepromConfig.accelBiasMPU[ZAXIS] - accelTCBias[ZAXIS]) * eepromConfig.accelScaleFactorMPU[ZAXIS]; sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR; MargAHRSupdate(sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW], sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], eepromConfig.accelCutoff, magDataUpdate, dt500Hz); magDataUpdate = false; computeAxisCommands(dt500Hz); mixTable(); writeServos(); writeMotors(); executionTime500Hz = micros() - currentTime; #ifdef _DTIMING LA1_DISABLE; #endif } /////////////////////////////// if (frame_100Hz) { #ifdef _DTIMING LA3_ENABLE; #endif frame_100Hz = false; currentTime = micros(); deltaTime100Hz = currentTime - previous100HzTime; previous100HzTime = currentTime; TIM_Cmd(TIM11, DISABLE); timerValue = TIM_GetCounter(TIM11); TIM_SetCounter(TIM11, 0); TIM_Cmd(TIM11, ENABLE); dt100Hz = (float)timerValue * 0.0000005f; // For integrations in 100 Hz loop sensors.accel100Hz[XAXIS] = ((float)accelSummedSamples100Hz[XAXIS] * 0.1f - eepromConfig.accelBiasMPU[XAXIS] - accelTCBias[XAXIS]) * eepromConfig.accelScaleFactorMPU[XAXIS]; sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] * 0.1f - eepromConfig.accelBiasMPU[YAXIS] - accelTCBias[YAXIS]) * eepromConfig.accelScaleFactorMPU[YAXIS]; sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] * 0.1f - eepromConfig.accelBiasMPU[ZAXIS] - accelTCBias[ZAXIS]) * eepromConfig.accelScaleFactorMPU[ZAXIS]; createRotationMatrix(); bodyAccelToEarthAccel(); vertCompFilter(dt100Hz); if (armed == true) { if ( eepromConfig.activeTelemetry == 1 ) { // 500 Hz Accels telemPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS]); } if ( eepromConfig.activeTelemetry == 2 ) { // 500 Hz Gyros telemPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW ]); } if ( eepromConfig.activeTelemetry == 4 ) { // 500 Hz Attitudes telemPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ], sensors.attitude500Hz[PITCH], sensors.attitude500Hz[YAW ]); } if ( eepromConfig.activeTelemetry == 8 ) { // Vertical Variables telemPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %4ld\n", earthAxisAccels[ZAXIS], sensors.pressureAlt50Hz, hDotEstimate, hEstimate, ms5611Temperature); } if ( eepromConfig.activeTelemetry == 16) { // Vertical Variables telemPortPrintF("%9.4f, %9.4f, %9.4f, %4ld, %1d, %9.4f, %9.4f\n", verticalVelocityCmd, hDotEstimate, hEstimate, ms5611Temperature, verticalModeState, throttleCmd, eepromConfig.PID[HDOT_PID].iTerm); } } executionTime100Hz = micros() - currentTime; #ifdef _DTIMING LA3_DISABLE; #endif } /////////////////////////////// if (frame_5Hz) { frame_5Hz = false; currentTime = micros(); deltaTime5Hz = currentTime - previous5HzTime; previous5HzTime = currentTime; gpsUpdated(); //if (eepromConfig.mavlinkEnabled == true) //{ // mavlinkSendGpsRaw(); //} if (batMonVeryLowWarning > 0) { LED1_TOGGLE; batMonVeryLowWarning--; } if (execUp == true) BLUE_LED_TOGGLE; executionTime5Hz = micros() - currentTime; } /////////////////////////////// if (frame_1Hz) { frame_1Hz = false; currentTime = micros(); deltaTime1Hz = currentTime - previous1HzTime; previous1HzTime = currentTime; if (execUp == true) GREEN_LED_TOGGLE; if (execUp == false) execUpCount++; if ((execUpCount == 5) && (execUp == false)) { execUp = true; pwmEscInit(); homeData.magHeading = sensors.attitude500Hz[YAW]; } if (batMonLowWarning > 0) { LED1_TOGGLE; batMonLowWarning--; } if (eepromConfig.mavlinkEnabled == true) { mavlinkSendHeartbeat(); mavlinkSendSysStatus(); } executionTime1Hz = micros() - currentTime; } //////////////////////////////// } /////////////////////////////////////////////////////////////////////////// }
int main(void) { /////////////////////////////////////////////////////////////////////////// uint32_t currentTime; systemReady = false; systemInit(); systemReady = true; evrPush(EVR_StartingMain, 0); while (1) { evrCheck(); /////////////////////////////// if (frame_50Hz) { frame_50Hz = false; currentTime = micros(); deltaTime50Hz = currentTime - previous50HzTime; previous50HzTime = currentTime; processFlightCommands(); if (newTemperatureReading && newPressureReading) { d1Value = d1.value; d2Value = d2.value; calculateTemperature(); calculatePressureAltitude(); newTemperatureReading = false; newPressureReading = false; } sensors.pressureAlt50Hz = firstOrderFilter(sensors.pressureAlt50Hz, &firstOrderFilters[PRESSURE_ALT_LOWPASS]); executionTime50Hz = micros() - currentTime; } /////////////////////////////// if (frame_10Hz) { frame_10Hz = false; currentTime = micros(); deltaTime10Hz = currentTime - previous10HzTime; previous10HzTime = currentTime; if (newMagData == true) { sensors.mag10Hz[XAXIS] = (float)rawMag[XAXIS].value * magScaleFactor[XAXIS] - eepromConfig.magBias[XAXIS]; sensors.mag10Hz[YAXIS] = -((float)rawMag[YAXIS].value * magScaleFactor[YAXIS] - eepromConfig.magBias[YAXIS]); sensors.mag10Hz[ZAXIS] = -((float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS] - eepromConfig.magBias[ZAXIS]); newMagData = false; magDataUpdate = true; } decodeUbloxMsg(); batMonTick(); cliCom(); if (eepromConfig.mavlinkEnabled == true) { mavlinkSendAttitude(); mavlinkSendVfrHud(); } else { rfCom(); } executionTime10Hz = micros() - currentTime; } /////////////////////////////// if (frame_500Hz) { frame_500Hz = false; currentTime = micros(); deltaTime500Hz = currentTime - previous500HzTime; previous500HzTime = currentTime; TIM_Cmd(TIM6, DISABLE); timerValue = TIM_GetCounter(TIM6); TIM_SetCounter(TIM6, 0); TIM_Cmd(TIM6, ENABLE); dt500Hz = (float)timerValue * 0.0000005f; // For integrations in 500 Hz loop computeMPU6000TCBias(); sensors.accel500Hz[XAXIS] = -((float)accelSummedSamples500Hz[XAXIS] * 0.5f - eepromConfig.accelBiasMPU[XAXIS] - accelTCBias[XAXIS]) * eepromConfig.accelScaleFactorMPU[XAXIS]; sensors.accel500Hz[YAXIS] = ((float)accelSummedSamples500Hz[YAXIS] * 0.5f - eepromConfig.accelBiasMPU[YAXIS] - accelTCBias[YAXIS]) * eepromConfig.accelScaleFactorMPU[YAXIS]; sensors.accel500Hz[ZAXIS] = -((float)accelSummedSamples500Hz[ZAXIS] * 0.5f - eepromConfig.accelBiasMPU[ZAXIS] - accelTCBias[ZAXIS]) * eepromConfig.accelScaleFactorMPU[ZAXIS]; //sensors.accel500Hz[XAXIS] = firstOrderFilter(sensors.accel500Hz[XAXIS], &firstOrderFilters[ACCEL500HZ_X_LOWPASS]); //sensors.accel500Hz[YAXIS] = firstOrderFilter(sensors.accel500Hz[YAXIS], &firstOrderFilters[ACCEL500HZ_Y_LOWPASS]); //sensors.accel500Hz[ZAXIS] = firstOrderFilter(sensors.accel500Hz[ZAXIS], &firstOrderFilters[ACCEL500HZ_Z_LOWPASS]); sensors.gyro500Hz[ROLL ] = -((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[PITCH] = ((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR; MargAHRSupdate( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW], sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], eepromConfig.accelCutoff, magDataUpdate, dt500Hz ); magDataUpdate = false; computeAxisCommands(dt500Hz); mixTable(); writeServos(); writeMotors(); executionTime500Hz = micros() - currentTime; } /////////////////////////////// if (frame_100Hz) { frame_100Hz = false; currentTime = micros(); deltaTime100Hz = currentTime - previous100HzTime; previous100HzTime = currentTime; TIM_Cmd(TIM7, DISABLE); timerValue = TIM_GetCounter(TIM7); TIM_SetCounter(TIM7, 0); TIM_Cmd(TIM7, ENABLE); dt100Hz = (float)timerValue * 0.0000005f; // For integrations in 100 Hz loop sensors.accel100Hz[XAXIS] = -((float)accelSummedSamples100Hz[XAXIS] * 0.1f - eepromConfig.accelBiasMPU[XAXIS] - accelTCBias[XAXIS]) * eepromConfig.accelScaleFactorMPU[XAXIS]; sensors.accel100Hz[YAXIS] = ((float)accelSummedSamples100Hz[YAXIS] * 0.1f - eepromConfig.accelBiasMPU[YAXIS] - accelTCBias[YAXIS]) * eepromConfig.accelScaleFactorMPU[YAXIS]; sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] * 0.1f - eepromConfig.accelBiasMPU[ZAXIS] - accelTCBias[ZAXIS]) * eepromConfig.accelScaleFactorMPU[ZAXIS]; //sensors.accel100Hz[XAXIS] = firstOrderFilter(sensors.accel100Hz[XAXIS], &firstOrderFilters[ACCEL100HZ_X_LOWPASS]); //sensors.accel100Hz[YAXIS] = firstOrderFilter(sensors.accel100Hz[YAXIS], &firstOrderFilters[ACCEL100HZ_Y_LOWPASS]); //sensors.accel100Hz[ZAXIS] = firstOrderFilter(sensors.accel100Hz[ZAXIS], &firstOrderFilters[ACCEL100HZ_Z_LOWPASS]); createRotationMatrix(); bodyAccelToEarthAccel(); vertCompFilter(dt100Hz); if (armed == true) { if ( eepromConfig.activeTelemetry == 1 ) { // 500 Hz Accels telemetryPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS]); } if ( eepromConfig.activeTelemetry == 2 ) { // 500 Hz Gyros telemetryPrintF("%9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW ]); } if ( eepromConfig.activeTelemetry == 4 ) { // 500 Hz Attitudes telemetryPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ], sensors.attitude500Hz[PITCH], sensors.attitude500Hz[YAW ]); } if ( eepromConfig.activeTelemetry == 8 ) { // Vertical Variables telemetryPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %4ld\n", earthAxisAccels[ZAXIS], sensors.pressureAlt50Hz, hDotEstimate, hEstimate, ms5611Temperature); } if ( eepromConfig.activeTelemetry == 16 ) { // Vertical Variables telemetryPrintF("%9.4f, %9.4f, %9.4f, %4ld, %1d, %9.4f, %9.4f\n", verticalVelocityCmd, hDotEstimate, hEstimate, ms5611Temperature, verticalModeState, throttleCmd, eepromConfig.PID[HDOT_PID].iTerm); } } executionTime100Hz = micros() - currentTime; } /////////////////////////////// if (frame_5Hz) { frame_5Hz = false; currentTime = micros(); deltaTime5Hz = currentTime - previous5HzTime; previous5HzTime = currentTime; gpsUpdated(); if (eepromConfig.mavlinkEnabled == true) { mavlinkSendGpsRaw(); } if (batMonVeryLowWarning > 0) { BEEP_TOGGLE; batMonVeryLowWarning--; } if (execUp == true) LED0_TOGGLE; executionTime5Hz = micros() - currentTime; } /////////////////////////////// if (frame_1Hz) { frame_1Hz = false; currentTime = micros(); deltaTime1Hz = currentTime - previous1HzTime; previous1HzTime = currentTime; if (execUp == false) execUpCount++; if ((execUpCount == 5) && (execUp == false)) { execUp = true; pwmEscInit(); homeData.magHeading = sensors.attitude500Hz[YAW]; } if (batMonLowWarning > 0) { BEEP_TOGGLE; batMonLowWarning--; } if (eepromConfig.mavlinkEnabled == true) { mavlinkSendHeartbeat(); mavlinkSendSysStatus(); } executionTime1Hz = micros() - currentTime; } //////////////////////////////// } /////////////////////////////////////////////////////////////////////////// }
void CrystalEditorFrame::OnMouseMove(wxMouseEvent& event) { if (!dragging) return; // Turn mouse movement to a movement of a virtual trackball (which is as big as the window). Vector3 currentMousePos(event.GetX(), event.GetY(), 0); int width, height; GetSize(&width, &height); log_var(CRYSTAL_EDITOR_MOUSE_MOVE, width); log_var(CRYSTAL_EDITOR_MOUSE_MOVE, height); double windowSize = std::min(width, height); log_var(CRYSTAL_EDITOR_MOUSE_MOVE, windowSize); Vector3 center(0.5 * width, 0.5 * height, 0); log_var(CRYSTAL_EDITOR_MOUSE_MOVE, center); Vector3 normPrevPos = (prevMousePos - center) / windowSize; log_var(CRYSTAL_EDITOR_MOUSE_MOVE, normPrevPos); Vector3 normPos = (currentMousePos - center) / windowSize; log_var(CRYSTAL_EDITOR_MOUSE_MOVE, normPos); // Invert vertical axis, because in camera space Y grow upward. normPrevPos.y = -normPrevPos.y; normPos.y = -normPos.y; // Calculate the Z position on the trackball normPrevPos.z = normPrevPos * normPrevPos <= 1 ? sqrt(1.0 - normPrevPos * normPrevPos) : 0; normPos.z = normPos * normPos <= 1 ? sqrt(1.0 - normPos * normPos) : 0; log_var(CRYSTAL_EDITOR_MOUSE_MOVE, normPrevPos); log_var(CRYSTAL_EDITOR_MOUSE_MOVE, normPos); // Calculate the rotation axis Vector3 rotAxis = normPrevPos % normPos; double rotAxisLength = ~rotAxis; if (rotAxisLength == 0) { // No rotation. return; } rotAxis /= rotAxisLength; log_var(CRYSTAL_EDITOR_MOUSE_MOVE, rotAxis); // Calculate the real rotation axis and the angle. double rotAngle = acos((normPrevPos * normPos)/(~normPrevPos * ~normPos)); log_var(CRYSTAL_EDITOR_MOUSE_MOVE, rotAngle); Vector3 realRotAxis = rotAxis.x * right + rotAxis.y * upward + rotAxis.z * backward; // Create the true rotation axis (in camera space). log_var(CRYSTAL_EDITOR_MOUSE_MOVE, realRotAxis); Matrix rotation = createRotationMatrix(realRotAxis, -rotAngle); // Negative as we rotate the view, not the object log_var(CRYSTAL_EDITOR_MOUSE_MOVE, rotation); backward = transformVector(rotation, backward); right = transformVector(rotation, right); upward = transformVector(rotation, upward); log_var(CRYSTAL_EDITOR_MOUSE_MOVE, backward); log_var(CRYSTAL_EDITOR_MOUSE_MOVE, right); log_var(CRYSTAL_EDITOR_MOUSE_MOVE, upward); fix3VectorSystem(right, upward, backward); prevMousePos.x = event.GetX(); prevMousePos.y = event.GetY(); prevMousePos.z = 0; // Calculate ray path updateRayPaths(); mainPanel->Refresh(); log_printf(CRYSTAL_EDITOR_MOUSE_MOVE, "========================\n"); }
int main(void) { uint32_t currentTime; systemInit(); systemReady = true; while (1) { /////////////////////////////// if (frame_50Hz) { frame_50Hz = false; currentTime = micros(); deltaTime50Hz = currentTime - previous50HzTime; previous50HzTime = currentTime; processFlightCommands(); executionTime50Hz = micros() - currentTime; } /////////////////////////////// if (frame_10Hz) { frame_10Hz = false; currentTime = micros(); deltaTime10Hz = currentTime - previous10HzTime; previous10HzTime = currentTime; sensors.mag10Hz[XAXIS] = -((float)rawMag[XAXIS].value * magScaleFactor[XAXIS] - eepromConfig.magBias[XAXIS]); sensors.mag10Hz[YAXIS] = (float)rawMag[YAXIS].value * magScaleFactor[YAXIS] - eepromConfig.magBias[YAXIS]; sensors.mag10Hz[ZAXIS] = -((float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS] - eepromConfig.magBias[ZAXIS]); newMagData = false; magDataUpdate = true; pressureAverage = pressureSum / 10; pressureSum = 0; calculateTemperature(); calculatePressureAltitude(); sensors.pressureAlt10Hz = pressureAlt; cliCom(); executionTime10Hz = micros() - currentTime; } /////////////////////////////// if (frame_200Hz) { frame_200Hz = false; currentTime = micros(); deltaTime200Hz = currentTime - previous200HzTime; previous200HzTime = currentTime; dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - eepromConfig.accelBias[XAXIS]) * eepromConfig.accelScaleFactor[XAXIS]; sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - eepromConfig.accelBias[YAXIS]) * eepromConfig.accelScaleFactor[YAXIS]; sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - eepromConfig.accelBias[ZAXIS]) * eepromConfig.accelScaleFactor[ZAXIS]; sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]); sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]); sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]); computeGyroTCBias(); sensors.gyro200Hz[ROLL ] = ((float)gyroSummedSamples200Hz[ROLL] / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR; sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR; sensors.gyro200Hz[YAW ] = -((float)gyroSummedSamples200Hz[YAW] / 5.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR; MargAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW], sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], eepromConfig.accelCutoff, magDataUpdate, dt200Hz ); magDataUpdate = false; computeAxisCommands(dt200Hz); mixTable(); writeServos(); writeMotors(); executionTime200Hz = micros() - currentTime; } /////////////////////////////// if (frame_100Hz) { frame_100Hz = false; currentTime = micros(); deltaTime100Hz = currentTime - previous100HzTime; previous100HzTime = currentTime; dt100Hz = (float)deltaTime100Hz * 0.000001f; // For integrations in 100 Hz loop sensors.accel100Hz[XAXIS] = -((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelRTBias[XAXIS] - eepromConfig.accelBias[XAXIS]) * eepromConfig.accelScaleFactor[XAXIS]; sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelRTBias[YAXIS] - eepromConfig.accelBias[YAXIS]) * eepromConfig.accelScaleFactor[YAXIS]; sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelRTBias[ZAXIS] - eepromConfig.accelBias[ZAXIS]) * eepromConfig.accelScaleFactor[ZAXIS]; sensors.accel100Hz[XAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[XAXIS], &fourthOrder100Hz[AX_FILTER]); sensors.accel100Hz[YAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[YAXIS], &fourthOrder100Hz[AY_FILTER]); sensors.accel100Hz[ZAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[ZAXIS], &fourthOrder100Hz[AZ_FILTER]); //computeGyroTCBias(); //sensors.gyro100Hz[ROLL ] = ((float)gyroSummedSamples100Hz[ROLL] / 10.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR; //sensors.gyro100Hz[PITCH] = -((float)gyroSummedSamples100Hz[PITCH] / 10.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR; //sensors.gyro100Hz[YAW ] = -((float)gyroSummedSamples100Hz[YAW] / 10.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR; createRotationMatrix(); bodyAccelToEarthAccel(); vertCompFilter(dt100Hz); //computeAxisCommands(dt100Hz); //mixTable(); //writeServos(); //writeMotors(); if ( rfTelem1Enabled == true ) { // 200 Hz Accels cliPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS]); } if ( rfTelem2Enabled == true ) { // 200 Hz Gyros cliPrintF("%9.4f, %9.4f, %9.4f\n", sensors.gyro200Hz[ROLL ], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW ]); } if ( rfTelem3Enabled == true ) { // Roll Rate, Roll Rate Command cliPrintF("%9.4f, %9.4f\n", sensors.gyro200Hz[ROLL], rxCommand[ROLL]); } if ( rfTelem4Enabled == true ) { // Pitch Rate, Pitch Rate Command cliPrintF("%9.4f, %9.4f\n", sensors.gyro200Hz[PITCH], rxCommand[PITCH]); } if ( rfTelem5Enabled == true ) { // Yaw Rate, Yaw Rate Command cliPrintF("%9.4f, %9.4f\n", sensors.gyro200Hz[YAW], rxCommand[YAW]); } if ( rfTelem6Enabled == true ) { // 200 Hz Attitudes cliPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude200Hz[ROLL ], sensors.attitude200Hz[PITCH], sensors.attitude200Hz[YAW ]); } executionTime100Hz = micros() - currentTime; } /////////////////////////////// if (frame_5Hz) { frame_5Hz = false; currentTime = micros(); deltaTime5Hz = currentTime - previous5HzTime; previous5HzTime = currentTime; executionTime5Hz = micros() - currentTime; } /////////////////////////////// if (frame_1Hz) { frame_1Hz = false; currentTime = micros(); deltaTime1Hz = currentTime - previous1HzTime; previous1HzTime = currentTime; if (execUp == false) execUpCount++; if ((execUpCount == 5) && (execUp == false)) { execUp = true; LED0_OFF; LED1_OFF; } executionTime1Hz = micros() - currentTime; } //////////////////////////////// } }
int main(void) { /////////////////////////////////////////////////////////////////////////// #ifdef _DTIMING #define LA1_ENABLE GPIO_SetBits(GPIOA, GPIO_Pin_4) #define LA1_DISABLE GPIO_ResetBits(GPIOA, GPIO_Pin_4) #define LA4_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_5) #define LA4_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_5) #define LA2_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_2) #define LA2_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_2) #define LA3_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_3) #define LA3_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_3) GPIO_InitTypeDef GPIO_InitStructure; RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); GPIO_StructInit(&GPIO_InitStructure); // Init pins GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOA, &GPIO_InitStructure); // Init pins GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1; //GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; //GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOB, &GPIO_InitStructure); // Init pins GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_5; //GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; //GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOC, &GPIO_InitStructure); // PB0_DISABLE; LA4_DISABLE; LA2_DISABLE; LA3_DISABLE; LA1_DISABLE; #endif uint32_t currentTime; systemInit(); systemReady = true; evrPush(EVR_StartingMain, 0); while (1) { evrCheck(); /////////////////////////////// if (frame_50Hz) { #ifdef _DTIMING LA2_ENABLE; #endif frame_50Hz = false; currentTime = micros(); deltaTime50Hz = currentTime - previous50HzTime; previous50HzTime = currentTime; processFlightCommands(); if (newTemperatureReading && newPressureReading) { d1Value = d1.value; d2Value = d2.value; calculateTemperature(); calculatePressureAltitude(); newTemperatureReading = false; newPressureReading = false; } sensors.pressureAlt50Hz = firstOrderFilter(sensors.pressureAlt50Hz, &firstOrderFilters[PRESSURE_ALT_LOWPASS]); executionTime50Hz = micros() - currentTime; #ifdef _DTIMING LA2_DISABLE; #endif } /////////////////////////////// if (frame_10Hz) { #ifdef _DTIMING LA4_ENABLE; #endif frame_10Hz = false; currentTime = micros(); deltaTime10Hz = currentTime - previous10HzTime; previous10HzTime = currentTime; if (newMagData == true) { sensors.mag10Hz[XAXIS] = (float)rawMag[XAXIS].value * magScaleFactor[XAXIS] - eepromConfig.magBias[XAXIS]; sensors.mag10Hz[YAXIS] = (float)rawMag[YAXIS].value * magScaleFactor[YAXIS] - eepromConfig.magBias[YAXIS]; sensors.mag10Hz[ZAXIS] = -((float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS] - eepromConfig.magBias[ZAXIS]); newMagData = false; magDataUpdate = true; } cliCom(); rfCom(); batMonTick(); /////////////////////////// executionTime10Hz = micros() - currentTime; #ifdef _DTIMING LA4_DISABLE; #endif } /////////////////////////////// if (frame_500Hz) { #ifdef _DTIMING LA1_ENABLE; #endif frame_500Hz = false; currentTime = micros(); deltaTime500Hz = currentTime - previous500HzTime; previous500HzTime = currentTime; TIM_Cmd(TIM10, DISABLE); timerValue = TIM_GetCounter(TIM10); TIM_SetCounter(TIM10, 0); TIM_Cmd(TIM10, ENABLE); dt500Hz = (float)timerValue * 0.0000005f; // For integrations in 500 Hz loop computeMPU6000TCBias(); /* sensorTemp1 = computeMPU6000SensorTemp(); sensorTemp2 = sensorTemp1 * sensorTemp1; sensorTemp3 = sensorTemp2 * sensorTemp1; */ sensors.accel500Hz[XAXIS] = ((float)accelSummedSamples500Hz[XAXIS] / 2.0f - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR; sensors.accel500Hz[YAXIS] = -((float)accelSummedSamples500Hz[YAXIS] / 2.0f - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR; sensors.accel500Hz[ZAXIS] = -((float)accelSummedSamples500Hz[ZAXIS] / 2.0f - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR; sensors.accel500HzMXR[XAXIS] = -(accelSummedSamples500HzMXR[XAXIS] / 2.0f - eepromConfig.accelBiasMXR[XAXIS]) * eepromConfig.accelScaleFactorMXR[XAXIS]; sensors.accel500HzMXR[YAXIS] = -(accelSummedSamples500HzMXR[YAXIS] / 2.0f - eepromConfig.accelBiasMXR[YAXIS]) * eepromConfig.accelScaleFactorMXR[YAXIS]; sensors.accel500HzMXR[ZAXIS] = (accelSummedSamples500HzMXR[ZAXIS] / 2.0f - eepromConfig.accelBiasMXR[ZAXIS]) * eepromConfig.accelScaleFactorMXR[ZAXIS]; /* sensors.accel500Hz[XAXIS] = ((float)accelSummedSamples500Hz[XAXIS] / 2.0f + eepromConfig.accelBiasP0[XAXIS] + eepromConfig.accelBiasP1[XAXIS] * sensorTemp1 + eepromConfig.accelBiasP2[XAXIS] * sensorTemp2 + eepromConfig.accelBiasP3[XAXIS] * sensorTemp3 ) * ACCEL_SCALE_FACTOR; sensors.accel500Hz[YAXIS] = -((float)accelSummedSamples500Hz[YAXIS] / 2.0f + eepromConfig.accelBiasP0[YAXIS] + eepromConfig.accelBiasP1[YAXIS] * sensorTemp1 + eepromConfig.accelBiasP2[YAXIS] * sensorTemp2 + eepromConfig.accelBiasP3[YAXIS] * sensorTemp3 ) * ACCEL_SCALE_FACTOR; sensors.accel500Hz[ZAXIS] = -((float)accelSummedSamples500Hz[ZAXIS] / 2.0f + eepromConfig.accelBiasP0[ZAXIS] + eepromConfig.accelBiasP1[ZAXIS] * sensorTemp1 + eepromConfig.accelBiasP2[ZAXIS] * sensorTemp2 + eepromConfig.accelBiasP3[ZAXIS] * sensorTemp3 ) * ACCEL_SCALE_FACTOR; */ sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR; /* sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL ] / 2.0f + gyroBiasP0[ROLL ] + eepromConfig.gyroBiasP1[ROLL ] * sensorTemp1 + eepromConfig.gyroBiasP2[ROLL ] * sensorTemp2 + eepromConfig.gyroBiasP3[ROLL ] * sensorTemp3 ) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f + gyroBiasP0[PITCH] + eepromConfig.gyroBiasP1[PITCH] * sensorTemp1 + eepromConfig.gyroBiasP2[PITCH] * sensorTemp2 + eepromConfig.gyroBiasP3[PITCH] * sensorTemp3 ) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f + gyroBiasP0[YAW ] + eepromConfig.gyroBiasP1[YAW ] * sensorTemp1 + eepromConfig.gyroBiasP2[YAW ] * sensorTemp2 + eepromConfig.gyroBiasP3[YAW ] * sensorTemp3 ) * GYRO_SCALE_FACTOR; */ #if defined(MPU_ACCEL) MargAHRSupdate(sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW], sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], eepromConfig.accelCutoff, magDataUpdate, dt500Hz); #endif #if defined(MXR_ACCEL) sensors.accel500HzMXR[XAXIS] = firstOrderFilter(sensors.accel500HzMXR[XAXIS], &firstOrderFilters[ACCEL500HZ_X_LOWPASS]); sensors.accel500HzMXR[YAXIS] = firstOrderFilter(sensors.accel500HzMXR[YAXIS], &firstOrderFilters[ACCEL500HZ_Y_LOWPASS]); sensors.accel500HzMXR[ZAXIS] = firstOrderFilter(sensors.accel500HzMXR[ZAXIS], &firstOrderFilters[ACCEL500HZ_Z_LOWPASS]); MargAHRSupdate(sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW], sensors.accel500HzMXR[XAXIS], sensors.accel500HzMXR[YAXIS], sensors.accel500HzMXR[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], eepromConfig.accelCutoff, magDataUpdate, dt500Hz); #endif magDataUpdate = false; computeAxisCommands(dt500Hz); mixTable(); writeServos(); writeMotors(); executionTime500Hz = micros() - currentTime; #ifdef _DTIMING LA1_DISABLE; #endif } /////////////////////////////// if (frame_100Hz) { #ifdef _DTIMING LA3_ENABLE; #endif frame_100Hz = false; currentTime = micros(); deltaTime100Hz = currentTime - previous100HzTime; previous100HzTime = currentTime; TIM_Cmd(TIM11, DISABLE); timerValue = TIM_GetCounter(TIM11); TIM_SetCounter(TIM11, 0); TIM_Cmd(TIM11, ENABLE); dt100Hz = (float)timerValue * 0.0000005f; // For integrations in 100 Hz loop sensors.accel100Hz[XAXIS] = ((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR; sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR; sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR; sensors.accel100HzMXR[XAXIS] = -(accelSummedSamples100HzMXR[XAXIS] / 10.0f - eepromConfig.accelBiasMXR[XAXIS]) * eepromConfig.accelScaleFactorMXR[XAXIS]; sensors.accel100HzMXR[YAXIS] = -(accelSummedSamples100HzMXR[YAXIS] / 10.0f - eepromConfig.accelBiasMXR[YAXIS]) * eepromConfig.accelScaleFactorMXR[YAXIS]; sensors.accel100HzMXR[ZAXIS] = (accelSummedSamples100HzMXR[ZAXIS] / 10.0f - eepromConfig.accelBiasMXR[ZAXIS]) * eepromConfig.accelScaleFactorMXR[ZAXIS]; sensors.accel100HzMXR[XAXIS] = firstOrderFilter(sensors.accel100HzMXR[XAXIS], &firstOrderFilters[ACCEL100HZ_X_LOWPASS]); sensors.accel100HzMXR[YAXIS] = firstOrderFilter(sensors.accel100HzMXR[YAXIS], &firstOrderFilters[ACCEL100HZ_Y_LOWPASS]); sensors.accel100HzMXR[ZAXIS] = firstOrderFilter(sensors.accel100HzMXR[ZAXIS], &firstOrderFilters[ACCEL100HZ_Z_LOWPASS]); createRotationMatrix(); bodyAccelToEarthAccel(); vertCompFilter(dt100Hz); if (armed == true) { if ( eepromConfig.activeTelemetry == 1 ) { // 500 Hz Accels openLogPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS], sensors.accel500HzMXR[XAXIS], sensors.accel500HzMXR[YAXIS], sensors.accel500HzMXR[ZAXIS]); } if ( eepromConfig.activeTelemetry == 2 ) { // 500 Hz Gyros openLogPrintF("%9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW ]); } if ( eepromConfig.activeTelemetry == 4 ) { // 500 Hz Attitudes openLogPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ], sensors.attitude500Hz[PITCH], sensors.attitude500Hz[YAW ]); } if ( eepromConfig.activeTelemetry == 8 ) { // Vertical Variables openLogPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %4ld\n", earthAxisAccels[ZAXIS], sensors.pressureAlt50Hz, hDotEstimate, hEstimate, ms5611Temperature); } if ( eepromConfig.activeTelemetry == 16) { // Vertical Variables openLogPrintF("%9.4f, %9.4f, %9.4f, %4ld, %1d, %9.4f, %9.4f\n", verticalVelocityCmd, hDotEstimate, hEstimate, ms5611Temperature, verticalModeState, throttleCmd, eepromConfig.PID[HDOT_PID].iTerm); } } executionTime100Hz = micros() - currentTime; #ifdef _DTIMING LA3_DISABLE; #endif } /////////////////////////////// if (frame_5Hz) { frame_5Hz = false; currentTime = micros(); deltaTime5Hz = currentTime - previous5HzTime; previous5HzTime = currentTime; if (execUp == true) BLUE_LED_TOGGLE; while (batMonVeryLowWarning > 0) { //BEEP_TOGGLE; batMonVeryLowWarning--; } executionTime5Hz = micros() - currentTime; } /////////////////////////////// if (frame_1Hz) { frame_1Hz = false; currentTime = micros(); deltaTime1Hz = currentTime - previous1HzTime; previous1HzTime = currentTime; if (execUp == true) GREEN_LED_TOGGLE; if (execUp == false) execUpCount++; if ((execUpCount == 5) && (execUp == false)) { execUp = true; pwmEscInit(eepromConfig.escPwmRate); } while (batMonLowWarning > 0) { //BEEP_TOGGLE; batMonLowWarning--; } executionTime1Hz = micros() - currentTime; } //////////////////////////////// } /////////////////////////////////////////////////////////////////////////// }
int main(void) { /////////////////////////////////////////////////////////////////////////// uint32_t currentTime; arm_matrix_instance_f32 a; arm_matrix_instance_f32 b; arm_matrix_instance_f32 x; systemReady = false; systemInit(); systemReady = true; evrPush(EVR_StartingMain, 0); #ifdef _DTIMING #define LA1_ENABLE GPIO_SetBits(GPIOA, GPIO_Pin_4) #define LA1_DISABLE GPIO_ResetBits(GPIOA, GPIO_Pin_4) #define LA4_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_5) #define LA4_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_5) #define LA2_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_2) #define LA2_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_2) #define LA3_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_3) #define LA3_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_3) GPIO_InitTypeDef GPIO_InitStructure; RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); GPIO_StructInit(&GPIO_InitStructure); // Init pins GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOA, &GPIO_InitStructure); // Init pins GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1; //GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; //GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOB, &GPIO_InitStructure); // Init pins GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_5; //GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; //GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOC, &GPIO_InitStructure); // PB0_DISABLE; LA4_DISABLE; LA2_DISABLE; LA3_DISABLE; LA1_DISABLE; #endif while (1) { checkUsbActive(false); evrCheck(); /////////////////////////////// if (frame_50Hz) { #ifdef _DTIMING LA2_ENABLE; #endif frame_50Hz = false; currentTime = micros(); deltaTime50Hz = currentTime - previous50HzTime; previous50HzTime = currentTime; processFlightCommands(); if (newTemperatureReading && newPressureReading) { d1Value = d1.value; d2Value = d2.value; calculateTemperature(); calculatePressureAltitude(); newTemperatureReading = false; newPressureReading = false; } sensors.pressureAlt50Hz = firstOrderFilter(sensors.pressureAlt50Hz, &firstOrderFilters[PRESSURE_ALT_LOWPASS]); rssiMeasure(); updateMax7456(currentTime, 0); executionTime50Hz = micros() - currentTime; #ifdef _DTIMING LA2_DISABLE; #endif } /////////////////////////////// if (frame_10Hz) { #ifdef _DTIMING LA4_ENABLE; #endif frame_10Hz = false; currentTime = micros(); deltaTime10Hz = currentTime - previous10HzTime; previous10HzTime = currentTime; if (newMagData == true) { nonRotatedMagData[XAXIS] = (rawMag[XAXIS].value * magScaleFactor[XAXIS]) - eepromConfig.magBias[XAXIS + eepromConfig.externalHMC5883]; nonRotatedMagData[YAXIS] = (rawMag[YAXIS].value * magScaleFactor[YAXIS]) - eepromConfig.magBias[YAXIS + eepromConfig.externalHMC5883]; nonRotatedMagData[ZAXIS] = (rawMag[ZAXIS].value * magScaleFactor[ZAXIS]) - eepromConfig.magBias[ZAXIS + eepromConfig.externalHMC5883]; arm_mat_init_f32(&a, 3, 3, (float *)hmcOrientationMatrix); arm_mat_init_f32(&b, 3, 1, (float *)nonRotatedMagData); arm_mat_init_f32(&x, 3, 1, sensors.mag10Hz); arm_mat_mult_f32(&a, &b, &x); newMagData = false; magDataUpdate = true; } decodeUbloxMsg(); batMonTick(); cliCom(); if (eepromConfig.mavlinkEnabled == true) { mavlinkSendAttitude(); mavlinkSendVfrHud(); } executionTime10Hz = micros() - currentTime; #ifdef _DTIMING LA4_DISABLE; #endif } /////////////////////////////// if (frame_500Hz) { #ifdef _DTIMING LA1_ENABLE; #endif frame_500Hz = false; currentTime = micros(); deltaTime500Hz = currentTime - previous500HzTime; previous500HzTime = currentTime; TIM_Cmd(TIM10, DISABLE); timerValue = TIM_GetCounter(TIM10); TIM_SetCounter(TIM10, 0); TIM_Cmd(TIM10, ENABLE); dt500Hz = (float)timerValue * 0.0000005f; // For integrations in 500 Hz loop computeMPU6000TCBias(); nonRotatedAccelData[XAXIS] = ((float)accelSummedSamples500Hz[XAXIS] * 0.5f - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR; nonRotatedAccelData[YAXIS] = ((float)accelSummedSamples500Hz[YAXIS] * 0.5f - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR; nonRotatedAccelData[ZAXIS] = ((float)accelSummedSamples500Hz[ZAXIS] * 0.5f - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR; arm_mat_init_f32(&a, 3, 3, (float *)mpuOrientationMatrix); arm_mat_init_f32(&b, 3, 1, (float *)nonRotatedAccelData); arm_mat_init_f32(&x, 3, 1, sensors.accel500Hz); arm_mat_mult_f32(&a, &b, &x); nonRotatedGyroData[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] * 0.5f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR; nonRotatedGyroData[PITCH] = ((float)gyroSummedSamples500Hz[PITCH] * 0.5f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR; nonRotatedGyroData[YAW ] = ((float)gyroSummedSamples500Hz[YAW] * 0.5f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR; arm_mat_init_f32(&a, 3, 3, (float *)mpuOrientationMatrix); arm_mat_init_f32(&b, 3, 1, (float *)nonRotatedGyroData); arm_mat_init_f32(&x, 3, 1, sensors.gyro500Hz); arm_mat_mult_f32(&a, &b, &x); MargAHRSupdate(sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW], sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], eepromConfig.accelCutoff, magDataUpdate, dt500Hz); magDataUpdate = false; computeAxisCommands(dt500Hz); mixTable(); writeServos(); writeMotors(); executionTime500Hz = micros() - currentTime; #ifdef _DTIMING LA1_DISABLE; #endif } /////////////////////////////// if (frame_100Hz) { #ifdef _DTIMING LA3_ENABLE; #endif frame_100Hz = false; currentTime = micros(); deltaTime100Hz = currentTime - previous100HzTime; previous100HzTime = currentTime; TIM_Cmd(TIM11, DISABLE); timerValue = TIM_GetCounter(TIM11); TIM_SetCounter(TIM11, 0); TIM_Cmd(TIM11, ENABLE); dt100Hz = (float)timerValue * 0.0000005f; // For integrations in 100 Hz loop nonRotatedAccelData[XAXIS] = ((float)accelSummedSamples100Hz[XAXIS] * 0.1f - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR; nonRotatedAccelData[YAXIS] = ((float)accelSummedSamples100Hz[YAXIS] * 0.1f - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR; nonRotatedAccelData[ZAXIS] = ((float)accelSummedSamples100Hz[ZAXIS] * 0.1f - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR; arm_mat_init_f32(&a, 3, 3, (float *)mpuOrientationMatrix); arm_mat_init_f32(&b, 3, 1, (float *)nonRotatedAccelData); arm_mat_init_f32(&x, 3, 1, sensors.accel100Hz); arm_mat_mult_f32(&a, &b, &x); createRotationMatrix(); bodyAccelToEarthAccel(); vertCompFilter(dt100Hz); if (armed == true) { if ( eepromConfig.activeTelemetry == 1 ) { // Roll Loop openLogPortPrintF("1,%1d,%9.4f,%9.4f,%9.4f,%9.4f,%9.4f,%9.4f\n", flightMode, rateCmd[ROLL], sensors.gyro500Hz[ROLL], ratePID[ROLL], attCmd[ROLL], sensors.attitude500Hz[ROLL], attPID[ROLL]); } if ( eepromConfig.activeTelemetry == 2 ) { // Pitch Loop openLogPortPrintF("2,%1d,%9.4f,%9.4f,%9.4f,%9.4f,%9.4f,%9.4f\n", flightMode, rateCmd[PITCH], sensors.gyro500Hz[PITCH], ratePID[PITCH], attCmd[PITCH], sensors.attitude500Hz[PITCH], attPID[PITCH]); } if ( eepromConfig.activeTelemetry == 4 ) { // Sensors openLogPortPrintF("3,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,\n", sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS], sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], sensors.attitude500Hz[ROLL], sensors.attitude500Hz[PITCH], sensors.attitude500Hz[YAW]); } if ( eepromConfig.activeTelemetry == 8 ) { } if ( eepromConfig.activeTelemetry == 16) { // Vertical Variables openLogPortPrintF("%9.4f, %9.4f, %9.4f, %4ld, %1d, %9.4f\n", verticalVelocityCmd, hDotEstimate, hEstimate, ms5611Temperature, verticalModeState, throttleCmd); } } executionTime100Hz = micros() - currentTime; #ifdef _DTIMING LA3_DISABLE; #endif } /////////////////////////////// if (frame_5Hz) { frame_5Hz = false; currentTime = micros(); deltaTime5Hz = currentTime - previous5HzTime; previous5HzTime = currentTime; if (gpsValid() == true) { } //if (eepromConfig.mavlinkEnabled == true) //{ // mavlinkSendGpsRaw(); //} if (batMonVeryLowWarning > 0) { LED1_TOGGLE; batMonVeryLowWarning--; } if (execUp == true) BLUE_LED_TOGGLE; executionTime5Hz = micros() - currentTime; } /////////////////////////////// if (frame_1Hz) { frame_1Hz = false; currentTime = micros(); deltaTime1Hz = currentTime - previous1HzTime; previous1HzTime = currentTime; if (execUp == true) GREEN_LED_TOGGLE; if (execUp == false) execUpCount++; // Initialize sensors after being warmed up if ((execUpCount == 20) && (execUp == false)) { computeMPU6000RTData(); initMag(); initPressure(); } // Initialize PWM and set mag after sensor warmup if ((execUpCount == 25) && (execUp == false)) { execUp = true; pwmEscInit(); homeData.magHeading = sensors.attitude500Hz[YAW]; } if (batMonLowWarning > 0) { LED1_TOGGLE; batMonLowWarning--; } if (eepromConfig.mavlinkEnabled == true) { mavlinkSendHeartbeat(); mavlinkSendSysStatus(); } executionTime1Hz = micros() - currentTime; } //////////////////////////////// } /////////////////////////////////////////////////////////////////////////// }