static void subTaskMotorUpdate(void) { uint32_t startTime = 0; if (debugMode == DEBUG_CYCLETIME) { startTime = micros(); static uint32_t previousMotorUpdateTime; const uint32_t currentDeltaTime = startTime - previousMotorUpdateTime; debug[2] = currentDeltaTime; debug[3] = currentDeltaTime - targetPidLooptime; previousMotorUpdateTime = startTime; } else if (debugMode == DEBUG_PIDLOOP) { startTime = micros(); } mixTable(currentPidProfile); #ifdef USE_SERVOS // motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos. if (isMixerUsingServos()) { writeServos(); } #endif if (motorControlEnable) { writeMotors(); } DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime); }
void subTaskMotorUpdate(void) { const uint32_t startTime = micros(); if (debugMode == DEBUG_CYCLETIME) { static uint32_t previousMotorUpdateTime; const uint32_t currentDeltaTime = startTime - previousMotorUpdateTime; debug[2] = currentDeltaTime; debug[3] = currentDeltaTime - targetPidLooptime; previousMotorUpdateTime = startTime; } mixTable(¤tProfile->pidProfile); #ifdef USE_SERVOS filterServos(); writeServos(); #endif if (motorControlEnable) { writeMotors(); } if (debugMode == DEBUG_PIDLOOP) {debug[3] = micros() - startTime;} }
void loop(void) { static uint32_t loopTime; #if defined(BARO) || defined(SONAR) static bool haveProcessedAnnexCodeOnce = false; #endif updateRx(currentTime); if (shouldProcessRx(currentTime)) { 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 } else { // not processing rx this iteration executePeriodicTasks(); // 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 #ifdef GPS if (feature(FEATURE_GPS)) { gpsThread(); } #endif } currentTime = micros(); if (shouldRunLoop(loopTime)) { loopTime = currentTime + targetLooptime; imuUpdate(¤tProfile->accelerometerTrims); // Measure loop rate just after reading the sensors currentTime = micros(); cycleTime = (int32_t)(currentTime - previousTime); previousTime = currentTime; dT = (float)cycleTime * 0.000001f; annexCode(); if (masterConfig.rxConfig.rcSmoothing) { filterRc(); } #if defined(BARO) || defined(SONAR) haveProcessedAnnexCodeOnce = true; #endif #ifdef MAG if (sensors(SENSOR_MAG)) { updateMagHold(); } #endif #ifdef GTUNE updateGtuneState(); #endif #if defined(BARO) || defined(SONAR) if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { applyAltHold(&masterConfig.airplaneConfig); } } #endif // If we're armed, at minimum throttle, and we do arming via the // sticks, do not process yaw input from the rx. We do this so the // motors do not spin up while we are trying to arm or disarm. // Allow yaw control for tricopters if the user wants the servo to move even when unarmed. if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck #ifndef USE_QUAD_MIXER_ONLY #ifdef USE_SERVOS && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo) #endif && masterConfig.mixerMode != MIXER_AIRPLANE && masterConfig.mixerMode != MIXER_FLYING_WING #endif ) { rcCommand[YAW] = 0; } if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value); } #ifdef GPS if (sensors(SENSOR_GPS)) { if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) { updateGpsStateForHomeAndHoldMode(); } } #endif // PID - note this is function pointer set by setPIDController() pid_controller( ¤tProfile->pidProfile, currentControlRateProfile, masterConfig.max_angle_inclination, ¤tProfile->accelerometerTrims, &masterConfig.rxConfig ); mixTable(); #ifdef USE_SERVOS filterServos(); writeServos(); #endif if (motorControlEnable) { writeMotors(); } #ifdef BLACKBOX if (!cliMode && feature(FEATURE_BLACKBOX)) { handleBlackbox(); } #endif } #ifdef TELEMETRY if (!cliMode && feature(FEATURE_TELEMETRY)) { telemetryProcess(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); } #endif #ifdef LED_STRIP if (feature(FEATURE_LED_STRIP)) { updateLedStrip(); } #endif }
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; } //////////////////////////////// } /////////////////////////////////////////////////////////////////////////// }
void taskMainPidLoop(void) { cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; imuUpdateAccelerometer(); imuUpdateGyroAndAttitude(); annexCode(); if (masterConfig.rxConfig.rcSmoothing) { filterRc(isRXDataNew); } #if defined(NAV) if (isRXDataNew) { updateWaypointsAndNavigationMode(); } #endif isRXDataNew = false; #if defined(NAV) updatePositionEstimator(); applyWaypointNavigationAndAltitudeHold(); #endif // If we're armed, at minimum throttle, and we do arming via the // sticks, do not process yaw input from the rx. We do this so the // motors do not spin up while we are trying to arm or disarm. // Allow yaw control for tricopters if the user wants the servo to move even when unarmed. if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck #ifndef USE_QUAD_MIXER_ONLY #ifdef USE_SERVOS && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.servoMixerConfig.tri_unarmed_servo) #endif && masterConfig.mixerMode != MIXER_AIRPLANE && masterConfig.mixerMode != MIXER_FLYING_WING && masterConfig.mixerMode != MIXER_CUSTOM_AIRPLANE #endif ) { rcCommand[YAW] = 0; } // Apply throttle tilt compensation if (!STATE(FIXED_WING)) { int16_t thrTiltCompStrength = 0; if (navigationRequiresThrottleTiltCompensation()) { thrTiltCompStrength = 100; } else if (currentProfile->throttle_tilt_compensation_strength && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { thrTiltCompStrength = currentProfile->throttle_tilt_compensation_strength; } if (thrTiltCompStrength) { rcCommand[THROTTLE] = constrain(masterConfig.motorConfig.minthrottle + (rcCommand[THROTTLE] - masterConfig.motorConfig.minthrottle) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength), masterConfig.motorConfig.minthrottle, masterConfig.motorConfig.maxthrottle); } } pidController(¤tProfile->pidProfile, currentControlRateProfile, &masterConfig.rxConfig); #ifdef HIL if (hilActive) { hilUpdateControlState(); motorControlEnable = false; } #endif mixTable(); #ifdef USE_SERVOS if (isMixerUsingServos()) { servoMixer(currentProfile->flaperon_throw_offset, currentProfile->flaperon_throw_inverted); } if (feature(FEATURE_SERVO_TILT)) { processServoTilt(); } //Servos should be filtered or written only when mixer is using servos or special feaures are enabled if (isServoOutputEnabled()) { filterServos(); writeServos(); } #endif if (motorControlEnable) { writeMotors(); } #ifdef USE_SDCARD afatfs_poll(); #endif #ifdef BLACKBOX if (!cliMode && feature(FEATURE_BLACKBOX)) { handleBlackbox(); } #endif }
void executePeriodicTasks(void) { static int periodicTaskIndex = 0; switch (periodicTaskIndex++) { #ifdef MAG case UPDATE_COMPASS_TASK: if (sensors(SENSOR_MAG)) { updateCompass(&masterConfig.magZero); } break; #endif #ifdef BARO case UPDATE_BARO_TASK: if (sensors(SENSOR_BARO)) { baroUpdate(currentTime); } break; #endif #if defined(BARO) || defined(SONAR) case CALCULATE_ALTITUDE_TASK: #if defined(BARO) && !defined(SONAR) if (sensors(SENSOR_BARO) && isBaroReady()) { #endif #if defined(BARO) && defined(SONAR) if ((sensors(SENSOR_BARO) && isBaroReady()) || sensors(SENSOR_SONAR)) { #endif #if !defined(BARO) && defined(SONAR) if (sensors(SENSOR_SONAR)) { #endif calculateEstimatedAltitude(currentTime); } break; #endif #ifdef SONAR case UPDATE_SONAR_TASK: if (sensors(SENSOR_SONAR)) { sonarUpdate(); } break; #endif #ifdef DISPLAY case UPDATE_DISPLAY_TASK: if (feature(FEATURE_DISPLAY)) { updateDisplay(); } break; #endif } if (periodicTaskIndex >= PERIODIC_TASK_COUNT) { periodicTaskIndex = 0; } } void processRx(void) { calculateRxChannelsAndUpdateFailsafe(currentTime); // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) mwDisarm(); } updateRSSI(currentTime); if (feature(FEATURE_FAILSAFE)) { if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsEnabled()) { failsafeEnable(); } failsafeUpdateState(); } throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); if (throttleStatus == THROTTLE_LOW) { pidResetErrorAngle(); pidResetErrorGyro(); } // When armed and motors aren't spinning, disarm board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) && masterConfig.auto_disarm_delay != 0 && isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { if ((int32_t)(disarmAt - millis()) < 0) // delay is over mwDisarm(); } else { disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; // extend delay } } processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.retarded_arm, masterConfig.disarm_kill_switch); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); } updateActivatedModes(currentProfile->modeActivationConditions); if (!cliMode) { updateAdjustmentStates(currentProfile->adjustmentRanges); processRcAdjustments(currentControlRateProfile, &masterConfig.rxConfig); } bool canUseHorizonMode = true; if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeHasTimerElapsed())) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode canUseHorizonMode = false; if (!FLIGHT_MODE(ANGLE_MODE)) { pidResetErrorAngle(); ENABLE_FLIGHT_MODE(ANGLE_MODE); } } else { DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { pidResetErrorAngle(); ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else { DISABLE_FLIGHT_MODE(HORIZON_MODE); } if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; } else { LED1_OFF; } #ifdef MAG if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { ENABLE_FLIGHT_MODE(MAG_MODE); magHold = heading; } } else { DISABLE_FLIGHT_MODE(MAG_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { headFreeModeHold = heading; // acquire new heading } } #endif #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsWaypointsAndMode(); } #endif if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { DISABLE_FLIGHT_MODE(PASSTHRU_MODE); } if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } } void loop(void) { static uint32_t loopTime; #if defined(BARO) || defined(SONAR) static bool haveProcessedAnnexCodeOnce = false; #endif updateRx(); if (shouldProcessRx(currentTime)) { processRx(); #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 } else { // not processing rx this iteration executePeriodicTasks(); // 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 #ifdef GPS if (feature(FEATURE_GPS)) { gpsThread(); } #endif } currentTime = micros(); if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) { loopTime = currentTime + masterConfig.looptime; imuUpdate(¤tProfile->accelerometerTrims, masterConfig.mixerMode); // Measure loop rate just after reading the sensors currentTime = micros(); cycleTime = (int32_t)(currentTime - previousTime); previousTime = currentTime; annexCode(); #if defined(BARO) || defined(SONAR) haveProcessedAnnexCodeOnce = true; #endif #ifdef AUTOTUNE updateAutotuneState(); #endif #ifdef MAG if (sensors(SENSOR_MAG)) { updateMagHold(); } #endif #if defined(BARO) || defined(SONAR) if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { applyAltHold(&masterConfig.airplaneConfig); } } #endif // If we're armed, at minimum throttle, and we do arming via the // sticks, do not process yaw input from the rx. We do this so the // motors do not spin up while we are trying to arm or disarm. if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck) { rcCommand[YAW] = 0; } if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value); } #ifdef GPS if (sensors(SENSOR_GPS)) { if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) { updateGpsStateForHomeAndHoldMode(); } } #endif // PID - note this is function pointer set by setPIDController() pid_controller( ¤tProfile->pidProfile, currentControlRateProfile, masterConfig.max_angle_inclination, ¤tProfile->accelerometerTrims, &masterConfig.rxConfig ); mixTable(); #ifdef USE_SERVOS filterServos(); writeServos(); #endif writeMotors(); #ifdef BLACKBOX if (!cliMode && feature(FEATURE_BLACKBOX)) { handleBlackbox(); } #endif } #ifdef TELEMETRY if (!cliMode && feature(FEATURE_TELEMETRY)) { handleTelemetry(); } #endif #ifdef LED_STRIP if (feature(FEATURE_LED_STRIP)) { updateLedStrip(); } #endif }
void executePeriodicTasks(void) { static int periodicTaskIndex = 0; switch (periodicTaskIndex++) { #ifdef MAG case UPDATE_COMPASS_TASK: if (sensors(SENSOR_MAG)) { updateCompass(&masterConfig.magZero); } break; #endif #ifdef BARO case UPDATE_BARO_TASK: if (sensors(SENSOR_BARO)) { baroUpdate(currentTime); } break; #endif #if defined(BARO) || defined(SONAR) case CALCULATE_ALTITUDE_TASK: #if defined(BARO) && !defined(SONAR) if (sensors(SENSOR_BARO) && isBaroReady()) { #endif #if defined(BARO) && defined(SONAR) if ((sensors(SENSOR_BARO) && isBaroReady()) || sensors(SENSOR_SONAR)) { #endif #if !defined(BARO) && defined(SONAR) if (sensors(SENSOR_SONAR)) { #endif calculateEstimatedAltitude(currentTime); } break; #endif #ifdef SONAR case UPDATE_SONAR_TASK: if (sensors(SENSOR_SONAR)) { sonarUpdate(); } break; #endif #ifdef DISPLAY case UPDATE_DISPLAY_TASK: if (feature(FEATURE_DISPLAY)) { updateDisplay(); } break; #endif } if (periodicTaskIndex >= PERIODIC_TASK_COUNT) { periodicTaskIndex = 0; } } void processRx(void) { static bool armedBeeperOn = false; calculateRxChannelsAndUpdateFailsafe(currentTime); // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) mwDisarm(); } updateRSSI(currentTime); if (feature(FEATURE_FAILSAFE)) { if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) { failsafeStartMonitoring(); } failsafeUpdateState(); } throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); if (throttleStatus == THROTTLE_LOW) { pidResetErrorAngle(); pidResetErrorGyro(); } // When armed and motors aren't spinning, do beeps and then disarm // board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) ) { if (isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { if (masterConfig.auto_disarm_delay != 0 && (int32_t)(disarmAt - millis()) < 0 ) { // auto-disarm configured and delay is over mwDisarm(); armedBeeperOn = false; } else { // still armed; do warning beeps while armed beeper(BEEPER_ARMED); armedBeeperOn = true; } } else { // throttle is not low if (masterConfig.auto_disarm_delay != 0) { // extend disarm time disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; } if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } else { // arming is via AUX switch; beep while throttle low if (throttleStatus == THROTTLE_LOW) { beeper(BEEPER_ARMED); armedBeeperOn = true; } else if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.retarded_arm, masterConfig.disarm_kill_switch); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); } updateActivatedModes(currentProfile->modeActivationConditions); if (!cliMode) { updateAdjustmentStates(currentProfile->adjustmentRanges); processRcAdjustments(currentControlRateProfile, &masterConfig.rxConfig); } bool canUseHorizonMode = true; if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive())) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode canUseHorizonMode = false; if (!FLIGHT_MODE(ANGLE_MODE)) { pidResetErrorAngle(); ENABLE_FLIGHT_MODE(ANGLE_MODE); } } else { DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { pidResetErrorAngle(); ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else { DISABLE_FLIGHT_MODE(HORIZON_MODE); } if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; } else { LED1_OFF; } #ifdef MAG if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { ENABLE_FLIGHT_MODE(MAG_MODE); magHold = heading; } } else { DISABLE_FLIGHT_MODE(MAG_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { headFreeModeHold = heading; // acquire new heading } } #endif #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsWaypointsAndMode(); } #endif if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { DISABLE_FLIGHT_MODE(PASSTHRU_MODE); } if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { if ((!masterConfig.telemetryConfig.telemetry_switch && ARMING_FLAG(ARMED)) || (masterConfig.telemetryConfig.telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) { releaseSharedTelemetryPorts(); } else { // the telemetry state must be checked immediately so that shared serial ports are released. telemetryCheckState(); mspAllocateSerialPorts(&masterConfig.serialConfig); } } #endif } void filterRc(void){ static int16_t lastCommand[4] = { 0, 0, 0, 0 }; static int16_t deltaRC[4] = { 0, 0, 0, 0 }; static int16_t factor, rcInterpolationFactor; static filterStatePt1_t filteredCycleTimeState; uint16_t rxRefreshRate, filteredCycleTime; // Set RC refresh rate for sampling and channels to filter initRxRefreshRate(&rxRefreshRate); filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1); rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1; if (isRXDataNew) { for (int channel=0; channel < 4; channel++) { deltaRC[channel] = rcData[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); lastCommand[channel] = rcData[channel]; } isRXDataNew = false; factor = rcInterpolationFactor - 1; } else { factor--; } // Interpolate steps of rcData if (factor > 0) { for (int channel=0; channel < 4; channel++) { rcData[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; } } else { factor = 0; } } void loop(void) { static uint32_t loopTime; #if defined(BARO) || defined(SONAR) static bool haveProcessedAnnexCodeOnce = false; #endif updateRx(currentTime); if (shouldProcessRx(currentTime)) { 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 } else { // not processing rx this iteration executePeriodicTasks(); // 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 #ifdef GPS if (feature(FEATURE_GPS)) { gpsThread(); } #endif } currentTime = micros(); if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) { loopTime = currentTime + masterConfig.looptime; imuUpdate(¤tProfile->accelerometerTrims); // Measure loop rate just after reading the sensors currentTime = micros(); cycleTime = (int32_t)(currentTime - previousTime); previousTime = currentTime; // Gyro Low Pass if (currentProfile->pidProfile.gyro_cut_hz) { int axis; static filterStatePt1_t gyroADCState[XYZ_AXIS_COUNT]; for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz); } } if (masterConfig.rxConfig.rcSmoothing) { filterRc(); } annexCode(); #if defined(BARO) || defined(SONAR) haveProcessedAnnexCodeOnce = true; #endif #ifdef MAG if (sensors(SENSOR_MAG)) { updateMagHold(); } #endif #ifdef GTUNE updateGtuneState(); #endif #if defined(BARO) || defined(SONAR) if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { applyAltHold(&masterConfig.airplaneConfig); } } #endif // If we're armed, at minimum throttle, and we do arming via the // sticks, do not process yaw input from the rx. We do this so the // motors do not spin up while we are trying to arm or disarm. // Allow yaw control for tricopters if the user wants the servo to move even when unarmed. if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck #ifndef USE_QUAD_MIXER_ONLY && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo) && masterConfig.mixerMode != MIXER_AIRPLANE && masterConfig.mixerMode != MIXER_FLYING_WING #endif ) { rcCommand[YAW] = 0; } if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value); } #ifdef GPS if (sensors(SENSOR_GPS)) { if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) { updateGpsStateForHomeAndHoldMode(); } } #endif // PID - note this is function pointer set by setPIDController() pid_controller( ¤tProfile->pidProfile, currentControlRateProfile, masterConfig.max_angle_inclination, ¤tProfile->accelerometerTrims, &masterConfig.rxConfig ); mixTable(); #ifdef USE_SERVOS filterServos(); writeServos(); #endif if (motorControlEnable) { writeMotors(); } #ifdef BLACKBOX if (!cliMode && feature(FEATURE_BLACKBOX)) { handleBlackbox(); } #endif } #ifdef TELEMETRY if (!cliMode && feature(FEATURE_TELEMETRY)) { telemetryProcess(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); } #endif #ifdef LED_STRIP if (feature(FEATURE_LED_STRIP)) { updateLedStrip(); } #endif }
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; } //////////////////////////////// } /////////////////////////////////////////////////////////////////////////// }
void taskMainPidLoop(void) { cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; // Calculate average cycle time and average jitter filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1, dT); debug[0] = cycleTime; debug[1] = cycleTime - filteredCycleTime; imuUpdateGyroAndAttitude(); annexCode(); if (rxConfig()->rcSmoothing) { filterRc(); } #if defined(BARO) || defined(SONAR) haveProcessedAnnexCodeOnce = true; #endif #ifdef MAG if (sensors(SENSOR_MAG)) { updateMagHold(); } #endif #if defined(BARO) || defined(SONAR) if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { applyAltHold(); } } #endif // If we're armed, at minimum throttle, and we do arming via the // sticks, do not process yaw input from the rx. We do this so the // motors do not spin up while we are trying to arm or disarm. // Allow yaw control for tricopters if the user wants the servo to move even when unarmed. if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck #ifndef USE_QUAD_MIXER_ONLY #ifdef USE_SERVOS && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && mixerConfig()->tri_unarmed_servo) #endif && mixerConfig()->mixerMode != MIXER_AIRPLANE && mixerConfig()->mixerMode != MIXER_FLYING_WING #endif ) { rcCommand[YAW] = 0; } if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value); } #ifdef GPS if (sensors(SENSOR_GPS)) { if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) { updateGpsStateForHomeAndHoldMode(); } } #endif // PID - note this is function pointer set by setPIDController() pid_controller( pidProfile(), currentControlRateProfile, imuConfig()->max_angle_inclination, &accelerometerConfig()->accelerometerTrims, rxConfig() ); mixTable(); #ifdef USE_SERVOS filterServos(); writeServos(); #endif if (motorControlEnable) { writeMotors(); } #ifdef USE_SDCARD afatfs_poll(); #endif #ifdef BLACKBOX if (!cliMode && feature(FEATURE_BLACKBOX)) { handleBlackbox(); } #endif }
int main(void) { uint32_t currentTime; // High Speed Telemetry Test Code Begin char numberString[12]; // High Speed Telemetry Test Code End RCC_GetClocksFreq(&clocks); USB_Interrupts_Config(); Set_USBClock(); USB_Init(); // Wait until device configured //while(bDeviceState != CONFIGURED); testInit(); LED0_ON; systemReady = true; //nrf_tx_mode_no_aa(addr,5,40); nrf_rx_mode_dual(addr,5,40); { uint8_t status = nrf_read_reg(NRF_STATUS); nrf_write_reg(NRF_WRITE_REG|NRF_STATUS,status); // clear IRQ flags nrf_write_reg(NRF_FLUSH_RX, 0xff); nrf_write_reg(NRF_FLUSH_TX, 0xff); } while (1) { uint8_t buf[64]; static uint8_t last_tx_done = 0; if(ring_buf_pop(nrf_rx_buffer,buf,32)){ // get data from the adapter switch(buf[0]){ case SET_ATT: break; case SET_MOTOR: break; case SET_MODE: report_mode = buf[1]; break; } last_tx_done = 1; } if(tx_done){ tx_done = 0; // report ACK success last_tx_done = 1; } if(ring_buf_pop(nrf_tx_buffer,buf,32)){ if(last_tx_done){ last_tx_done = 0; nrf_ack_packet(0, buf, 32); } } if (frame_50Hz) { int16_t motor_val[4]; frame_50Hz = false; currentTime = micros(); deltaTime50Hz = currentTime - previous50HzTime; previous50HzTime = currentTime; //memcpy(buf, accelSummedSamples100Hz, 12); //memcpy(buf+12, gyroSummedSamples100Hz, 12); //memcpy(buf+24, magSumed, 6); if(report_mode == DT_ATT){ buf[0] = DT_ATT; memcpy(buf + 1, &sensors.attitude200Hz[0], 12); memcpy(buf + 13, &executionTime200Hz, 4); motor_val[0] = motor[0]; motor_val[1] = motor[1]; motor_val[2] = motor[2]; motor_val[3] = motor[3]; memcpy(buf + 17, motor_val, 8); usb_send_data(buf , 64); executionTime50Hz = micros() - currentTime; }else if(report_mode == DT_SENSOR){ buf[0] = DT_SENSOR; memcpy(buf + 1, gyroSummedSamples100Hz, 12); memcpy(buf + 13, accelSummedSamples100Hz, 12); memcpy(buf + 25, magSumed, 6); } //nrf_tx_packet(buf,16); //if(nrf_rx_packet(buf,16) == NRF_RX_OK) //{ // LED0_TOGGLE; //} ring_buf_push(nrf_tx_buffer, buf, 32); } if(frame_10Hz) { frame_10Hz = false; magSumed[XAXIS] = magSum[XAXIS]; magSumed[YAXIS] = magSum[YAXIS]; magSumed[ZAXIS] = magSum[ZAXIS]; magSum[XAXIS] = 0; magSum[YAXIS] = 0; magSum[ZAXIS] = 0; newMagData = true; } if (frame_100Hz) { frame_100Hz = false; computeAxisCommands(dt100Hz); mixTable(); writeServos(); writeMotors(); } if (frame_200Hz) { frame_200Hz = false; currentTime = micros(); deltaTime200Hz = currentTime - previous200HzTime; previous200HzTime = currentTime; dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop #if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS) sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS]; sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS]; sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.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; #endif #if defined(USE_MADGWICK_AHRS) MadgwickAHRSupdate( 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], sensorConfig.accelCutoff, newMagData, dt200Hz ); newMagData = false; q0q0 = q0 * q0; q1q1 = q1 * q1; q2q2 = q2 * q2; q3q3 = q3 * q3; sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 ); sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) ); sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 ); #endif #if defined(USE_MARG_AHRS) 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], sensorConfig.accelCutoff, newMagData, dt200Hz ); newMagData = false; q0q0 = q0 * q0; q1q1 = q1 * q1; q2q2 = q2 * q2; q3q3 = q3 * q3; sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 ); sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) ); sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 ); #endif executionTime200Hz = micros() - 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) { LED0_TOGGLE; frame_10Hz = false; currentTime = micros(); deltaTime10Hz = currentTime - previous10HzTime; previous10HzTime = currentTime; sensors.mag10Hz[XAXIS] = -((float)magSum[XAXIS] / 5.0f * magScaleFactor[XAXIS] - sensorConfig.magBias[XAXIS]); sensors.mag10Hz[YAXIS] = (float)magSum[YAXIS] / 5.0f * magScaleFactor[YAXIS] - sensorConfig.magBias[YAXIS]; sensors.mag10Hz[ZAXIS] = -((float)magSum[ZAXIS] / 5.0f * magScaleFactor[ZAXIS] - sensorConfig.magBias[ZAXIS]); magSum[XAXIS] = 0; magSum[YAXIS] = 0; magSum[ZAXIS] = 0; newMagData = true; pressureAverage = pressureSum / 10; pressureSum = 0; calculateTemperature(); calculatePressureAltitude(); sensors.pressureAltitude10Hz = pressureAlt; serialCom(); if ( EKF_Initialized == false ) EKF_Init( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS] ); 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 computeGyroTCBias(); 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; #if defined(USE_CHR6DM_AHRS) if ( EKF_Initialized == true ) EKF_Predict( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW], dt500Hz ); sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi; sensors.attitude200Hz[PITCH] = gEstimatedStates.theta; sensors.attitude200Hz[YAW ] = gEstimatedStates.psi; #endif executionTime500Hz = 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 #if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS) sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS]; sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS]; sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.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; #endif #if defined(USE_MADGWICK_AHRS) MadgwickAHRSupdate( 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], sensorConfig.accelCutoff, newMagData, dt200Hz ); newMagData = false; q0q0 = q0 * q0; q1q1 = q1 * q1; q2q2 = q2 * q2; q3q3 = q3 * q3; sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 ); sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) ); sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 ); #endif #if defined(USE_MARG_AHRS) 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], sensorConfig.accelCutoff, newMagData, dt200Hz ); newMagData = false; q0q0 = q0 * q0; q1q1 = q1 * q1; q2q2 = q2 * q2; q3q3 = q3 * q3; sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 ); sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) ); sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 ); #endif 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] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS]; sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS]; sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.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; #if defined(USE_CHR6DM_AHRS) if ( EKF_Initialized == true ) EKF_Update( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], sensorConfig.accelCutoff, newMagData ); newMagData = false; sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi; sensors.attitude200Hz[PITCH] = gEstimatedStates.theta; sensors.attitude200Hz[YAW ] = gEstimatedStates.psi; #endif computeAxisCommands(dt100Hz); mixTable(); writeServos(); writeMotors(); // High Speed Telemetry Test Code Begin if ( highSpeedAccelTelemEnabled == true ) { // 100 Hz Accels ftoa(sensors.accel100Hz[XAXIS], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.accel100Hz[YAXIS], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.accel100Hz[ZAXIS], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedGyroTelemEnabled == true ) { // 100 Hz Gyros ftoa(sensors.gyro100Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.gyro100Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedRollRateTelemEnabled == true ) { // Roll Rate, Roll Rate Command ftoa(sensors.gyro100Hz[ROLL], numberString); uartPrint(numberString); uartPrint(","); ftoa(rxCommand[ROLL], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedPitchRateTelemEnabled == true ) { // Pitch Rate, Pitch Rate Command ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(","); ftoa(rxCommand[PITCH], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedYawRateTelemEnabled == true ) { // Yaw Rate, Yaw Rate Command ftoa(sensors.gyro100Hz[YAW], numberString); uartPrint(numberString); uartPrint(","); ftoa(rxCommand[YAW], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedAttitudeTelemEnabled == true ) { // 200 Hz Attitudes ftoa(sensors.attitude200Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.attitude200Hz[PITCH], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.attitude200Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n"); } // High Speed Telemetry Test Code End 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; executionTime1Hz = micros() - currentTime; } //////////////////////////////// } }
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; } //////////////////////////////// } /////////////////////////////////////////////////////////////////////////// }
int main(void) { uint32_t currentTime; // High Speed Telemetry Test Code Begin char numberString[12]; // High Speed Telemetry Test Code End 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)magSum[XAXIS] / 5.0f * magScaleFactor[XAXIS] - sensorConfig.magBias[XAXIS]); sensors.mag10Hz[YAXIS] = (float)magSum[YAXIS] / 5.0f * magScaleFactor[YAXIS] - sensorConfig.magBias[YAXIS]; sensors.mag10Hz[ZAXIS] = -((float)magSum[ZAXIS] / 5.0f * magScaleFactor[ZAXIS] - sensorConfig.magBias[ZAXIS]); magSum[XAXIS] = 0; magSum[YAXIS] = 0; magSum[ZAXIS] = 0; newMagData = true; pressureAverage = pressureSum / 10; pressureSum = 0; calculateTemperature(); calculatePressureAltitude(); sensors.pressureAltitude10Hz = pressureAlt; serialCom(); if ( EKF_Initialized == false ) EKF_Init( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS] ); 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 computeGyroTCBias(); 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; #if defined(USE_CHR6DM_AHRS) if ( EKF_Initialized == true ) EKF_Predict( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW], dt500Hz ); sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi; sensors.attitude200Hz[PITCH] = gEstimatedStates.theta; sensors.attitude200Hz[YAW ] = gEstimatedStates.psi; #endif executionTime500Hz = 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 #if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS) sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS]; sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS]; sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.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; #endif #if defined(USE_MADGWICK_AHRS) MadgwickAHRSupdate( 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], sensorConfig.accelCutoff, newMagData, dt200Hz ); newMagData = false; q0q0 = q0 * q0; q1q1 = q1 * q1; q2q2 = q2 * q2; q3q3 = q3 * q3; sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 ); sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) ); sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 ); #endif #if defined(USE_MARG_AHRS) 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], sensorConfig.accelCutoff, newMagData, dt200Hz ); newMagData = false; q0q0 = q0 * q0; q1q1 = q1 * q1; q2q2 = q2 * q2; q3q3 = q3 * q3; sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 ); sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) ); sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 ); #endif 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] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS]; sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS]; sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.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; #if defined(USE_CHR6DM_AHRS) if ( EKF_Initialized == true ) EKF_Update( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], sensorConfig.accelCutoff, newMagData ); newMagData = false; sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi; sensors.attitude200Hz[PITCH] = gEstimatedStates.theta; sensors.attitude200Hz[YAW ] = gEstimatedStates.psi; #endif computeAxisCommands(dt100Hz); mixTable(); writeServos(); writeMotors(); // High Speed Telemetry Test Code Begin if ( highSpeedAccelTelemEnabled == true ) { // 100 Hz Accels ftoa(sensors.accel100Hz[XAXIS], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.accel100Hz[YAXIS], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.accel100Hz[ZAXIS], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedGyroTelemEnabled == true ) { // 100 Hz Gyros ftoa(sensors.gyro100Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.gyro100Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedRollRateTelemEnabled == true ) { // Roll Rate, Roll Rate Command ftoa(sensors.gyro100Hz[ROLL], numberString); uartPrint(numberString); uartPrint(","); ftoa(rxCommand[ROLL], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedPitchRateTelemEnabled == true ) { // Pitch Rate, Pitch Rate Command ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(","); ftoa(rxCommand[PITCH], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedYawRateTelemEnabled == true ) { // Yaw Rate, Yaw Rate Command ftoa(sensors.gyro100Hz[YAW], numberString); uartPrint(numberString); uartPrint(","); ftoa(rxCommand[YAW], numberString); uartPrint(numberString); uartPrint("\n"); } if ( highSpeedAttitudeTelemEnabled == true ) { // 200 Hz Attitudes ftoa(sensors.attitude200Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.attitude200Hz[PITCH], numberString); uartPrint(numberString); uartPrint(","); ftoa(sensors.attitude200Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n"); } // High Speed Telemetry Test Code End 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; 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(); 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; } //////////////////////////////// } }
void loop(void) { static uint32_t loopTime; #ifdef BARO static bool haveProcessedAnnexCodeOnce = false; #endif updateRx(); if (shouldProcessRx(currentTime)) { processRx(); #ifdef BARO // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data. if (haveProcessedAnnexCodeOnce) { if (sensors(SENSOR_BARO)) { updateAltHoldState(); } } #endif } else { // not processing rx this iteration executePeriodicTasks(); } currentTime = micros(); if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) { loopTime = currentTime + masterConfig.looptime; computeIMU(¤tProfile.accelerometerTrims, masterConfig.mixerConfiguration); // Measure loop rate just after reading the sensors currentTime = micros(); cycleTime = (int32_t)(currentTime - previousTime); previousTime = currentTime; annexCode(); #ifdef BARO haveProcessedAnnexCodeOnce = true; #endif #ifdef AUTOTUNE updateAutotuneState(); #endif #ifdef MAG if (sensors(SENSOR_MAG)) { updateMagHold(); } #endif #ifdef BARO if (sensors(SENSOR_BARO)) { if (f.BARO_MODE) { updateAltHold(); } debug[0] = rcCommand[THROTTLE]; } #endif if (currentProfile.throttle_correction_value && (f.ANGLE_MODE || f.HORIZON_MODE)) { rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile.throttle_correction_value); } #ifdef GPS if (sensors(SENSOR_GPS)) { if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME) { updateGpsStateForHomeAndHoldMode(); } } #endif // PID - note this is function pointer set by setPIDController() pid_controller( ¤tProfile.pidProfile, ¤tProfile.controlRateConfig, masterConfig.max_angle_inclination, ¤tProfile.accelerometerTrims ); mixTable(); writeServos(); writeMotors(); } #ifdef TELEMETRY if (!cliMode && feature(FEATURE_TELEMETRY)) { handleTelemetry(); } #endif #ifdef LED_STRIP if (feature(FEATURE_LED_STRIP)) { updateLedStrip(); } #endif }
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; 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; } //////////////////////////////// } /////////////////////////////////////////////////////////////////////////// }