void downloadMax7456Font(void)
{
uint16_t ch;
if (sizeof(fontdata)!=16384)
{
cliPrint("\nERROR: fontdata with invalid size, aborting!!!\n\n");
return;
}
cliPrint("\nDownloading font to MAX7456 NVM, this may take a while...\n\n");
for (ch = 0; ch < 256; ch++)
{
cliPrintF("%d3", ch);
writeNVMcharacter(ch, 64*ch);
delay(30);
cliPrint(" Done\n");
}
// force soft reset on Max7456
resetMax7456();
showMax7456Font();
cliPrint("\nDone with MAX7456 font download\n\n");
}
void magCalibration(void)
{
uint16_t calibrationCounter = 0;
uint16_t population[2][3];
float d[600][3]; // 600 Samples = 60 seconds of data at 10 Hz
float sphereOrigin[3];
float sphereRadius;
magCalibrating = true;
cliPrintF("\nMagnetometer Calibration:\n\n");
cliPrintF("Rotate magnetometer around all axes multiple times\n");
cliPrintF("Must complete within 60 seconds....\n\n");
cliPrintF(" Send a character when ready to begin and another when complete\n\n");
while (cliAvailable() == false);
cliPrintF(" Start rotations.....\n");
getChar();
while ((cliAvailable() == false) && (calibrationCounter < 600))
{
if (readMag() == true)
{
d[calibrationCounter][XAXIS] = (float)rawMag[XAXIS].value * magScaleFactor[XAXIS];
d[calibrationCounter][YAXIS] = (float)rawMag[YAXIS].value * magScaleFactor[YAXIS];
d[calibrationCounter][ZAXIS] = (float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS];
calibrationCounter++;
}
delay(100);
}
cliPrintF("\n\nMagnetometer Bias Calculation, %3ld samples collected out of 600 max)\n", calibrationCounter);
sphereFit(d, calibrationCounter, 100, 0.0f, population, sphereOrigin, &sphereRadius);
eepromConfig.magBias[XAXIS] = sphereOrigin[XAXIS];
eepromConfig.magBias[YAXIS] = sphereOrigin[YAXIS];
eepromConfig.magBias[ZAXIS] = sphereOrigin[ZAXIS];
magCalibrating = false;
}
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++)
cliPrintF("%02X", by[i * line_length + j]);
cliPrint("\n");
}
e->CRCAtEnd[0] = old_crc;
}
void sensorCLI()
{
uint8_t sensorQuery = 'x';
uint8_t tempInt;
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering Sensor CLI....\n\n");
while(true)
{
cliPrint("Sensor CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
sensorQuery = cliRead();
cliPrint("\n");
switch(sensorQuery)
{
///////////////////////////
case 'a': // Sensor Data
cliPrintF("\n");
cliPrintF("External HMC5883 in use: %s\n", eepromConfig.externalHMC5883 ? "Yes" : "No");
cliPrintF("External MS5611 in use: %s\n", eepromConfig.externalMS5611 ? "Yes" : "No");
cliPrintF("\n");
cliPrintF("Accel Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasSlope[XAXIS],
eepromConfig.accelTCBiasSlope[YAXIS],
eepromConfig.accelTCBiasSlope[ZAXIS]);
cliPrintF("Accel Temp Comp Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasIntercept[XAXIS],
eepromConfig.accelTCBiasIntercept[YAXIS],
eepromConfig.accelTCBiasIntercept[ZAXIS]);
cliPrintF("Gyro Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasSlope[ROLL ],
eepromConfig.gyroTCBiasSlope[PITCH],
eepromConfig.gyroTCBiasSlope[YAW ]);
cliPrintF("Gyro Temp Comp Intercept: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasIntercept[ROLL ],
eepromConfig.gyroTCBiasIntercept[PITCH],
eepromConfig.gyroTCBiasIntercept[YAW ]);
cliPrintF("Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS],
eepromConfig.magBias[YAXIS],
eepromConfig.magBias[ZAXIS]);
cliPrintF("Accel One G: %9.4f\n", accelOneG);
cliPrintF("Accel Cutoff: %9.4f\n", eepromConfig.accelCutoff);
cliPrintF("KpAcc (MARG): %9.4f\n", eepromConfig.KpAcc);
cliPrintF("KiAcc (MARG): %9.4f\n", eepromConfig.KiAcc);
cliPrintF("KpMag (MARG): %9.4f\n", eepromConfig.KpMag);
cliPrintF("KiMag (MARG): %9.4f\n", eepromConfig.KiMag);
cliPrintF("hdot est/h est Comp Fil A: %9.4f\n", eepromConfig.compFilterA);
cliPrintF("hdot est/h est Comp Fil B: %9.4f\n", eepromConfig.compFilterB);
cliPrint("MPU6000 DLPF: ");
switch(eepromConfig.dlpfSetting)
{
case DLPF_256HZ:
cliPrint("256 Hz\n");
break;
case DLPF_188HZ:
cliPrint("188 Hz\n");
break;
case DLPF_98HZ:
cliPrint("98 Hz\n");
break;
case DLPF_42HZ:
cliPrint("42 Hz\n");
break;
}
cliPrint("Magnetic Variation: ");
if (eepromConfig.magVar >= 0.0f)
cliPrintF("E%6.4f\n\n", eepromConfig.magVar * R2D);
else
cliPrintF("W%6.4f\n\n", -eepromConfig.magVar * R2D);
if (eepromConfig.verticalVelocityHoldOnly)
cliPrint("Vertical Velocity Hold Only\n\n");
else
cliPrint("Vertical Velocity and Altitude Hold\n\n");
validQuery = false;
break;
///////////////////////////
case 'b': // MPU6000 Calibration
mpu6000Calibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Magnetometer Calibration
magCalibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'e': // Toggle External HMC5883 Use
if (eepromConfig.externalHMC5883)
eepromConfig.externalHMC5883 = false;
else
eepromConfig.externalHMC5883 = true;
initMag();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'f': // Toggle External MS5611 Use
if (eepromConfig.externalMS5611)
eepromConfig.externalMS5611 = false;
else
eepromConfig.externalMS5611 = true;
initPressure();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'v': // Toggle Vertical Velocity Hold Only
if (eepromConfig.verticalVelocityHoldOnly)
eepromConfig.verticalVelocityHoldOnly = false;
else
eepromConfig.verticalVelocityHoldOnly = true;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPrint("\nExiting Sensor CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Set MPU6000 Digital Low Pass Filter
tempInt = (uint8_t)readFloatCLI();
switch(tempInt)
{
case DLPF_256HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_256HZ;
break;
case DLPF_188HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_188HZ;
break;
case DLPF_98HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_98HZ;
break;
case DLPF_42HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_42HZ;
break;
}
setSPIdivisor(MPU6000_SPI, 64); // 0.65625 MHz SPI clock (within 20 +/- 10%)
GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
spiTransfer(MPU6000_SPI, MPU6000_CONFIG);
spiTransfer(MPU6000_SPI, eepromConfig.dlpfSetting);
GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
setSPIdivisor(MPU6000_SPI, 2); // 21 MHz SPI clock (within 20 +/- 10%)
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Accel Cutoff
eepromConfig.accelCutoff = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // kpAcc, kiAcc
eepromConfig.KpAcc = readFloatCLI();
eepromConfig.KiAcc = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // kpMag, kiMag
eepromConfig.KpMag = readFloatCLI();
eepromConfig.KiMag = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // h dot est/h est Comp Filter A/B
eepromConfig.compFilterA = readFloatCLI();
eepromConfig.compFilterB = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'M': // Magnetic Variation
eepromConfig.magVar = readFloatCLI() * D2R;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
break;
///////////////////////////
case '?':
cliPrint("\n");
cliPrint("'a' Display Sensor Data 'A' Set MPU6000 DLPF A0 thru 3, see aq32Plus.h\n");
cliPrint("'b' MPU6000 Temp Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CkpAcc;kiAcc\n");
cliPrint(" 'D' Set kpMag/kiMag DkpMag;kiMag\n");
cliPrint("'e' Toggle External HMC5883 State 'E' Set h dot est/h est Comp Filter A/B EA;B\n");
cliPrint("'f' Toggle External MS5611 State 'M' Set Mag Variation (+ East, - West) MMagVar\n");
cliPrint("'v' Toggle Vertical Velocity Hold Only\n");
cliPrint(" 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit Sensor CLI '?' Command Summary\n\n");
validQuery = false;
break;
///////////////////////////
}
}
}
void systemInit(void)
{
RCC_ClocksTypeDef rccClocks;
///////////////////////////////////
// Init cycle counter
cycleCounterInit();
// SysTick
SysTick_Config(SystemCoreClock / 1000);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB |
RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO |
RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM8 |
RCC_APB2Periph_ADC1, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 |
RCC_APB1Periph_TIM5 | RCC_APB1Periph_TIM6 |
RCC_APB1Periph_I2C2, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
#ifdef _DTIMING
timingSetup();
#endif
///////////////////////////////////////////////////////////////////////////
checkFirstTime(false);
readEEPROM();
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority
pwmMotorDriverInit();
cliInit();
gpioInit();
adcInit();
LED2_ON;
delay(10000); // 10 seconds of 20 second delay for sensor stabilization
if (GetVCPConnectMode() != eVCPConnectReset)
{
cliPrintF("\r\nUSB startup delay...\r\n");
delay(3000);
if (GetVCPConnectMode() == eVCPConnectData)
{
cliPrintF("\r\nBGC32 firmware starting up, USB connected...\r\n");
}
}
else
{
cliPrintF("\r\nDelaying for usb/serial driver to settle\r\n");
delay(3000);
cliPrintF("\r\nBGC32 firmware starting up, serial active...\r\n");
}
#ifdef __VERSION__
cliPrintF("\ngcc version " __VERSION__ "\n");
#endif
cliPrintF("BGC32 Firmware V%s, Build Date " __DATE__ " "__TIME__" \n", __BGC32_VERSION);
if ((RCC->CR & RCC_CR_HSERDY) != RESET)
{
cliPrintF("\nRunning on external HSE clock....\n");
}
else
{
cliPrintF("\nERROR: Running on internal HSI clock....\n");
}
RCC_GetClocksFreq(&rccClocks);
cliPrintF("\nADCCLK-> %2d MHz\n", rccClocks.ADCCLK_Frequency / 1000000);
cliPrintF( "HCLK-> %2d MHz\n", rccClocks.HCLK_Frequency / 1000000);
cliPrintF( "PCLK1-> %2d MHz\n", rccClocks.PCLK1_Frequency / 1000000);
cliPrintF( "PCLK2-> %2d MHz\n", rccClocks.PCLK2_Frequency / 1000000);
cliPrintF( "SYSCLK-> %2d MHz\n\n", rccClocks.SYSCLK_Frequency / 1000000);
delay(10000); // Remaining 10 seconds of 20 second delay for sensor stabilization - probably not long enough..
LED1_ON;
i2cInit(I2C2);
rcInit();
timingFunctionsInit();
BKPInit();
initFirstOrderFilter();
initPID();
initSinArray();
orientIMU();
initMPU6050();
// initMag();
}
void computeGeoMagElements(void)
{
setupGeoMagWorkspace();
coordGeodetic.lambda = (float)gps.longitude * 1E-7f; // Convert to decimal degrees
coordGeodetic.phi = (float)gps.latitude * 1E-7f; // Convert to decimal degrees
coordGeodetic.HeightAboveEllipsoid = (float)gps.height * 1E-6f; // Convert from mm to km
coordGeodetic.UseGeoid = 0;
userDate.Year = gps.year;
userDate.Month = gps.month;
userDate.Day = gps.day;
//cliPrintF("GPS Lat: %7.3f\n", coordGeodetic.phi);
//cliPrintF("GPS Lon: %7.3f\n", coordGeodetic.lambda);
//cliPrintF("GPS Alt: %7.3f\n", coordGeodetic.HeightAboveEllipsoid);
//cliPrint ("\n");
//MAG_DateToYear(&userDate);
//cliPrintF("GPS Mon: %4ld\n", userDate.Month);
//cliPrintF("GPS Day: %4ld\n", userDate.Day);
//cliPrintF("GPS Yr: %4ld\n", userDate.Year);
//cliPrintF("GPS Dec Yr: %7.3f\n", userDate.DecimalYear);
//cliPrint ("\n");
userDate.DecimalYear = readFloatCLI();
coordGeodetic.HeightAboveEllipsoid = readFloatCLI();
coordGeodetic.phi = readFloatCLI();
coordGeodetic.lambda = readFloatCLI();
MAG_GeodeticToSpherical(ellipsoid, coordGeodetic, &coordSpherical);
MAG_TimelyModifyMagneticModel(userDate, magneticModel, timedMagneticModel);
MAG_Geomag(ellipsoid, coordSpherical, coordGeodetic, timedMagneticModel, &geoMagneticElements);
cliPrint ("\n");
cliPrintF("Dec Yr: %8.1f\n", userDate.DecimalYear);
cliPrintF("Alt: %8.1f\n", coordGeodetic.HeightAboveEllipsoid);
cliPrintF("Lat: %8.1f\n", coordGeodetic.phi);
cliPrintF("Lon: %8.1f\n", coordGeodetic.lambda);
cliPrint ("\n");
cliPrintF("X: %8.1f\n", geoMagneticElements.X);
cliPrintF("Y: %8.1f\n", geoMagneticElements.Y);
cliPrintF("Z: %8.1f\n", geoMagneticElements.Z);
cliPrintF("H: %8.1f\n", geoMagneticElements.H);
cliPrintF("F: %8.1f\n", geoMagneticElements.F);
cliPrintF("INCL: %8.1f\n", geoMagneticElements.Incl);
cliPrintF("DECL: %8.1f\n", geoMagneticElements.Decl);
cliPrint ("\n");
clearGeoMagWorkspace();
}
int main(void)
{
uint32_t currentTime;
systemInit();
systemReady = true;
while (1)
{
///////////////////////////////
if (frame_50Hz)
{
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
processFlightCommands();
executionTime50Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_10Hz)
{
frame_10Hz = false;
currentTime = micros();
deltaTime10Hz = currentTime - previous10HzTime;
previous10HzTime = currentTime;
sensors.mag10Hz[XAXIS] = -((float)rawMag[XAXIS].value * magScaleFactor[XAXIS] - eepromConfig.magBias[XAXIS]);
sensors.mag10Hz[YAXIS] = (float)rawMag[YAXIS].value * magScaleFactor[YAXIS] - eepromConfig.magBias[YAXIS];
sensors.mag10Hz[ZAXIS] = -((float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS] - eepromConfig.magBias[ZAXIS]);
newMagData = false;
magDataUpdate = true;
pressureAverage = pressureSum / 10;
pressureSum = 0;
calculateTemperature();
calculatePressureAltitude();
sensors.pressureAlt10Hz = pressureAlt;
cliCom();
executionTime10Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_200Hz)
{
frame_200Hz = false;
currentTime = micros();
deltaTime200Hz = currentTime - previous200HzTime;
previous200HzTime = currentTime;
dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop
sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - eepromConfig.accelBias[XAXIS]) * eepromConfig.accelScaleFactor[XAXIS];
sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - eepromConfig.accelBias[YAXIS]) * eepromConfig.accelScaleFactor[YAXIS];
sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - eepromConfig.accelBias[ZAXIS]) * eepromConfig.accelScaleFactor[ZAXIS];
sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]);
sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]);
sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro200Hz[ROLL ] = ((float)gyroSummedSamples200Hz[ROLL] / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[YAW ] = -((float)gyroSummedSamples200Hz[YAW] / 5.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
MargAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
eepromConfig.accelCutoff,
magDataUpdate,
dt200Hz );
magDataUpdate = false;
computeAxisCommands(dt200Hz);
mixTable();
writeServos();
writeMotors();
executionTime200Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_100Hz)
{
frame_100Hz = false;
currentTime = micros();
deltaTime100Hz = currentTime - previous100HzTime;
previous100HzTime = currentTime;
dt100Hz = (float)deltaTime100Hz * 0.000001f; // For integrations in 100 Hz loop
sensors.accel100Hz[XAXIS] = -((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelRTBias[XAXIS] - eepromConfig.accelBias[XAXIS]) * eepromConfig.accelScaleFactor[XAXIS];
sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelRTBias[YAXIS] - eepromConfig.accelBias[YAXIS]) * eepromConfig.accelScaleFactor[YAXIS];
sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelRTBias[ZAXIS] - eepromConfig.accelBias[ZAXIS]) * eepromConfig.accelScaleFactor[ZAXIS];
sensors.accel100Hz[XAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[XAXIS], &fourthOrder100Hz[AX_FILTER]);
sensors.accel100Hz[YAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[YAXIS], &fourthOrder100Hz[AY_FILTER]);
sensors.accel100Hz[ZAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[ZAXIS], &fourthOrder100Hz[AZ_FILTER]);
//computeGyroTCBias();
//sensors.gyro100Hz[ROLL ] = ((float)gyroSummedSamples100Hz[ROLL] / 10.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
//sensors.gyro100Hz[PITCH] = -((float)gyroSummedSamples100Hz[PITCH] / 10.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
//sensors.gyro100Hz[YAW ] = -((float)gyroSummedSamples100Hz[YAW] / 10.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
createRotationMatrix();
bodyAccelToEarthAccel();
vertCompFilter(dt100Hz);
//computeAxisCommands(dt100Hz);
//mixTable();
//writeServos();
//writeMotors();
if ( rfTelem1Enabled == true )
{
// 200 Hz Accels
cliPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel200Hz[XAXIS],
sensors.accel200Hz[YAXIS],
sensors.accel200Hz[ZAXIS]);
}
if ( rfTelem2Enabled == true )
{
// 200 Hz Gyros
cliPrintF("%9.4f, %9.4f, %9.4f\n", sensors.gyro200Hz[ROLL ],
sensors.gyro200Hz[PITCH],
sensors.gyro200Hz[YAW ]);
}
if ( rfTelem3Enabled == true )
{
// Roll Rate, Roll Rate Command
cliPrintF("%9.4f, %9.4f\n", sensors.gyro200Hz[ROLL],
rxCommand[ROLL]);
}
if ( rfTelem4Enabled == true )
{
// Pitch Rate, Pitch Rate Command
cliPrintF("%9.4f, %9.4f\n", sensors.gyro200Hz[PITCH],
rxCommand[PITCH]);
}
if ( rfTelem5Enabled == true )
{
// Yaw Rate, Yaw Rate Command
cliPrintF("%9.4f, %9.4f\n", sensors.gyro200Hz[YAW],
rxCommand[YAW]);
}
if ( rfTelem6Enabled == true )
{
// 200 Hz Attitudes
cliPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude200Hz[ROLL ],
sensors.attitude200Hz[PITCH],
sensors.attitude200Hz[YAW ]);
}
executionTime100Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_5Hz)
{
frame_5Hz = false;
currentTime = micros();
deltaTime5Hz = currentTime - previous5HzTime;
previous5HzTime = currentTime;
executionTime5Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_1Hz)
{
frame_1Hz = false;
currentTime = micros();
deltaTime1Hz = currentTime - previous1HzTime;
previous1HzTime = currentTime;
if (execUp == false)
execUpCount++;
if ((execUpCount == 5) && (execUp == false))
{
execUp = true;
LED0_OFF;
LED1_OFF;
}
executionTime1Hz = micros() - currentTime;
}
////////////////////////////////
}
}
void sensorCLI()
{
uint8_t sensorQuery;
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering Sensor CLI....\n\n");
while(true)
{
cliPrint("Sensor CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
sensorQuery = cliRead();
cliPrint("\n");
switch(sensorQuery)
{
///////////////////////////
case 'a': // Sensor Data
cliPrintF("\nAccel Scale Factor: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelScaleFactor[XAXIS],
eepromConfig.accelScaleFactor[YAXIS],
eepromConfig.accelScaleFactor[ZAXIS]);
cliPrintF("Accel Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelBias[XAXIS],
eepromConfig.accelBias[YAXIS],
eepromConfig.accelBias[ZAXIS]);
cliPrintF("Gyro TC Bias Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasSlope[ROLL ],
eepromConfig.gyroTCBiasSlope[PITCH],
eepromConfig.gyroTCBiasSlope[YAW ]);
cliPrintF("Gyro TC Bias Intercept: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasIntercept[ROLL ],
eepromConfig.gyroTCBiasIntercept[PITCH],
eepromConfig.gyroTCBiasIntercept[YAW ]);
cliPrintF("Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS],
eepromConfig.magBias[YAXIS],
eepromConfig.magBias[ZAXIS]);
cliPrintF("Accel One G: %9.4f\n", accelOneG);
cliPrintF("Accel Cutoff: %9.4f\n", eepromConfig.accelCutoff);
cliPrintF("KpAcc (MARG): %9.4f\n", eepromConfig.KpAcc);
cliPrintF("KiAcc (MARG): %9.4f\n", eepromConfig.KiAcc);
cliPrintF("KpMag (MARG): %9.4f\n", eepromConfig.KpMag);
cliPrintF("KiMag (MARG): %9.4f\n", eepromConfig.KiMag);
cliPrintF("hdot est/h est Comp Fil A: %9.4f\n", eepromConfig.compFilterA);
cliPrintF("hdot est/h est Comp Fil B: %9.4f\n\n", eepromConfig.compFilterB);
validQuery = false;
break;
///////////////////////////
case 'b': // Accel Calibration
accelCalibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Magnetometer Calibration
magCalibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'd': // GyroTemp Calibration
gyroTempCalibration();
computeGyroRTBias();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPrint("\nExiting Sensor CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'B': // Accel Cutoff
eepromConfig.accelCutoff = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // kpAcc, kiAcc
eepromConfig.KpAcc = readFloatCLI();
eepromConfig.KiAcc = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // kpMag, kiMag
eepromConfig.KpMag = readFloatCLI();
eepromConfig.KiMag = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // h dot est/h est Comp Filter A/B
eepromConfig.compFilterA = readFloatCLI();
eepromConfig.compFilterB = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
break;
///////////////////////////
case '?':
cliPrint("\n");
cliPrint("'a' Display Sensor Data\n");
cliPrint("'b' Accel Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CKpAcc;KiAcc\n");
cliPrint("'d' Gyro Temp Calibration 'D' Set kpMag/kiMag DKpMag;KiMag\n");
cliPrint(" 'E' Set h dot est/h est Comp Filter A/B EA;B\n");
cliPrint(" 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit Sensor CLI '?' Command Summary\n");
cliPrint("\n");
break;
///////////////////////////
}
}
}
void receiverCLI()
{
char rcOrderString[9];
float tempFloat;
uint8_t index;
uint8_t receiverQuery;
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering Receiver CLI....\n\n");
while(true)
{
cliPrint("Receiver CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
receiverQuery = cliRead();
cliPrint("\n");
switch(receiverQuery)
{
///////////////////////////
case 'a': // Receiver Configuration
cliPrint("\nReceiver Type: ");
switch(eepromConfig.receiverType)
{
case PARALLEL_PWM:
cliPrint("Parallel\n");
break;
case SERIAL_PWM:
cliPrint("Serial\n");
break;
case SPEKTRUM:
cliPrint("Spektrum\n");
break;
}
cliPrint("Current RC Channel Assignment: ");
for (index = 0; index < 8; index++)
rcOrderString[eepromConfig.rcMap[index]] = rcChannelLetters[index];
rcOrderString[index] = '\0';
cliPrint(rcOrderString); cliPrint("\n");
cliPrintF("Spektrum Resolution: %s\n", eepromConfig.spektrumHires ? "11 Bit Mode" : "10 Bit Mode");
cliPrintF("Number of Spektrum Channels: %2d\n", eepromConfig.spektrumChannels);
cliPrintF("Mid Command: %4ld\n", (uint16_t)eepromConfig.midCommand);
cliPrintF("Min Check: %4ld\n", (uint16_t)eepromConfig.minCheck);
cliPrintF("Max Check: %4ld\n", (uint16_t)eepromConfig.maxCheck);
cliPrintF("Min Throttle: %4ld\n", (uint16_t)eepromConfig.minThrottle);
cliPrintF("Max Thottle: %4ld\n\n", (uint16_t)eepromConfig.maxThrottle);
tempFloat = eepromConfig.rateScaling * 180000.0 / PI;
cliPrintF("Max Rate Command: %6.2f DPS\n", tempFloat);
tempFloat = eepromConfig.attitudeScaling * 180000.0 / PI;
cliPrintF("Max Attitude Command: %6.2f Degrees\n\n", tempFloat);
cliPrintF("Arm Delay Count: %3d Frames\n", eepromConfig.armCount);
cliPrintF("Disarm Delay Count: %3d Frames\n\n", eepromConfig.disarmCount);
validQuery = false;
break;
///////////////////////////
case 'b': // Read Max Rate Value
eepromConfig.rateScaling = readFloatCLI() / 180000 * PI;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Read Max Attitude Value
eepromConfig.attitudeScaling = readFloatCLI() / 180000 * PI;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPrint("\nExiting Receiver CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Read RX Input Type
eepromConfig.receiverType = (uint8_t)readFloatCLI();
cliPrint( "\nReceiver Type Changed....\n");
cliPrint("\nSystem Resetting....\n");
delay(100);
writeEEPROM();
systemReset(false);
break;
///////////////////////////
case 'B': // Read RC Control Order
readStringCLI( rcOrderString, 8 );
parseRcChannels( rcOrderString );
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // Read Spektrum Resolution
eepromConfig.spektrumHires = (uint8_t)readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // Read Number of Spektrum Channels
eepromConfig.spektrumChannels = (uint8_t)readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // Read RC Control Points
eepromConfig.midCommand = readFloatCLI();
eepromConfig.minCheck = readFloatCLI();
eepromConfig.maxCheck = readFloatCLI();
eepromConfig.minThrottle = readFloatCLI();
eepromConfig.maxThrottle = readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'F': // Read Arm/Disarm Counts
eepromConfig.armCount = (uint8_t)readFloatCLI();
eepromConfig.disarmCount = (uint8_t)readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
break;
///////////////////////////
case '?':
cliPrint("\n");
cliPrint("'a' Receiver Configuration Data 'A' Set RX Input Type AX, 1=Parallel, 2=Serial, 3=Spektrum\n");
cliPrint("'b' Set Maximum Rate Command 'B' Set RC Control Order BTAER1234\n");
cliPrint("'c' Set Maximum Attitude Command 'C' Set Spektrum Resolution C0 or C1\n");
cliPrint(" 'D' Set Number of Spektrum Channels D6 thru D12\n");
cliPrint(" 'E' Set RC Control Points EmidCmd;minChk;maxChk;minThrot;maxThrot\n");
cliPrint(" 'F' Set Arm/Disarm Counts FarmCount;disarmCount\n");
cliPrint(" 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit Receiver CLI '?' Command Summary\n");
cliPrint("\n");
break;
///////////////////////////
}
}
}
void mixerCLI()
{
float tempFloat;
uint8_t index;
uint8_t rows, columns;
uint8_t mixerQuery;
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_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();
pwmOutputConfig.escPwmRate = eepromConfig.escPwmRate;
pwmOutputConfig.servoPwmRate = eepromConfig.servoPwmRate;
pwmOutputInit(&pwmOutputConfig);
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 17, see baseFlightPlus.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;
///////////////////////////
}
}
}
void gpsCLI()
{
USART_InitTypeDef USART_InitStructure;
uint8_t gpsQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering GPS CLI....\n\n");
while(true)
{
cliPrint("GPS CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
gpsQuery = cliRead();
cliPrint("\n");
switch(gpsQuery)
{
///////////////////////////
case 'a': // GPS Installation Data
cliPrint("\n");
switch(eepromConfig.gpsType)
{
///////////////
case NO_GPS:
cliPrint("No GPS Installed....\n\n");
break;
///////////////
case MEDIATEK_3329_BINARY:
cliPrint("MediaTek 3329 GPS installed, Binary Mode....\n\n");
break;
///////////////
case MEDIATEK_3329_NMEA:
cliPrint("MediaTek 3329 GPS Installed, NMEA Mode....\n\n");
break;
///////////////
case UBLOX:
cliPrint("UBLOX GPS Installed, Binary Mode....\n\n");
break;
///////////////
}
cliPrintF("GPS Baud Rate: %6ld\n\n", eepromConfig.gpsBaudRate);
validQuery = false;
break;
///////////////////////////
case 'x':
cliPrint("\nExiting GPS CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Set GPS Installed State to False
eepromConfig.gpsType = NO_GPS;
gpsQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Set GPS Type to MediaTek 3329 Binary
eepromConfig.gpsType = MEDIATEK_3329_BINARY;
initGPS();
gpsQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // Set GPS Type to MediaTek 3329 NMEA
eepromConfig.gpsType = MEDIATEK_3329_NMEA;
initGPS();
gpsQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // Set GPS Type to UBLOX Binary
eepromConfig.gpsType = UBLOX;
initGPS();
gpsQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'S': // Read GPS Baud Rate
eepromConfig.gpsBaudRate = (uint16_t)readFloatCLI();
USART_StructInit(&USART_InitStructure);
USART_InitStructure.USART_BaudRate = eepromConfig.gpsBaudRate;
USART_Init(USART2, &USART_InitStructure);
gpsQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
break;
///////////////////////////
case '?':
cliPrint("\n");
cliPrint("'a' Display GPS Installation Data 'A' Set GPS Type to No GPS\n");
cliPrint(" 'B' Set GPS Type to MediaTek 3329 Binary\n");
cliPrint(" 'C' Set GPS Type to MediaTek 3329 NMEA\n");
cliPrint(" 'D' Set GPS Type to UBLOX\n");
cliPrint(" 'S' Set GPS Baud Rate\n");
cliPrint(" 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit GPS CLI '?' Command Summary\n");
cliPrint("\n");
break;
///////////////////////////
}
}
}
void eepromCLI()
{
uint32_t c1, c2;
uint8_t eepromQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering EEPROM CLI....\n\n");
while(true)
{
cliPrint("EEPROM CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
eepromQuery = cliRead();
cliPrint("\n");
switch(eepromQuery)
{
// 'a' is the standard "print all the information" character
case 'a': // config struct data
c1 = eepromConfig.CRCAtEnd[0];
zeroPIDintegralError();
zeroPIDstates();
c2 = crc32bEEPROM(&eepromConfig, false);
cliPrintF("Config structure information:\n");
cliPrintF("Version : %d\n", eepromConfig.version );
cliPrintF("Size : %d\n", sizeof(eepromConfig) );
cliPrintF("CRC on last read : %08X\n", c1 );
cliPrintF("Current CRC : %08X\n", c2 );
if ( c1 != c2 )
cliPrintF(" CRCs differ. Current Config has not yet been saved.\n");
cliPrintF("CRC Flags :\n");
cliPrintF(" History Bad : %s\n", eepromConfig.CRCFlags & CRC_HistoryBad ? "true" : "false" );
validQuery = false;
break;
///////////////////////////
case 'c': // Write out to Console in Hex. (RAM -> console)
// we assume the flyer is not in the air, so that this is ok;
// these change randomly when not in flight and can mistakenly
// make one think that the in-memory eeprom struct has changed
zeroPIDintegralError();
zeroPIDstates();
cliPrintF("\n");
cliPrintEEPROM(&eepromConfig);
cliPrintF("\n");
if (crcCheckVal != crc32bEEPROM(&eepromConfig, true))
{
cliPrint("NOTE: in-memory config CRC invalid; there have probably been changes to\n");
cliPrint(" eepromConfig since the last write to flash/eeprom.\n");
}
validQuery = false;
break;
///////////////////////////
case 'H': // clear bad history flag
cliPrintF("Clearing Bad History flag.\n");
eepromConfig.CRCFlags &= ~CRC_HistoryBad;
validQuery = false;
break;
///////////////////////////
case 'C': // Read in from Console in hex. Console -> RAM
;
uint32_t sz = sizeof(eepromConfig);
eepromConfig_t e;
uint8_t *p = (uint8_t*)&e;
uint8_t *end = (uint8_t*)(&e + 1);
uint32_t t = millis();
enum { Timeout = 100 }; // timeout is in ms
int second_nibble = 0; // 0 or 1
char c;
uint32_t chars_encountered = 0;
cliPrintF("Ready to read in config. Expecting %d (0x%03X) bytes as %d\n",
sz, sz, sz * 2);
cliPrintF("hexadecimal characters, optionally separated by [ \\n\\r_].\n");
cliPrintF("Times out if no character is received for %dms\n", Timeout);
memset(p, 0, end - p);
while (p < end)
{
while (!cliAvailable() && millis() - t < Timeout) {}
t = millis();
c = cliAvailable() ? cliRead() : '\0';
int8_t hex = parse_hex(c);
int ignore = c == ' ' || c == '\n' || c == '\r' || c == '_' ? true : false;
if (c != '\0') // assume the person isn't sending null chars
chars_encountered++;
if (ignore)
continue;
if (hex == -1)
break;
*p |= second_nibble ? hex : hex << 4;
p += second_nibble;
second_nibble ^= 1;
}
if (c == 0)
{
cliPrintF("Did not receive enough hex chars! (got %d, expected %d)\n",
(p - (uint8_t*)&e) * 2 + second_nibble, sz * 2);
}
else if (p < end || second_nibble)
{
cliPrintF("Invalid character found at position %d: '%c' (0x%02x)",
chars_encountered, c, c);
}
else if (crcCheckVal != crc32bEEPROM(&e, true))
{
cliPrintF("CRC mismatch! Not writing to in-memory config.\n");
cliPrintF("Here's what was received:\n\n");
cliPrintEEPROM(&e);
}
else
{
// check to see if the newly received eeprom config
// actually differs from what's in-memory
zeroPIDintegralError();
zeroPIDstates();
int i;
for (i = 0; i < sz; i++)
if (((uint8_t*)&e)[i] != ((uint8_t*)&eepromConfig)[i])
break;
if (i == sz)
{
cliPrintF("NOTE: uploaded config was identical to in-memory config.\n");
}
else
{
eepromConfig = e;
cliPrintF("In-memory config updated!\n");
cliPrintF("NOTE: config not written to EEPROM; use 'W' to do so.\n");
}
}
// eat the next 100ms (or whatever Timeout is) of characters,
// in case the person pasted too much by mistake or something
t = millis();
while (millis() - t < Timeout)
if (cliAvailable())
cliRead();
validQuery = false;
break;
///////////////////////////
case 'E': // Read in from EEPROM. (EEPROM -> RAM)
cliPrint("Re-reading EEPROM.\n");
readEEPROM();
validQuery = false;
break;
///////////////////////////
case 'x': // exit EEPROM CLI
cliPrint("\nExiting EEPROM CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'W':
case 'e': // Write out to EEPROM. (RAM -> EEPROM)
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
validQuery = false;
break;
///////////////////////////
case 'f': // Write out to sdCard FILE. (RAM -> FILE)
validQuery = false;
break;
///////////////////////////
case 'F': // Read in from sdCard FILE. (FILE -> RAM)
validQuery = false;
break;
///////////////////////////
case 'V': // Reset EEPROM Parameters
cliPrint( "\nEEPROM Parameters Reset....(not rebooting)\n" );
checkFirstTime(true);
validQuery = false;
break;
///////////////////////////
case '?':
// 0 1 2 3 4 5 6 7
// 01234567890123456789012345678901234567890123456789012345678901234567890123456789
cliPrintF("\n");
cliPrintF("'a' Display in-RAM config information\n");
cliPrintF("'c' Write in-RAM -> Console (as Hex) 'C' Read Console (as Hex) -> in-RAM\n");
cliPrintF("'e' Write in-RAM -> EEPROM 'E' Read EEPROM -> in-RAM\n");
cliPrintF("'f' Write in-RAM -> sd FILE (Not yet imp) 'F' Read sd FILE -> in-RAM (Not imp)\n");
cliPrintF(" 'H' Clear CRC Bad History flag\n");
cliPrintF(" 'V' Reset in-RAM config to default.\n");
cliPrintF("'x' Exit EEPROM CLI '?' Command Summary\n");
cliPrintF("\n");
cliPrintF("For compatability: 'W' Write in-RAM -> EEPROM\n");
cliPrintF("\n");
break;
///////////////////////////
}
}
}
void sensorCLI()
{
uint8_t sensorQuery = 'x';
uint8_t tempInt;
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering Sensor CLI....\n\n");
while(true)
{
cliPrint("Sensor CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
sensorQuery = cliRead();
cliPrint("\n");
switch(sensorQuery)
{
///////////////////////////
case 'a': // Sensor Data
cliPrintF("MPU Accel Bias: %9.3f, %9.3f, %9.3f\n", eepromConfig.accelBiasMPU[XAXIS],
eepromConfig.accelBiasMPU[YAXIS],
eepromConfig.accelBiasMPU[ZAXIS]);
cliPrintF("MPU Accel Scale Factor: %9.7f, %9.7f, %9.7f\n", eepromConfig.accelScaleFactorMPU[XAXIS],
eepromConfig.accelScaleFactorMPU[YAXIS],
eepromConfig.accelScaleFactorMPU[ZAXIS]);
cliPrintF("Accel Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasSlope[XAXIS],
eepromConfig.accelTCBiasSlope[YAXIS],
eepromConfig.accelTCBiasSlope[ZAXIS]);
cliPrintF("Accel Temp Comp Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasIntercept[XAXIS],
eepromConfig.accelTCBiasIntercept[YAXIS],
eepromConfig.accelTCBiasIntercept[ZAXIS]);
cliPrintF("Gyro Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasSlope[ROLL ],
eepromConfig.gyroTCBiasSlope[PITCH],
eepromConfig.gyroTCBiasSlope[YAW ]);
cliPrintF("Gyro Temp Comp Intercept: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasIntercept[ROLL ],
eepromConfig.gyroTCBiasIntercept[PITCH],
eepromConfig.gyroTCBiasIntercept[YAW ]);
cliPrintF("Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS],
eepromConfig.magBias[YAXIS],
eepromConfig.magBias[ZAXIS]);
cliPrintF("Accel One G: %9.4f\n", accelOneG);
cliPrintF("Accel Cutoff: %9.4f\n", eepromConfig.accelCutoff);
cliPrintF("KpAcc (MARG): %9.4f\n", eepromConfig.KpAcc);
cliPrintF("KiAcc (MARG): %9.4f\n", eepromConfig.KiAcc);
cliPrintF("KpMag (MARG): %9.4f\n", eepromConfig.KpMag);
cliPrintF("KiMag (MARG): %9.4f\n", eepromConfig.KiMag);
cliPrintF("hdot est/h est Comp Fil A: %9.4f\n", eepromConfig.compFilterA);
cliPrintF("hdot est/h est Comp Fil B: %9.4f\n", eepromConfig.compFilterB);
cliPrint("MPU6000 DLPF: ");
switch(eepromConfig.dlpfSetting)
{
case DLPF_256HZ:
cliPrint("256 Hz\n");
break;
case DLPF_188HZ:
cliPrint("188 Hz\n");
break;
case DLPF_98HZ:
cliPrint("98 Hz\n");
break;
case DLPF_42HZ:
cliPrint("42 Hz\n");
break;
}
if (eepromConfig.verticalVelocityHoldOnly)
cliPrint("Vertical Velocity Hold Only\n\n");
else
cliPrint("Vertical Velocity and Altitude Hold\n\n");
cliPrintF("Voltage Monitor Scale: %9.4f\n", eepromConfig.voltageMonitorScale);
cliPrintF("Voltage Monitor Bias: %9.4f\n", eepromConfig.voltageMonitorBias);
cliPrintF("Number of Battery Cells: %1d\n\n", eepromConfig.batteryCells);
cliPrintF("Battery Low Setpoint: %4.2f volts\n", eepromConfig.batteryLow);
cliPrintF("Battery Very Low Setpoint: %4.2f volts\n", eepromConfig.batteryVeryLow);
cliPrintF("Battery Max Low Setpoint: %4.2f volts\n\n", eepromConfig.batteryMaxLow);
validQuery = false;
break;
///////////////////////////
case 'b': // MPU6000 Calibration
mpu6000Calibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Magnetometer Calibration
magCalibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'd': // Accel Bias and Scale Factor Calibration
accelCalibrationMPU();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'v': // Toggle Vertical Velocity Hold Only
if (eepromConfig.verticalVelocityHoldOnly)
eepromConfig.verticalVelocityHoldOnly = false;
else
eepromConfig.verticalVelocityHoldOnly = true;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPrint("\nExiting Sensor CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Set MPU6000 Digital Low Pass Filter
tempInt = (uint8_t)readFloatCLI();
switch(tempInt)
{
case DLPF_256HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_256HZ;
break;
case DLPF_188HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_188HZ;
break;
case DLPF_98HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_98HZ;
break;
case DLPF_42HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_42HZ;
break;
}
setSPIdivisor(MPU6000_SPI, 64); // 0.65625 MHz SPI clock
GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
spiTransfer(MPU6000_SPI, MPU6000_CONFIG);
spiTransfer(MPU6000_SPI, eepromConfig.dlpfSetting);
GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
setSPIdivisor(MPU6000_SPI, 2); // 21 MHz SPI clock (within 20 +/- 10%)
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Accel Cutoff
eepromConfig.accelCutoff = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // kpAcc, kiAcc
eepromConfig.KpAcc = readFloatCLI();
eepromConfig.KiAcc = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // kpMag, kiMag
eepromConfig.KpMag = readFloatCLI();
eepromConfig.KiMag = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // h dot est/h est Comp Filter A/B
eepromConfig.compFilterA = readFloatCLI();
eepromConfig.compFilterB = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'M': // Set Voltage Monitor Trip Points
eepromConfig.batteryLow = readFloatCLI();
eepromConfig.batteryVeryLow = readFloatCLI();
eepromConfig.batteryMaxLow = readFloatCLI();
thresholds[BATTERY_LOW].value = eepromConfig.batteryLow;
thresholds[BATTERY_VERY_LOW].value = eepromConfig.batteryVeryLow;
thresholds[BATTRY_MAX_LOW].value = eepromConfig.batteryMaxLow;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'V': // Set Voltage Monitor Parameters
eepromConfig.voltageMonitorScale = readFloatCLI();
eepromConfig.voltageMonitorBias = readFloatCLI();
eepromConfig.batteryCells = (uint8_t)readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPrint("\n");
cliPrint("'a' Display Sensor Data 'A' Set MPU6000 DLPF A0 thru 3\n");
cliPrint("'b' MPU6000 Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CKpAcc;KiAcc\n");
cliPrint("'d' Accel Bias and SF Calibration 'D' Set kpMag/kiMag DKpMag;KiMag\n");
cliPrint(" 'E' Set h dot est/h est Comp Filter A/B EA;B\n");
cliPrint(" 'M' Set Voltage Monitor Trip Points Mlow;veryLow;maxLow\n");
cliPrint("'v' Toggle Vertical Velocity Hold Only 'V' Set Voltage Monitor Parameters Vscale;bias;cells\n");
cliPrint(" 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit Sensor CLI '?' Command Summary\n");
cliPrint("\n");
break;
///////////////////////////
}
}
}
void sensorCLI()
{
uint8_t sensorQuery = 'x';
uint8_t tempInt;
uint8_t validQuery = false;
cliBusy = true;
cliPrintF("\nEntering Sensor CLI....\n\n");
while (true)
{
cliPrintF("Sensor CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
sensorQuery = getChar();
//cliRead(sensorQuery, 1);
cliPrintF("\n");
switch (sensorQuery)
{
///////////////////////////
case 'a': // Sensor Data
cliPrintF("Accel Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasSlope[XAXIS],
eepromConfig.accelTCBiasSlope[YAXIS],
eepromConfig.accelTCBiasSlope[ZAXIS]);
cliPrintF("Accel Temp Comp Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasIntercept[XAXIS],
eepromConfig.accelTCBiasIntercept[YAXIS],
eepromConfig.accelTCBiasIntercept[ZAXIS]);
cliPrintF("Gyro Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasSlope[ROLL ],
eepromConfig.gyroTCBiasSlope[PITCH],
eepromConfig.gyroTCBiasSlope[YAW ]);
cliPrintF("Gyro Temp Comp Intercept: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasIntercept[ROLL ],
eepromConfig.gyroTCBiasIntercept[PITCH],
eepromConfig.gyroTCBiasIntercept[YAW ]);
cliPrintF("Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS],
eepromConfig.magBias[YAXIS],
eepromConfig.magBias[ZAXIS]);
cliPrintF("Accel One G: %9.4f\n", accelOneG);
cliPrintF("Accel Cutoff: %9.4f\n", eepromConfig.accelCutoff);
cliPrintF("KpAcc (MARG): %9.4f\n", eepromConfig.KpAcc);
cliPrintF("KiAcc (MARG): %9.4f\n", eepromConfig.KiAcc);
cliPrintF("KpMag (MARG): %9.4f\n", eepromConfig.KpMag);
cliPrintF("KiMag (MARG): %9.4f\n", eepromConfig.KiMag);
cliPrintF("MPU6000 DLPF: ");
switch (eepromConfig.dlpfSetting)
{
case DLPF_256HZ:
cliPrintF("256 Hz\n");
break;
case DLPF_188HZ:
cliPrintF("188 Hz\n");
break;
case DLPF_98HZ:
cliPrintF("98 Hz\n");
break;
case DLPF_42HZ:
cliPrintF("42 Hz\n");
break;
}
validQuery = false;
break;
///////////////////////////
case 'b': // MPU6050Calibration
mpu6050Calibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
// HJI case 'c': // Magnetometer Calibration
// HJI magCalibration();
// HJI sensorQuery = 'a';
// HJI validQuery = true;
// HJI break;
///////////////////////////
case 'x':
cliPrintF("\nExiting Sensor CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Set MPU6000 Digital Low Pass Filter
tempInt = (uint8_t)readFloatCLI();
switch (tempInt)
{
case DLPF_256HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_256HZ;
break;
case DLPF_188HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_188HZ;
break;
case DLPF_98HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_98HZ;
break;
case DLPF_42HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_42HZ;
break;
}
i2cWrite(MPU6050_ADDRESS, MPU6050_CONFIG, eepromConfig.dlpfSetting); // Accel and Gyro DLPF Setting
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Accel Cutoff
eepromConfig.accelCutoff = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // kpAcc, kiAcc
eepromConfig.KpAcc = readFloatCLI();
eepromConfig.KiAcc = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // kpMag, kiMag
eepromConfig.KpMag = readFloatCLI();
eepromConfig.KiMag = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPrintF("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
break;
///////////////////////////
case '?':
cliPrintF("\n");
cliPrintF("'a' Display Sensor Data 'A' Set MPU6000 DLPF A0 thru 3\n");
cliPrintF("'b' MPU6050 Temp Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPrintF(" 'C' Set kpAcc/kiAcc CkpAcc;kiAcc\n");
// HJI cliPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CkpAcc;kiAcc\n");
cliPrintF(" 'D' Set kpMag/kiMag DkpMag;kiMag\n");
cliPrintF(" 'W' Write EEPROM Parameters\n");
cliPrintF("'x' Exit Sensor CLI '?' Command Summary\n\n");
break;
///////////////////////////
}
}
}
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;
///////////////////////////
}
}
}
