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 loop() { // Process AI if (Settings[S_ENABLEADC]){ //temperature=(analogRead(temperaturePin)-102)/2.048; // Does someone use this ATM?? if (!Settings[S_MAINVOLTAGE_VBAT]){ static uint16_t ind = 0; static uint32_t voltageRawArray[8]; voltageRawArray[(ind++)%8] = analogRead(voltagePin); uint16_t voltageRaw = 0; for (uint16_t i=0;i<8;i++) voltageRaw += voltageRawArray[i]; voltage = float(voltageRaw) * Settings[S_DIVIDERRATIO] /1023; } if (!Settings[S_VIDVOLTAGE_VBAT]) { static uint16_t ind = 0; static uint32_t voltageRawArray[8]; voltageRawArray[(ind++)%8] = analogRead(vidvoltagePin); uint16_t voltageRaw = 0; for (uint16_t i=0;i<8;i++) voltageRaw += voltageRawArray[i]; vidvoltage = float(voltageRaw) * Settings[S_VIDDIVIDERRATIO] /1023; } if (!Settings[S_MWRSSI] && !Settings[S_PWMRSSI]) { rssiADC = analogRead(rssiPin)/4; // RSSI Readings, rssiADC=0 to 1023/4 (avoid a number > 255) } if (!Settings[S_MWAMPERAGE]) { int16_t currsensOffSet=(Settings[S_CURRSENSOFFSET_L] | (Settings[S_CURRSENSOFFSET_H] << 8)); // Read OffSetH/L amperageADC = analogRead(amperagePin); if (amperageADC > currsensOffSet) amperageADC=((amperageADC-currsensOffSet)*4.8828)/Settings[S_CURRSENSSENSITIVITY]; // [A] Positive Current flow (512...1023) or Unidir (0...1023) else amperageADC=((currsensOffSet-amperageADC)*4.8828)/Settings[S_CURRSENSSENSITIVITY]; // [A] Negative Current flow (0...512) } } if (Settings[S_MWAMPERAGE]) { amperagesum = MW_ANALOG.pMeterSum; amperage = MW_ANALOG.Amperage /100; } if (Settings[S_MWRSSI]) { rssiADC = MW_ANALOG.Rssi/4; // RSSI from MWii, rssiADC=0 to 1023/4 (avoid a number > 255) } if (Settings[S_PWMRSSI] && !Settings[S_MWRSSI]){ rssiADC = pulseIn(PWMrssiPin, HIGH,15000)/Settings[S_PWMRSSIDIVIDER]; // Reading W/time out (microseconds to wait for pulse to start: 15000=0.015sec) } // Blink Basic Sanity Test Led at 1hz - this stuff introduces strange behavior on my system if(tenthSec>10) digitalWrite(7,HIGH); else digitalWrite(7,LOW); //--------------- Start Timed Service Routines --------------------------------------- uint16_t currentMillis = millis(); if((currentMillis - previous_millis_low) >= lo_speed_cycle) // 10 Hz (Executed every 100ms) { previous_millis_low = currentMillis; tenthSec++; TempBlinkAlarm++; Blink10hz=!Blink10hz; if(!fontMode) blankserialRequest(MSP_ATTITUDE); if(Settings[L_RSSIPOSITIONDSPL]) calculateRssi(); } // End of slow Timed Service Routine (100ms loop) if((currentMillis - previous_millis_high) >= hi_speed_cycle) // 20 Hz (Executed every 50ms) { previous_millis_high = currentMillis; calculateTrip(); // Speed integration on 50msec if (!Settings[S_MWAMPERAGE]) calculateAmperage(); // Amperage and amperagesum integration on 50msec uint8_t MSPcmdsend; if(queuedMSPRequests == 0) queuedMSPRequests = modeMSPRequests; uint32_t req = queuedMSPRequests & -queuedMSPRequests; queuedMSPRequests &= ~req; switch(req) { case REQ_MSP_IDENT: MSPcmdsend = MSP_IDENT; break; case REQ_MSP_STATUS: MSPcmdsend = MSP_STATUS; break; case REQ_MSP_RAW_IMU: MSPcmdsend = MSP_RAW_IMU; break; case REQ_MSP_RC: MSPcmdsend = MSP_RC; break; case REQ_MSP_RAW_GPS: MSPcmdsend = MSP_RAW_GPS; break; case REQ_MSP_COMP_GPS: MSPcmdsend = MSP_COMP_GPS; break; case REQ_MSP_ATTITUDE: MSPcmdsend = MSP_ATTITUDE; break; case REQ_MSP_ALTITUDE: MSPcmdsend = MSP_ALTITUDE; break; case REQ_MSP_ANALOG: MSPcmdsend = MSP_ANALOG; break; case REQ_MSP_RC_TUNING: MSPcmdsend = MSP_RC_TUNING; break; case REQ_MSP_PID: MSPcmdsend = MSP_PID; break; case REQ_MSP_BOX: MSPcmdsend = MSP_BOXIDS; break; case REQ_MSP_FONT: MSPcmdsend = MSP_OSD; break; } if(!fontMode) blankserialRequest(MSPcmdsend); //MAX7456_DrawScreen(); if( allSec < 6 ){ displayIntro(KVTeamVersionPosition); lastCallSign = onTime; } else { if(armed){ previousarmedstatus=1; } if(previousarmedstatus && !armed){ configPage=9; ROW=10; COL=1; configMode=1; setMspRequests(); } if(fontMode) { displayFontScreen(); } else if(configMode) { displayConfigScreen(); } else { displayVoltage(); displayVidVoltage(); displayRSSI(); displayTime(); displaySensor(); displayGPSMode(); displayMode(); //if((temperature<Settings[S_TEMPERATUREMAX])||(BlinkAlarm)) displayTemperature(); displayAmperage(); displaypMeterSum(); displayArmed(); displayCurrentThrottle(); displayautoPilot(); if ( (onTime > (lastCallSign+300)) || (onTime < (lastCallSign+4))) { // Displays 4 sec every 5min (no blink during flight) if ( onTime > (lastCallSign+300))lastCallSign = onTime; displayCallsign(); } if(MW_STATUS.sensorPresent&ACCELEROMETER) displayHorizon(MW_ATT.Angle[0],MW_ATT.Angle[1]); if(MW_STATUS.sensorPresent&MAGNETOMETER) { displayHeadingGraph(); displayHeading(); } if(MW_STATUS.sensorPresent&BAROMETER) { displayAltitude(); displayClimbRate(); } if(MW_STATUS.sensorPresent&GPSSENSOR) if(Settings[S_DISPLAYGPS]){ displayNumberOfSat(); displayDirectionToHome(); displayDistanceToHome(); displayAngleToHome(); displayGPS_speed(); displayGPSPosition(); //displayGPS_altitude(); // Do not remove yet } } } MAX7456_DrawScreen(); } // End of fast Timed Service Routine (50ms loop) //--------------------- End of Timed Service Routine --------------------------------------- if(TempBlinkAlarm >= Settings[S_BLINKINGHZ]) { // selectable alarm blink freq TempBlinkAlarm = 0; BlinkAlarm =!BlinkAlarm; // 10=1Hz, 9=1.1Hz, 8=1.25Hz, 7=1.4Hz, 6=1.6Hz, 5=2Hz, 4=2.5Hz, 3=3.3Hz, 2=5Hz, 1=10Hz } if(tenthSec >= 10) // this execute 1 time a second { onTime++; tenthSec=0; if(!armed) { flyTime=0; } else { flyTime++; flyingTime++; configMode=0; setMspRequests(); } allSec++; if((accCalibrationTimer==1)&&(configMode)) { blankserialRequest(MSP_ACC_CALIBRATION); accCalibrationTimer=0; } if((magCalibrationTimer==1)&&(configMode)) { blankserialRequest(MSP_MAG_CALIBRATION); magCalibrationTimer=0; } if((eepromWriteTimer==1)&&(configMode)) { blankserialRequest(MSP_EEPROM_WRITE); eepromWriteTimer=0; } if(accCalibrationTimer>0) accCalibrationTimer--; if(magCalibrationTimer>0) magCalibrationTimer--; if(eepromWriteTimer>0) eepromWriteTimer--; if((rssiTimer==1)&&(configMode)) { Settings[S_RSSIMIN]=rssiADC; // set MIN RSSI signal received (tx off?) rssiTimer=0; } if(rssiTimer>0) rssiTimer--; } serialMSPreceive(); } // End of main loop