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
