void systemInit(void)
{
// Init cycle counter
cycleCounterInit();
// SysTick
SysTick_Config(SystemCoreClock / 1000);
///////////////////////////////////
checkFirstTime(false);
readEEPROM();
if (eepromConfig.receiverType == SPEKTRUM)
checkSpektrumBind();
checkResetType();
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority
initMixer();
ledInit();
cliInit();
BLUE_LED_ON;
delay(20000); // 20 sec total delay for sensor stabilization - probably not long enough.....
adcInit();
batteryInit();
gpsInit();
i2cInit(I2C1);
i2cInit(I2C2);
pwmEscInit(eepromConfig.escPwmRate);
pwmServoInit(eepromConfig.servoPwmRate);
rxInit();
spiInit(SPI2);
spiInit(SPI3);
telemetryInit();
timingFunctionsInit();
initFirstOrderFilter();
initGPS();
initMax7456();
initPID();
GREEN_LED_ON;
initMPU6000();
initMag(HMC5883L_I2C);
initPressure(MS5611_I2C);
}
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;
#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;
}
////////////////////////////////
}
///////////////////////////////////////////////////////////////////////////
}
void mixerCLI()
{
float tempFloat;
uint8_t index;
uint8_t rows, columns;
uint8_t mixerQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering Mixer CLI....\n\n");
while(true)
{
cliPortPrint("Mixer CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
mixerQuery = cliPortRead();
cliPortPrint("\n");
switch(mixerQuery)
{
///////////////////////////
case 'a': // Mixer Configuration
cliPortPrint("\nMixer Configuration: ");
switch (eepromConfig.mixerConfiguration)
{
case MIXERTYPE_TRI:
cliPortPrint(" MIXERTYPE TRI\n");
break;
case MIXERTYPE_QUADX:
cliPortPrint("MIXERTYPE QUAD X\n");
break;
case MIXERTYPE_HEX6X:
cliPortPrint(" MIXERTYPE HEX X\n");
break;
case MIXERTYPE_FREE:
cliPortPrint(" MIXERTYPE FREE\n");
break;
}
cliPortPrintF("Number of Motors: %1d\n", numberMotor);
cliPortPrintF("ESC PWM Rate: %3ld\n", eepromConfig.escPwmRate);
cliPortPrintF("Servo PWM Rate: %3ld\n", eepromConfig.servoPwmRate);
if (eepromConfig.yawDirection == 1.0f)
cliPortPrintF("Yaw Direction: Normal\n\n");
else if (eepromConfig.yawDirection == -1.0f)
cliPortPrintF("Yaw Direction: Reverse\n\n");
else
cliPortPrintF("Yaw Direction: Undefined\n\n");
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
cliPortPrintF("TriCopter Yaw Servo PWM Rate: %3ld\n", eepromConfig.triYawServoPwmRate);
cliPortPrintF("TriCopter Yaw Servo Min PWM: %4ld\n", (uint16_t)eepromConfig.triYawServoMin);
cliPortPrintF("TriCopter Yaw Servo Mid PWM: %4ld\n", (uint16_t)eepromConfig.triYawServoMid);
cliPortPrintF("TriCopter Yaw Servo Max PWM: %4ld\n\n", (uint16_t)eepromConfig.triYawServoMax);
cliPortPrintF("Tricopter Yaw Cmd Time Constant: %5.3f\n\n", eepromConfig.triCopterYawCmd500HzLowPassTau);
}
if (eepromConfig.mixerConfiguration == MIXERTYPE_FREE)
{
cliPortPrintF("\nNumber of Free Mixer Motors: %1d\n Roll Pitch Yaw\n\n", eepromConfig.freeMixMotors);
for ( index = 0; index < eepromConfig.freeMixMotors; index++ )
{
cliPortPrintF("Motor%1d %6.3f %6.3f %6.3f\n", index,
eepromConfig.freeMix[index][ROLL ],
eepromConfig.freeMix[index][PITCH],
eepromConfig.freeMix[index][YAW ]);
}
cliPortPrint("\n");
}
validQuery = false;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Mixer CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Read Mixer Configuration
eepromConfig.mixerConfiguration = (uint8_t)readFloatCLI();
initMixer();
pwmEscInit();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Read ESC and Servo PWM Update Rates
eepromConfig.escPwmRate = (uint16_t)readFloatCLI();
eepromConfig.servoPwmRate = (uint16_t)readFloatCLI();
pwmEscInit();
pwmServoInit();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // Read yaw direction
tempFloat = readFloatCLI();
if (tempFloat >= 0.0)
tempFloat = 1.0;
else
tempFloat = -1.0;
eepromConfig.yawDirection = tempFloat;
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // Read TriCopter Servo PWM Rate
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
eepromConfig.triYawServoPwmRate = (uint16_t)readFloatCLI();
pwmEscInit();
}
else
{
tempFloat = readFloatCLI();
cliPortPrintF("\nTriCopter Mixing not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'F': // Read TriCopter Servo Min Point
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
eepromConfig.triYawServoMin = readFloatCLI();
pwmEscWrite(7, (uint16_t)eepromConfig.triYawServoMin);
}
else
{
tempFloat = readFloatCLI();
cliPortPrintF("\nTriCopter Mixing not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'G': // Read TriCopter Servo Mid Point
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
eepromConfig.triYawServoMid = readFloatCLI();
pwmEscWrite(7, (uint16_t)eepromConfig.triYawServoMid);
}
else
{
tempFloat = readFloatCLI();
cliPortPrintF("\nTriCopter Mixing not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'H': // Read TriCopter Servo Max Point
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
eepromConfig.triYawServoMax = readFloatCLI();
pwmEscWrite(7, (uint16_t)eepromConfig.triYawServoMax);
}
else
{
tempFloat = readFloatCLI();
cliPortPrintF("\nTriCopter Mixing not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'I': // Read TriCopter Yaw Command Time Constant
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
eepromConfig.triCopterYawCmd500HzLowPassTau = readFloatCLI();
initFirstOrderFilter();
}
else
{
tempFloat = readFloatCLI();
cliPortPrintF("\nTriCopter Mixing not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'J': // Read Free Mix Motor Number
if (eepromConfig.mixerConfiguration == MIXERTYPE_FREE)
{
eepromConfig.freeMixMotors = (uint8_t)readFloatCLI();
initMixer();
}
else
{
tempFloat = readFloatCLI();
cliPortPrintF("\nFree Mix not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'K': // Read Free Mix Matrix Element
if (eepromConfig.mixerConfiguration == MIXERTYPE_FREE)
{
rows = (uint8_t)readFloatCLI() - 1;
columns = (uint8_t)readFloatCLI() - 1;
eepromConfig.freeMix[rows][columns] = readFloatCLI();
}
else
{
tempFloat = readFloatCLI();
tempFloat = readFloatCLI();
tempFloat = readFloatCLI();
cliPortPrintF("\nFree Mix not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPortPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPortPrint("\n");
cliPortPrint("'a' Mixer Configuration Data 'A' Set Mixer Configuration A0 thru 3, see aq32Plus.h\n");
cliPortPrint(" 'B' Set PWM Rates BESC;Servo\n");
cliPortPrint(" 'D' Set Yaw Direction D1 or D-1\n");
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
cliPortPrint(" 'E' Set TriCopter Servo PWM Rate ERate\n");
cliPortPrint(" 'F' Set TriCopter Servo Min Point FMin\n");
cliPortPrint(" 'G' Set TriCopter Servo Mid Point GMid\n");
cliPortPrint(" 'H' Set TriCopter Servo Max Point HMax\n");
cliPortPrint(" 'I' Set TriCopter Yaw Cmd Time Constant ITimeConstant\n");
}
if (eepromConfig.mixerConfiguration == MIXERTYPE_FREE)
{
cliPortPrint(" 'J' Set Number of FreeMix Motors JNumb\n");
cliPortPrint(" 'K' Set FreeMix Matrix Element KRow;Col;Value\n");
}
cliPortPrint(" 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Exit Mixer CLI '?' Command Summary\n\n");
break;
///////////////////////////
}
}
}
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)
{
///////////////////////////////////////////////////////////////////////////
#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 mixerCLI()
{
float tempFloat;
uint8_t index;
uint8_t rows, columns;
uint8_t mixerQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering Mixer CLI....\n\n");
while(true)
{
cliPrint("Mixer CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
mixerQuery = cliRead();
cliPrint("\n");
switch(mixerQuery)
{
///////////////////////////
case 'a': // Mixer Configuration
cliPrint("\nMixer Configuration: ");
switch (eepromConfig.mixerConfiguration)
{
case MIXERTYPE_GIMBAL:
cliPrint("MIXERTYPE GIMBAL\n");
break;
///////////////////////
case MIXERTYPE_FLYING_WING:
cliPrint("MIXERTYPE FLYING WING\n");
break;
///////////////////////
case MIXERTYPE_BI:
cliPrint("MIXERTYPE BICOPTER\n");
break;
///////////////////////
case MIXERTYPE_TRI:
cliPrint("MIXERTYPE TRICOPTER\n");
break;
///////////////////////
case MIXERTYPE_QUADP:
cliPrint("MIXERTYPE QUAD PLUS\n");
break;
case MIXERTYPE_QUADX:
cliPrint("MIXERTYPE QUAD X\n");
break;
case MIXERTYPE_VTAIL4_NO_COMP:
cliPrint("MULTITYPE VTAIL NO COMP\n");
break;
case MIXERTYPE_VTAIL4_Y_COMP:
cliPrint("MULTITYPE VTAIL Y COMP\n");
break;
case MIXERTYPE_VTAIL4_RY_COMP:
cliPrint("MULTITYPE VTAIL RY COMP\n");
break;
case MIXERTYPE_VTAIL4_PY_COMP:
cliPrint("MULTITYPE VTAIL PY COMP\n");
break;
case MIXERTYPE_VTAIL4_RP_COMP:
cliPrint("MULTITYPE VTAIL RP COMP\n");
break;
case MIXERTYPE_VTAIL4_RPY_COMP:
cliPrint("MULTITYPE VTAIL RPY COMP\n");
break;
case MIXERTYPE_Y4:
cliPrint("MIXERTYPE Y4\n");
break;
///////////////////////
case MIXERTYPE_HEX6P:
cliPrint("MIXERTYPE HEX PLUS\n");
break;
case MIXERTYPE_HEX6X:
cliPrint("MIXERTYPE HEX X\n");
break;
case MIXERTYPE_Y6:
cliPrint("MIXERTYPE Y6\n");
break;
///////////////////////
case MIXERTYPE_OCTOF8P:
cliPrint("MIXERTYPE FLAT OCTO PLUS\n");
break;
case MIXERTYPE_OCTOF8X:
cliPrint("MIXERTYPE FLAT OCTO X\n");
break;
case MIXERTYPE_OCTOX8P:
cliPrint("MIXERTYPE COAXIAL OCTO PLUS\n");
break;
case MIXERTYPE_OCTOX8X:
cliPrint("MIXERTYPE COAXIAL OCTO X\n");
break;
///////////////////////
case MIXERTYPE_FREEMIX:
cliPrint("MIXERTYPE FREE MIX\n");
break;
}
cliPrintF("Number of Motors: %1d\n", numberMotor);
cliPrintF("ESC PWM Rate: %3ld\n", eepromConfig.escPwmRate);
cliPrintF("Servo PWM Rate: %3ld\n", eepromConfig.servoPwmRate);
if ( eepromConfig.mixerConfiguration == MIXERTYPE_BI )
{
cliPrintF("BiCopter Left Servo Min: %4ld\n", (uint16_t)eepromConfig.biLeftServoMin);
cliPrintF("BiCopter Left Servo Mid: %4ld\n", (uint16_t)eepromConfig.biLeftServoMid);
cliPrintF("BiCopter Left Servo Max: %4ld\n", (uint16_t)eepromConfig.biLeftServoMax);
cliPrintF("BiCopter Right Servo Min: %4ld\n", (uint16_t)eepromConfig.biRightServoMin);
cliPrintF("BiCopter Right Servo Mid: %4ld\n", (uint16_t)eepromConfig.biRightServoMid);
cliPrintF("BiCopter Right Servo Max: %4ld\n", (uint16_t)eepromConfig.biRightServoMax);
}
if ( eepromConfig.mixerConfiguration == MIXERTYPE_FLYING_WING )
{
cliPrintF("Roll Direction Left: %4ld\n", (uint16_t)eepromConfig.rollDirectionLeft);
cliPrintF("Roll Direction Right: %4ld\n", (uint16_t)eepromConfig.rollDirectionRight);
cliPrintF("Pitch Direction Left: %4ld\n", (uint16_t)eepromConfig.pitchDirectionLeft);
cliPrintF("Pitch Direction Right: %4ld\n", (uint16_t)eepromConfig.pitchDirectionRight);
cliPrintF("Wing Left Minimum: %4ld\n", (uint16_t)eepromConfig.wingLeftMinimum);
cliPrintF("Wing Left Maximum: %4ld\n", (uint16_t)eepromConfig.wingLeftMaximum);
cliPrintF("Wing Right Minimum: %4ld\n", (uint16_t)eepromConfig.wingRightMinimum);
cliPrintF("Wing Right Maximum: %4ld\n", (uint16_t)eepromConfig.wingRightMaximum);
}
if ( eepromConfig.mixerConfiguration == MIXERTYPE_GIMBAL )
{
cliPrintF("Gimbal Roll Servo Min: %4ld\n", (uint16_t)eepromConfig.gimbalRollServoMin);
cliPrintF("Gimbal Roll Servo Mid: %4ld\n", (uint16_t)eepromConfig.gimbalRollServoMid);
cliPrintF("Gimbal Roll Servo Max: %4ld\n", (uint16_t)eepromConfig.gimbalRollServoMax);
cliPrintF("Gimbal Roll Servo Gain: %7.3f\n", eepromConfig.gimbalRollServoGain);
cliPrintF("Gimbal Pitch Servo Min: %4ld\n", (uint16_t)eepromConfig.gimbalPitchServoMin);
cliPrintF("Gimbal Pitch Servo Mid: %4ld\n", (uint16_t)eepromConfig.gimbalPitchServoMid);
cliPrintF("Gimbal Pitch Servo Max: %4ld\n", (uint16_t)eepromConfig.gimbalPitchServoMax);
cliPrintF("Gimbal Pitch Servo Gain: %7.3f\n", eepromConfig.gimbalPitchServoGain);
}
if ( eepromConfig.mixerConfiguration == MIXERTYPE_TRI )
{
cliPrintF("TriCopter Yaw Servo Min: %4ld\n", (uint16_t)eepromConfig.triYawServoMin);
cliPrintF("TriCopter Yaw Servo Mid: %4ld\n", (uint16_t)eepromConfig.triYawServoMid);
cliPrintF("TriCopter Yaw Servo Max: %4ld\n", (uint16_t)eepromConfig.triYawServoMax);
}
if (eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_Y_COMP ||
eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_RY_COMP ||
eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_PY_COMP ||
eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_RP_COMP ||
eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_RPY_COMP)
{
cliPrintF("V Tail Angle %6.2f\n", eepromConfig.vTailAngle);
}
cliPrintF("Yaw Direction: %2d\n\n", (uint16_t)eepromConfig.yawDirection);
validQuery = false;
break;
///////////////////////////
case 'b': // Free Mix Matrix
cliPrintF("\nNumber of Free Mixer Motors: %1d\n Roll Pitch Yaw\n", eepromConfig.freeMixMotors);
for ( index = 0; index < eepromConfig.freeMixMotors; index++ )
{
cliPrintF("Motor%1d %6.3f %6.3f %6.3f\n", index,
eepromConfig.freeMix[index][ROLL ],
eepromConfig.freeMix[index][PITCH],
eepromConfig.freeMix[index][YAW ]);
}
cliPrint("\n");
validQuery = false;
break;
///////////////////////////
case 'x':
cliPrint("\nExiting Mixer CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Read Mixer Configuration
eepromConfig.mixerConfiguration = (uint8_t)readFloatCLI();
initMixer();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Read ESC and Servo PWM Update Rates
eepromConfig.escPwmRate = (uint16_t)readFloatCLI();
eepromConfig.servoPwmRate = (uint16_t)readFloatCLI();
pwmEscInit(eepromConfig.escPwmRate);
pwmServoInit(eepromConfig.servoPwmRate);
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // Read BiCopter Left Servo Parameters
eepromConfig.biLeftServoMin = readFloatCLI();
eepromConfig.biLeftServoMid = readFloatCLI();
eepromConfig.biLeftServoMax = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // Read BiCopter Right Servo Parameters
eepromConfig.biRightServoMin = readFloatCLI();
eepromConfig.biRightServoMid = readFloatCLI();
eepromConfig.biRightServoMax = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // Read Flying Wing Servo Directions
eepromConfig.rollDirectionLeft = readFloatCLI();
eepromConfig.rollDirectionRight = readFloatCLI();
eepromConfig.pitchDirectionLeft = readFloatCLI();
eepromConfig.pitchDirectionRight = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'F': // Read Flying Wing Servo Limits
eepromConfig.wingLeftMinimum = readFloatCLI();
eepromConfig.wingLeftMaximum = readFloatCLI();
eepromConfig.wingRightMinimum = readFloatCLI();
eepromConfig.wingRightMaximum = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'G': // Read Free Mix Motor Number
eepromConfig.freeMixMotors = (uint8_t)readFloatCLI();
initMixer();
mixerQuery = 'b';
validQuery = true;
break;
///////////////////////////
case 'H': // Read Free Mix Matrix Element
rows = (uint8_t)readFloatCLI();
columns = (uint8_t)readFloatCLI();
eepromConfig.freeMix[rows][columns] = readFloatCLI();
mixerQuery = 'b';
validQuery = true;
break;
///////////////////////////
case 'I': // Read Gimbal Roll Servo Parameters
eepromConfig.gimbalRollServoMin = readFloatCLI();
eepromConfig.gimbalRollServoMid = readFloatCLI();
eepromConfig.gimbalRollServoMax = readFloatCLI();
eepromConfig.gimbalRollServoGain = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'J': // Read Gimbal Pitch Servo Parameters
eepromConfig.gimbalPitchServoMin = readFloatCLI();
eepromConfig.gimbalPitchServoMid = readFloatCLI();
eepromConfig.gimbalPitchServoMax = readFloatCLI();
eepromConfig.gimbalPitchServoGain = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'K': // Read TriCopter YawServo Parameters
eepromConfig.triYawServoMin = readFloatCLI();
eepromConfig.triYawServoMid = readFloatCLI();
eepromConfig.triYawServoMax = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'L': // Read V Tail Angle
eepromConfig.vTailAngle = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'M': // Read yaw direction
tempFloat = readFloatCLI();
if (tempFloat >= 0.0)
tempFloat = 1.0;
else
tempFloat = -1.0;
eepromConfig.yawDirection = tempFloat;
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
break;
///////////////////////////
case '?':
cliPrint("\n");
cliPrint("'a' Mixer Configuration Data 'A' Set Mixer Configuration A1 thru 21, see aq32Plus.h\n");
cliPrint("'b' Free Mixer Configuration 'B' Set PWM Rates BESC;Servo\n");
cliPrint(" 'C' Set BiCopter Left Servo Parameters CMin;Mid;Max\n");
cliPrint(" 'D' Set BiCopter Right Servo Parameters DMin;Mid;Max\n");
cliPrint(" 'E' Set Flying Wing Servo Directions ERollLeft;RollRight;PitchLeft;PitchRight\n");
cliPrint(" 'F' Set Flying Wing Servo Limits FLeftMin;LeftMax;RightMin;RightMax\n");
cliPrint(" 'G' Set Number of FreeMix Motors GNumber\n");
cliPrint(" 'H' Set FreeMix Matrix Element HRow;Column;Element\n");
cliPrint(" 'I' Set Gimbal Roll Servo Parameters IMin;Mid;Max;Gain\n");
cliPrint(" 'J' Set Gimbal Pitch Servo Parameters JMin;Mid;Max;Gain\n");
cliPrint(" 'K' Set TriCopter Servo Parameters KMin;Mid;Max\n");
cliPrint(" 'L' Set V Tail Angle LAngle\n");
cliPrint(" 'M' Set Yaw Direction M1 or M-1\n");
cliPrint(" 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit Sensor CLI '?' Command Summary\n");
cliPrint("\n");
break;
///////////////////////////
}
}
}
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;
}
////////////////////////////////
}
///////////////////////////////////////////////////////////////////////////
}
void mixerCLI()
{
float tempFloat;
uint8_t mixerQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering Mixer CLI....\n\n");
while(true)
{
cliPrint("Mixer CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
mixerQuery = cliRead();
cliPrint("\n");
switch(mixerQuery)
{
///////////////////////////
case 'a': // Mixer Configuration
cliPrint("\nMixer Configuration: ");
switch (eepromConfig.mixerConfiguration)
{
case MIXERTYPE_QUADX:
cliPrint("MIXERTYPE QUAD X\n");
break;
case MIXERTYPE_HEX6X:
cliPrint(" MIXERTYPE HEX X\n");
break;
}
cliPrintF("Number of Motors: %1d\n", numberMotor);
cliPrintF("ESC PWM Rate: %3ld\n", eepromConfig.escPwmRate);
cliPrintF("Servo PWM Rate: %3ld\n\n", eepromConfig.servoPwmRate);
validQuery = false;
break;
///////////////////////////
case 'x':
cliPrint("\nExiting Mixer CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Read Mixer Configuration
eepromConfig.mixerConfiguration = (uint8_t)readFloatCLI();
initMixer();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Read ESC and Servo PWM Update Rates
eepromConfig.escPwmRate = (uint16_t)readFloatCLI();
eepromConfig.servoPwmRate = (uint16_t)readFloatCLI();
pwmEscInit(eepromConfig.escPwmRate);
pwmServoInit(eepromConfig.servoPwmRate);
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'M': // Read yaw direction
tempFloat = readFloatCLI();
if (tempFloat >= 0.0)
tempFloat = 1.0;
else
tempFloat = -1.0;
eepromConfig.yawDirection = tempFloat;
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
break;
///////////////////////////
case '?':
cliPrint("\n");
cliPrint("'a' Mixer Configuration Data 'A' Set Mixer Configuration A1 thru 21, see aq32Plus.h\n");
cliPrint("'b' Free Mixer Configuration 'B' Set PWM Rates BESC;Servo\n");
cliPrint(" 'C' Set BiCopter Left Servo Parameters CMin;Mid;Max\n");
cliPrint(" 'D' Set BiCopter Right Servo Parameters DMin;Mid;Max\n");
cliPrint(" 'E' Set Flying Wing Servo Directions ERollLeft;RollRight;PitchLeft;PitchRight\n");
cliPrint(" 'F' Set Flying Wing Servo Limits FLeftMin;LeftMax;RightMin;RightMax\n");
cliPrint(" 'G' Set Number of FreeMix Motors GNumber\n");
cliPrint(" 'H' Set FreeMix Matrix Element HRow;Column;Element\n");
cliPrint(" 'I' Set Gimbal Roll Servo Parameters IMin;Mid;Max;Gain\n");
cliPrint(" 'J' Set Gimbal Pitch Servo Parameters JMin;Mid;Max;Gain\n");
cliPrint(" 'K' Set TriCopter Servo Parameters KMin;Mid;Max\n");
cliPrint(" 'L' Set V Tail Angle LAngle\n");
cliPrint(" 'M' Set Yaw Direction M1 or M-1\n");
cliPrint(" 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit Sensor CLI '?' Command Summary\n");
cliPrint("\n");
break;
///////////////////////////
}
}
}
