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; /////////////////////////// } } }