void readSystemEEPROM(void) { uint8_t* start = (uint8_t*)(&systemConfig); uint8_t* end = (uint8_t*)(&systemConfig + 1); /////////////////////////////////// setSPIdivisor(EEPROM_SPI, 2); // 18 MHz SPI clock systemConfigaddr.value = SYSTEM_EEPROM_ADDR; /////////////////////////////////// ENABLE_EEPROM; spiTransfer(EEPROM_SPI, READ_DATA); spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[2]); spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[1]); spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[0]); while (start < end) *start++ = spiTransfer(EEPROM_SPI, 0x00); DISABLE_EEPROM; delayMicroseconds(2); /////////////////////////////////// if ( crcCheckVal != crc32B((uint32_t*)(&systemConfig), // CRC32B[systemConfig CRC32B[systemConfig]] (uint32_t*)(&systemConfig + 1))) { evrPush(EVR_SystemCRCFail,0); systemConfig.CRCFlags |= CRC_HistoryBad; } else if ( systemConfig.CRCFlags & CRC_HistoryBad ) { evrPush(EVR_ConfigBadSystemHistory,0); } /////////////////////////////////// if (systemConfig.yawDirection >= 0) systemConfig.yawDirection = 1.0f; else systemConfig.yawDirection = -1.0f; }
void batMonLow() { /* need to do slow beeping here, push back in telem to flash controler * lights, etc. */ evrPush(EVR_BatLow, (int)(v_bat_ave*1000.0)); }
void batMonVeryLow() { /* need to do fast beeping here, push back in telem to flash controler * lights, etc. * User needs to decsend now ... */ evrPush(EVR_BatVeryLow, (int)(v_bat_ave*1000.0)); }
void readSensorEEPROM(void) { uint8_t* start = (uint8_t*)(&sensorConfig); uint8_t* end = (uint8_t*)(&sensorConfig + 1); /////////////////////////////////// setSPIdivisor(EEPROM_SPI, 2); // 18 MHz SPI clock sensorConfigAddr.value = SENSOR_EEPROM_ADDR; /////////////////////////////////// ENABLE_EEPROM; spiTransfer(EEPROM_SPI, READ_DATA); spiTransfer(EEPROM_SPI, sensorConfigAddr.bytes[2]); spiTransfer(EEPROM_SPI, sensorConfigAddr.bytes[1]); spiTransfer(EEPROM_SPI, sensorConfigAddr.bytes[0]); while (start < end) *start++ = spiTransfer(EEPROM_SPI, 0x00); DISABLE_EEPROM; delayMicroseconds(2); /////////////////////////////////// if ( crcCheckVal != crc32B((uint32_t*)(&sensorConfig), // CRC32B[sensorConfig CRC32B[sensorConfig]] (uint32_t*)(&sensorConfig + 1))) { evrPush(EVR_SensorCRCFail,0); sensorConfig.CRCFlags |= CRC_HistoryBad; } else if ( sensorConfig.CRCFlags & CRC_HistoryBad ) { evrPush(EVR_ConfigBadSensorHistory,0); } /////////////////////////////////// accConfidenceDecay = 1.0f / sqrt(sensorConfig.accelCutoff); }
void batMonMaxLow() { /* User isn't listening flyer needs to auto-descend now .... */ evrPush(EVR_BatMaxLow, (int)(v_bat_ave*1000.0)); // Maybe do something more interesting like auto-descent or hover-hold. // armed = false; }
int evrRegisterListener(evrListener_fp f) { int r = evrListenerTop < evrListenerMAX ; if (r) evrListeners[evrListenerTop++] = f ; else evrPush(EVR_OutOfListeners,0); return !r; }
void readEEPROM(void) { eepromConfig_t *dst = &eepromConfig; *dst = *(eepromConfig_t*)FLASH_WRITE_EEPROM_ADDR ; if ( crcCheckVal != crc32bEEPROM(dst, true) ) { evrPush(EVR_FlashCRCFail,0); dst->CRCFlags |= CRC_HistoryBad; } else if ( dst->CRCFlags & CRC_HistoryBad ) evrPush(EVR_ConfigBadHistory,0); accConfidenceDecay = 1.0f / sqrt(eepromConfig.accelCutoff); eepromConfig.yawDirection = constrain(eepromConfig.yawDirection, -1.0f, 1.0f); }
int writeEEPROM(void) { FLASH_Status status; int32_t i; eepromConfig_t *src = &eepromConfig; uint32_t *dst = (uint32_t*)FLASH_WRITE_EEPROM_ADDR; // there's no reason to write these values to EEPROM, they'll just be noise zeroPIDstates(); if ( src->CRCFlags & CRC_HistoryBad ) evrPush(EVR_ConfigBadHistory,0); src->CRCAtEnd[0] = crc32B( (uint32_t*)&src[0], src->CRCAtEnd); FLASH_Unlock(); FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); status = FLASH_EraseSector(FLASH_Sector_1, VoltageRange_3); /////////////////////////////////// i = -1; while ( FLASH_COMPLETE == status && i++ < eepromConfigNUMWORD ) status = FLASH_ProgramWord((uint32_t)&dst[i], ((uint32_t*)src)[i]); if ( FLASH_COMPLETE != status ) evrPush( -1 == i ? EVR_FlashEraseFail : EVR_FlashProgramFail, status); /////////////////////////////////// FLASH_Lock(); readEEPROM(); return status; }
void readEEPROM(void) { eepromConfig_t *dst = &eepromConfig; *dst = *(eepromConfig_t*)FLASH_WRITE_EEPROM_ADDR; if ( crcCheckVal != crc32B((uint32_t*)(&eepromConfig), // CRC32BeepromConfig CRC32B[eepromConfig]] (uint32_t*)(&eepromConfig + 1))) { evrPush(EVR_FlashCRCFail,0); dst->CRCFlags |= CRC_HistoryBad; } else if ( dst->CRCFlags & CRC_HistoryBad ) evrPush(EVR_ConfigBadHistory,0); /////////////////////////////////// if (eepromConfig.yawDirection >= 0) eepromConfig.yawDirection = 1.0f; else eepromConfig.yawDirection = -1.0f; }
void batMonLow(void) { // Need to do slow beeping here, push back in telem to flash controller // lights, etc. if (batMonLowWarningTriggered == false) { evrPush(EVR_BatLow, (int)(v_bat_ave * 1000.0f)); batMonLowWarning = 10; batMonLowWarningTriggered = true; } }
void batMonVeryLow(void) { // Need to do fast beeping here, push back in telem to flash controller // lights, etc. // User needs to descend now ... if (batMonVeryLowWarningTriggered == false) { evrPush(EVR_BatVeryLow, (int)(v_bat_ave * 1000.0f)); batMonVeryLowWarning = 50; batMonVeryLowWarningTriggered = true; } }
void batMonMaxLow(void) { // User isn't listening flyer needs to auto-descend now .... // Maybe do something more interesting like auto-descent or hover-hold. if (batMonMaxLowWarningTriggered == false) { evrPush(EVR_BatMaxLow, (int)(v_bat_ave * 1000.0f)); LED1_ON; batMonMaxLowWarningTriggered = true; } }
uint8_t writeEEPROM(void) { // there's no reason to write these values to EEPROM, they'll just be noise zeroPIDintegralError(); zeroPIDstates(); FLASH_Status status; int i; uint32_t *dst = (uint32_t*)FLASH_WRITE_EEPROM_ADDR; eepromConfig_t *src = &eepromConfig; if ( src->CRCFlags & CRC_HistoryBad ) evrPush(EVR_ConfigBadHistory,0); src->CRCAtEnd[0] = crc32B( (uint32_t*)&src[0], src->CRCAtEnd); FLASH_Unlock(); FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR); i = -1; status = FLASH_ErasePage(FLASH_WRITE_EEPROM_ADDR); while ( status == FLASH_COMPLETE && i++ < eepromConfigNUMWORD ) status = FLASH_ProgramWord((uint32_t)&dst[i], ((uint32_t*)src)[i]); if ( status != FLASH_COMPLETE ) evrPush( -1 == i ? EVR_FlashEraseFail : EVR_FlashProgramFail, status); FLASH_Lock(); readEEPROM(); return status; }
void cliPrintEEPROM(eepromConfig_t *e) { uint32_t old_crc = e->CRCAtEnd[0]; enum { line_length = 32, len = sizeof(eepromConfig_t) }; uint8_t *by = (uint8_t*)e; int i, j; e->CRCAtEnd[0] = crc32bEEPROM(e, false); if (e->CRCFlags & CRC_HistoryBad) evrPush(EVR_ConfigBadHistory, 0); for (i = 0; i < ceil((float)len / line_length); i++) { for (j = 0; j < min(line_length, len - line_length * i); j++) cliPortPrintF("%02X", by[i * line_length + j]); cliPortPrint("\n"); } e->CRCAtEnd[0] = old_crc; }
void cliPrintSystemConfig(void) { uint32_t old_crc = systemConfig.CRCAtEnd[0]; uint8_t *by = (uint8_t*)&systemConfig; int i, j; systemConfig.CRCAtEnd[0] = crc32B((uint32_t*)(&systemConfig), // CRC32B[systemConfig] (uint32_t*)(&systemConfig.CRCAtEnd)); if (systemConfig.CRCFlags & CRC_HistoryBad) evrPush(EVR_ConfigBadHistory, 0); for (i = 0; i < ceil((float)sizeof(systemConfig) / LINE_LENGTH); i++) { for (j = 0; j < min(LINE_LENGTH, sizeof(systemConfig) - LINE_LENGTH * i); j++) cliPortPrintF("%02X", by[i * LINE_LENGTH + j]); cliPortPrint("\n"); } systemConfig.CRCAtEnd[0] = old_crc; }
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; 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; } //////////////////////////////// } /////////////////////////////////////////////////////////////////////////// }
uint8_t writeEEPROM(void) { FLASH_Status status; int32_t i; /////////////////////////////////// if (eepromConfig.receiverType == SPEKTRUM) { USART_Cmd(USART6, DISABLE); TIM_Cmd(TIM6, DISABLE); if (eepromConfig.slaveSpektrum == true) USART_Cmd(UART4, DISABLE); } /////////////////////////////////// eepromConfig_t *src = &eepromConfig; uint32_t *dst = (uint32_t*)FLASH_WRITE_EEPROM_ADDR; if ( src->CRCFlags & CRC_HistoryBad ) evrPush(EVR_ConfigBadHistory,0); src->CRCAtEnd = crc32B((uint32_t*)(&eepromConfig), // CRC32B[eepromConfig] (uint32_t*)(&eepromConfig.CRCAtEnd)); /////////////////////////////////// FLASH_Unlock(); FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); status = FLASH_EraseSector(FLASH_Sector_1, VoltageRange_3); /////////////////////////////////// i = -1; while (FLASH_COMPLETE == status && i++ < eepromConfigSizeUint32) status = FLASH_ProgramWord((uint32_t)&dst[i], ((uint32_t*)src)[i]); if ( FLASH_COMPLETE != status ) evrPush( -1 == i ? EVR_FlashEraseFail : EVR_FlashProgramFail, status); /////////////////////////////////// FLASH_Lock(); readEEPROM(); /////////////////////////////////// if (eepromConfig.receiverType == SPEKTRUM) { primarySpektrumState.reSync = 1; TIM_Cmd(TIM6, ENABLE); USART_Cmd(USART6, ENABLE); if (eepromConfig.slaveSpektrum == true) { slaveSpektrumState.reSync = 1; USART_Cmd(UART4, ENABLE); } } /////////////////////////////////// return status; }
void writeSensorEEPROM(void) { uint16_t byteCount; uint8_t pageIndex; uint8_t* start = (uint8_t*)(&sensorConfig); uint8_t* end = (uint8_t*)(&sensorConfig + 1); /////////////////////////////////// if (sensorConfig.CRCFlags & CRC_HistoryBad) evrPush(EVR_ConfigBadSensorHistory,0); sensorConfig.CRCAtEnd[0] = crc32B((uint32_t*)(&sensorConfig), // CRC32B[sensorConfig] (uint32_t*)(&sensorConfig.CRCAtEnd)); /////////////////////////////////// setSPIdivisor(EEPROM_SPI, 2); // 18 MHz SPI clock sensorConfigAddr.value = SENSOR_EEPROM_ADDR; /////////////////////////////////// // Sector Erase writeEnable(); ENABLE_EEPROM; spiTransfer(EEPROM_SPI, SECTOR_ERASE_64KB); spiTransfer(EEPROM_SPI, sensorConfigAddr.bytes[2]); spiTransfer(EEPROM_SPI, sensorConfigAddr.bytes[1]); spiTransfer(EEPROM_SPI, sensorConfigAddr.bytes[0]); DISABLE_EEPROM; delayMicroseconds(2); eepromBusy(); /////////////////////////////////// // Program Page(s) for (pageIndex = 0; pageIndex < ((sizeof(sensorConfig) / 256) + 1); pageIndex++) { writeEnable(); ENABLE_EEPROM; spiTransfer(EEPROM_SPI, PAGE_PROGRAM_256_BYTES); spiTransfer(EEPROM_SPI, sensorConfigAddr.bytes[2]); spiTransfer(EEPROM_SPI, sensorConfigAddr.bytes[1]); spiTransfer(EEPROM_SPI, sensorConfigAddr.bytes[0]); for (byteCount = 0; byteCount < 256; byteCount++) { spiTransfer(EEPROM_SPI, *start++); if (start >= end) break; } DISABLE_EEPROM; delayMicroseconds(2); eepromBusy(); sensorConfigAddr.value += 0x0100; } readSensorEEPROM(); }
void writeSystemEEPROM(void) { uint16_t byteCount; uint8_t pageIndex; uint8_t* start = (uint8_t*)(&systemConfig); uint8_t* end = (uint8_t*)(&systemConfig + 1); /////////////////////////////////// // there's no reason to write these values to EEPROM, they'll just be noise zeroPIDstates(); if (systemConfig.CRCFlags & CRC_HistoryBad) evrPush(EVR_ConfigBadSystemHistory,0); systemConfig.CRCAtEnd[0] = crc32B((uint32_t*)(&systemConfig), // CRC32B[systemConfig] (uint32_t*)(&systemConfig.CRCAtEnd)); /////////////////////////////////// setSPIdivisor(EEPROM_SPI, 2); // 18 MHz SPI clock systemConfigaddr.value = SYSTEM_EEPROM_ADDR; /////////////////////////////////// // Sector Erase writeEnable(); ENABLE_EEPROM; spiTransfer(EEPROM_SPI, SECTOR_ERASE_64KB); spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[2]); spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[1]); spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[0]); DISABLE_EEPROM; delayMicroseconds(2); eepromBusy(); /////////////////////////////////// // Program Page(s) for (pageIndex = 0; pageIndex < ((sizeof(systemConfig) / 256) + 1); pageIndex++) { writeEnable(); ENABLE_EEPROM; spiTransfer(EEPROM_SPI, PAGE_PROGRAM_256_BYTES); spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[2]); spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[1]); spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[0]); for (byteCount = 0; byteCount < 256; byteCount++) { spiTransfer(EEPROM_SPI, *start++); if (start >= end) break; } DISABLE_EEPROM; delayMicroseconds(2); eepromBusy(); systemConfigaddr.value += 0x0100; } readSystemEEPROM(); }
int main(void) { /////////////////////////////////////////////////////////////////////////// #ifdef _DTIMING #define LA1_ENABLE GPIO_SetBits(GPIOA, GPIO_Pin_4) #define LA1_DISABLE GPIO_ResetBits(GPIOA, GPIO_Pin_4) #define LA4_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_5) #define LA4_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_5) #define LA2_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_2) #define LA2_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_2) #define LA3_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_3) #define LA3_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_3) GPIO_InitTypeDef GPIO_InitStructure; RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); GPIO_StructInit(&GPIO_InitStructure); // Init pins GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOA, &GPIO_InitStructure); // Init pins GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1; //GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; //GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOB, &GPIO_InitStructure); // Init pins GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_5; //GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; //GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOC, &GPIO_InitStructure); // PB0_DISABLE; LA4_DISABLE; LA2_DISABLE; LA3_DISABLE; LA1_DISABLE; #endif uint32_t currentTime; systemInit(); systemReady = true; evrPush(EVR_StartingMain, 0); while (1) { evrCheck(); /////////////////////////////// if (frame_50Hz) { #ifdef _DTIMING LA2_ENABLE; #endif frame_50Hz = false; currentTime = micros(); deltaTime50Hz = currentTime - previous50HzTime; previous50HzTime = currentTime; processFlightCommands(); if (newTemperatureReading && newPressureReading) { d1Value = d1.value; d2Value = d2.value; calculateTemperature(); calculatePressureAltitude(); newTemperatureReading = false; newPressureReading = false; } sensors.pressureAlt50Hz = firstOrderFilter(sensors.pressureAlt50Hz, &firstOrderFilters[PRESSURE_ALT_LOWPASS]); executionTime50Hz = micros() - currentTime; #ifdef _DTIMING LA2_DISABLE; #endif } /////////////////////////////// if (frame_10Hz) { #ifdef _DTIMING LA4_ENABLE; #endif frame_10Hz = false; currentTime = micros(); deltaTime10Hz = currentTime - previous10HzTime; previous10HzTime = currentTime; if (newMagData == true) { sensors.mag10Hz[XAXIS] = (float)rawMag[XAXIS].value * magScaleFactor[XAXIS] - eepromConfig.magBias[XAXIS]; sensors.mag10Hz[YAXIS] = (float)rawMag[YAXIS].value * magScaleFactor[YAXIS] - eepromConfig.magBias[YAXIS]; sensors.mag10Hz[ZAXIS] = -((float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS] - eepromConfig.magBias[ZAXIS]); newMagData = false; magDataUpdate = true; } cliCom(); rfCom(); batMonTick(); /////////////////////////// executionTime10Hz = micros() - currentTime; #ifdef _DTIMING LA4_DISABLE; #endif } /////////////////////////////// if (frame_500Hz) { #ifdef _DTIMING LA1_ENABLE; #endif frame_500Hz = false; currentTime = micros(); deltaTime500Hz = currentTime - previous500HzTime; previous500HzTime = currentTime; TIM_Cmd(TIM10, DISABLE); timerValue = TIM_GetCounter(TIM10); TIM_SetCounter(TIM10, 0); TIM_Cmd(TIM10, ENABLE); dt500Hz = (float)timerValue * 0.0000005f; // For integrations in 500 Hz loop computeMPU6000TCBias(); /* sensorTemp1 = computeMPU6000SensorTemp(); sensorTemp2 = sensorTemp1 * sensorTemp1; sensorTemp3 = sensorTemp2 * sensorTemp1; */ sensors.accel500Hz[XAXIS] = ((float)accelSummedSamples500Hz[XAXIS] / 2.0f - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR; sensors.accel500Hz[YAXIS] = -((float)accelSummedSamples500Hz[YAXIS] / 2.0f - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR; sensors.accel500Hz[ZAXIS] = -((float)accelSummedSamples500Hz[ZAXIS] / 2.0f - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR; sensors.accel500HzMXR[XAXIS] = -(accelSummedSamples500HzMXR[XAXIS] / 2.0f - eepromConfig.accelBiasMXR[XAXIS]) * eepromConfig.accelScaleFactorMXR[XAXIS]; sensors.accel500HzMXR[YAXIS] = -(accelSummedSamples500HzMXR[YAXIS] / 2.0f - eepromConfig.accelBiasMXR[YAXIS]) * eepromConfig.accelScaleFactorMXR[YAXIS]; sensors.accel500HzMXR[ZAXIS] = (accelSummedSamples500HzMXR[ZAXIS] / 2.0f - eepromConfig.accelBiasMXR[ZAXIS]) * eepromConfig.accelScaleFactorMXR[ZAXIS]; /* sensors.accel500Hz[XAXIS] = ((float)accelSummedSamples500Hz[XAXIS] / 2.0f + eepromConfig.accelBiasP0[XAXIS] + eepromConfig.accelBiasP1[XAXIS] * sensorTemp1 + eepromConfig.accelBiasP2[XAXIS] * sensorTemp2 + eepromConfig.accelBiasP3[XAXIS] * sensorTemp3 ) * ACCEL_SCALE_FACTOR; sensors.accel500Hz[YAXIS] = -((float)accelSummedSamples500Hz[YAXIS] / 2.0f + eepromConfig.accelBiasP0[YAXIS] + eepromConfig.accelBiasP1[YAXIS] * sensorTemp1 + eepromConfig.accelBiasP2[YAXIS] * sensorTemp2 + eepromConfig.accelBiasP3[YAXIS] * sensorTemp3 ) * ACCEL_SCALE_FACTOR; sensors.accel500Hz[ZAXIS] = -((float)accelSummedSamples500Hz[ZAXIS] / 2.0f + eepromConfig.accelBiasP0[ZAXIS] + eepromConfig.accelBiasP1[ZAXIS] * sensorTemp1 + eepromConfig.accelBiasP2[ZAXIS] * sensorTemp2 + eepromConfig.accelBiasP3[ZAXIS] * sensorTemp3 ) * ACCEL_SCALE_FACTOR; */ sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR; /* sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL ] / 2.0f + gyroBiasP0[ROLL ] + eepromConfig.gyroBiasP1[ROLL ] * sensorTemp1 + eepromConfig.gyroBiasP2[ROLL ] * sensorTemp2 + eepromConfig.gyroBiasP3[ROLL ] * sensorTemp3 ) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f + gyroBiasP0[PITCH] + eepromConfig.gyroBiasP1[PITCH] * sensorTemp1 + eepromConfig.gyroBiasP2[PITCH] * sensorTemp2 + eepromConfig.gyroBiasP3[PITCH] * sensorTemp3 ) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f + gyroBiasP0[YAW ] + eepromConfig.gyroBiasP1[YAW ] * sensorTemp1 + eepromConfig.gyroBiasP2[YAW ] * sensorTemp2 + eepromConfig.gyroBiasP3[YAW ] * sensorTemp3 ) * GYRO_SCALE_FACTOR; */ #if defined(MPU_ACCEL) MargAHRSupdate(sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW], sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], eepromConfig.accelCutoff, magDataUpdate, dt500Hz); #endif #if defined(MXR_ACCEL) sensors.accel500HzMXR[XAXIS] = firstOrderFilter(sensors.accel500HzMXR[XAXIS], &firstOrderFilters[ACCEL500HZ_X_LOWPASS]); sensors.accel500HzMXR[YAXIS] = firstOrderFilter(sensors.accel500HzMXR[YAXIS], &firstOrderFilters[ACCEL500HZ_Y_LOWPASS]); sensors.accel500HzMXR[ZAXIS] = firstOrderFilter(sensors.accel500HzMXR[ZAXIS], &firstOrderFilters[ACCEL500HZ_Z_LOWPASS]); MargAHRSupdate(sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW], sensors.accel500HzMXR[XAXIS], sensors.accel500HzMXR[YAXIS], sensors.accel500HzMXR[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], eepromConfig.accelCutoff, magDataUpdate, dt500Hz); #endif magDataUpdate = false; computeAxisCommands(dt500Hz); mixTable(); writeServos(); writeMotors(); executionTime500Hz = micros() - currentTime; #ifdef _DTIMING LA1_DISABLE; #endif } /////////////////////////////// if (frame_100Hz) { #ifdef _DTIMING LA3_ENABLE; #endif frame_100Hz = false; currentTime = micros(); deltaTime100Hz = currentTime - previous100HzTime; previous100HzTime = currentTime; TIM_Cmd(TIM11, DISABLE); timerValue = TIM_GetCounter(TIM11); TIM_SetCounter(TIM11, 0); TIM_Cmd(TIM11, ENABLE); dt100Hz = (float)timerValue * 0.0000005f; // For integrations in 100 Hz loop sensors.accel100Hz[XAXIS] = ((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR; sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR; sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR; sensors.accel100HzMXR[XAXIS] = -(accelSummedSamples100HzMXR[XAXIS] / 10.0f - eepromConfig.accelBiasMXR[XAXIS]) * eepromConfig.accelScaleFactorMXR[XAXIS]; sensors.accel100HzMXR[YAXIS] = -(accelSummedSamples100HzMXR[YAXIS] / 10.0f - eepromConfig.accelBiasMXR[YAXIS]) * eepromConfig.accelScaleFactorMXR[YAXIS]; sensors.accel100HzMXR[ZAXIS] = (accelSummedSamples100HzMXR[ZAXIS] / 10.0f - eepromConfig.accelBiasMXR[ZAXIS]) * eepromConfig.accelScaleFactorMXR[ZAXIS]; sensors.accel100HzMXR[XAXIS] = firstOrderFilter(sensors.accel100HzMXR[XAXIS], &firstOrderFilters[ACCEL100HZ_X_LOWPASS]); sensors.accel100HzMXR[YAXIS] = firstOrderFilter(sensors.accel100HzMXR[YAXIS], &firstOrderFilters[ACCEL100HZ_Y_LOWPASS]); sensors.accel100HzMXR[ZAXIS] = firstOrderFilter(sensors.accel100HzMXR[ZAXIS], &firstOrderFilters[ACCEL100HZ_Z_LOWPASS]); createRotationMatrix(); bodyAccelToEarthAccel(); vertCompFilter(dt100Hz); if (armed == true) { if ( eepromConfig.activeTelemetry == 1 ) { // 500 Hz Accels openLogPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS], sensors.accel500HzMXR[XAXIS], sensors.accel500HzMXR[YAXIS], sensors.accel500HzMXR[ZAXIS]); } if ( eepromConfig.activeTelemetry == 2 ) { // 500 Hz Gyros openLogPrintF("%9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW ]); } if ( eepromConfig.activeTelemetry == 4 ) { // 500 Hz Attitudes openLogPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ], sensors.attitude500Hz[PITCH], sensors.attitude500Hz[YAW ]); } if ( eepromConfig.activeTelemetry == 8 ) { // Vertical Variables openLogPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %4ld\n", earthAxisAccels[ZAXIS], sensors.pressureAlt50Hz, hDotEstimate, hEstimate, ms5611Temperature); } if ( eepromConfig.activeTelemetry == 16) { // Vertical Variables openLogPrintF("%9.4f, %9.4f, %9.4f, %4ld, %1d, %9.4f, %9.4f\n", verticalVelocityCmd, hDotEstimate, hEstimate, ms5611Temperature, verticalModeState, throttleCmd, eepromConfig.PID[HDOT_PID].iTerm); } } executionTime100Hz = micros() - currentTime; #ifdef _DTIMING LA3_DISABLE; #endif } /////////////////////////////// if (frame_5Hz) { frame_5Hz = false; currentTime = micros(); deltaTime5Hz = currentTime - previous5HzTime; previous5HzTime = currentTime; if (execUp == true) BLUE_LED_TOGGLE; while (batMonVeryLowWarning > 0) { //BEEP_TOGGLE; batMonVeryLowWarning--; } executionTime5Hz = micros() - currentTime; } /////////////////////////////// if (frame_1Hz) { frame_1Hz = false; currentTime = micros(); deltaTime1Hz = currentTime - previous1HzTime; previous1HzTime = currentTime; if (execUp == true) GREEN_LED_TOGGLE; if (execUp == false) execUpCount++; if ((execUpCount == 5) && (execUp == false)) { execUp = true; pwmEscInit(eepromConfig.escPwmRate); } while (batMonLowWarning > 0) { //BEEP_TOGGLE; batMonLowWarning--; } executionTime1Hz = micros() - currentTime; } //////////////////////////////// } /////////////////////////////////////////////////////////////////////////// }
int main(void) { /////////////////////////////////////////////////////////////////////////// uint32_t currentTime; #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; 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; } //////////////////////////////// } /////////////////////////////////////////////////////////////////////////// }
uint8_t writeEEPROM(void) { FLASH_Status status; int32_t i; /////////////////////////////////// if (eepromConfig.receiverType == SPEKTRUM) { USART_Cmd(USART6, DISABLE); TIM_Cmd(TIM6, DISABLE); if (eepromConfig.slaveSpektrum == true) USART_Cmd(UART4, DISABLE); } /////////////////////////////////// eepromConfig_t *src = &eepromConfig; uint32_t *dst = (uint32_t*)FLASH_WRITE_EEPROM_ADDR; // there's no reason to write these values to EEPROM, they'll just be noise zeroPIDintegralError(); zeroPIDstates(); if ( src->CRCFlags & CRC_HistoryBad ) evrPush(EVR_ConfigBadHistory,0); src->CRCAtEnd[0] = crc32B( (uint32_t*)&src[0], src->CRCAtEnd); FLASH_Unlock(); FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); status = FLASH_EraseSector(FLASH_Sector_1, VoltageRange_3); /////////////////////////////////// i = -1; while ( FLASH_COMPLETE == status && i++ < eepromConfigNUMWORD ) status = FLASH_ProgramWord((uint32_t)&dst[i], ((uint32_t*)src)[i]); if ( FLASH_COMPLETE != status ) evrPush( -1 == i ? EVR_FlashEraseFail : EVR_FlashProgramFail, status); /////////////////////////////////// FLASH_Lock(); readEEPROM(); if (eepromConfig.receiverType == SPEKTRUM) { primarySpektrumState.reSync = 1; TIM_Cmd(TIM6, ENABLE); USART_Cmd(USART6, ENABLE); if (eepromConfig.slaveSpektrum == true) { slaveSpektrumState.reSync = 1; USART_Cmd(UART4, ENABLE); } } /////////////////////////////////// return status; }
uint8_t writeEEPROM(void) { FLASH_Status status; int32_t i; /////////////////////////////////// if (eepromConfig.receiverType == SPEKTRUM) { USART_Cmd(USART2, DISABLE); TIM_Cmd(TIM2, DISABLE); } /////////////////////////////////// eepromConfig_t *src = &eepromConfig; uint32_t *dst = (uint32_t*)FLASH_WRITE_EEPROM_ADDR; // there's no reason to write these values to EEPROM, they'll just be noise zeroPIDstates(); if (src->CRCFlags & CRC_HistoryBad) evrPush(EVR_ConfigBadHistory,0); src->CRCAtEnd[0] = crc32B( (uint32_t*)&src[0], src->CRCAtEnd); FLASH_Unlock(); FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); status = FLASH_ErasePage(FLASH_WRITE_EEPROM_ADDR); /////////////////////////////////// i = -1; while (status == FLASH_COMPLETE && i++ < eepromConfigNUMWORD) status = FLASH_ProgramWord((uint32_t)&dst[i], ((uint32_t*)src)[i]); if (status != FLASH_COMPLETE) evrPush( i == -1 ? EVR_FlashEraseFail : EVR_FlashProgramFail, status); /////////////////////////////////// FLASH_Lock(); readEEPROM(); /////////////////////////////////// if (eepromConfig.receiverType == SPEKTRUM) { primarySpektrumState.reSync = 1; TIM_Cmd(TIM2, ENABLE); USART_Cmd(USART2, ENABLE); } /////////////////////////////////// return status; }