/* *********************************************************************
Constructor
******************************************************************** */
GameController::GameController(OSystem* _osystem): state(_osystem) {
p_osystem = _osystem;
p_global_event_obj = p_osystem->event();
p_console = &(p_osystem->console());
MediaSource& mediasrc = p_console->mediaSource();
p_emulator_system = &(p_console->system());
i_screen_width = mediasrc.width();
i_screen_height = mediasrc.height();
b_send_screen_matrix = true;
b_send_console_ram = true;
i_skip_frames_num = 0;
i_skip_frames_counter = 0;
e_previous_a_action = PLAYER_A_NOOP;
e_previous_b_action = PLAYER_B_NOOP;
// load the RL wrapper for the ROM
string rom_file = p_osystem->settings().getString("rom_file");
m_rom_settings = buildRomRLWrapper(rom_file);
if (m_rom_settings == NULL) {
std::cerr << "Unsupported ROM file" << std::endl;
exit(1);
}
state.setSettings(m_rom_settings);
// MGB
p_num_system_reset_steps = atoi(_osystem->settings().getString("system_reset_steps").c_str());
systemReset();
}
// evaluate all other incoming serial data
static void evaluateOtherData(uint8_t sr)
{
if (sr == '#')
cliProcess();
else if (sr == mcfg.reboot_character)
systemReset(true); // reboot to bootloader
}
long cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
{
if (ptr) {
displayClear(pDisplay);
displayWrite(pDisplay, 5, 3, "REBOOTING...");
displayResync(pDisplay); // Was max7456RefreshAll(); why at this timing?
stopMotors();
stopPwmAllMotors();
delay(200);
cmsTraverseGlobalExit(&menuMain);
if (currentMenu->onExit)
currentMenu->onExit((OSD_Entry *)NULL); // Forced exit
saveConfigAndNotify();
}
cmsInMenu = false;
displayRelease(pDisplay);
currentMenu = NULL;
if (ptr)
systemReset();
ENABLE_ARMING_FLAG(OK_TO_ARM);
return 0;
}
void buttonsHandleColdBootButtonPresses(void)
{
uint8_t secondsRemaining = 10;
bool bothButtonsHeld;
do {
bothButtonsHeld = !digitalIn(BUTTON_A_PORT, BUTTON_A_PIN) && !digitalIn(BUTTON_B_PORT, BUTTON_B_PIN);
if (bothButtonsHeld) {
if (--secondsRemaining == 0) {
resetEEPROM();
systemReset();
}
if (secondsRemaining > 5) {
delay(1000);
} else {
// flash quicker after a few seconds
delay(500);
LED0_TOGGLE;
delay(500);
}
LED0_TOGGLE;
}
} while (bothButtonsHeld);
// buttons released between 5 and 10 seconds
if (secondsRemaining < 5) {
usbGenerateDisconnectPulse();
flashLedsAndBeep();
systemResetToBootloader();
}
}
UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent)
{
m_config = new Ui_UploaderWidget();
m_config->setupUi(this);
currentStep = IAP_STATE_READY;
resetOnly=false;
dfu = NULL;
m_timer = 0;
m_progress = 0;
// Listen to autopilot connection events
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
TelemetryManager* telMngr = pm->getObject<TelemetryManager>();
connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect()));
connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect()));
connect(m_config->haltButton, SIGNAL(clicked()), this, SLOT(goToBootloader()));
connect(m_config->resetButton, SIGNAL(clicked()), this, SLOT(systemReset()));
connect(m_config->bootButton, SIGNAL(clicked()), this, SLOT(systemBoot()));
connect(m_config->rescueButton, SIGNAL(clicked()), this, SLOT(systemRescue()));
getSerialPorts();
QIcon rbi;
rbi.addFile(QString(":uploader/images/view-refresh.svg"));
m_config->refreshPorts->setIcon(rbi);
connect(m_config->refreshPorts, SIGNAL(clicked()), this, SLOT(getSerialPorts()));
// And check whether by any chance we are not already connected
if (telMngr->isConnected())
onAutopilotConnect();
}
int main() {
systemReset(); // peripherals but not PC
setupCLK();
setupLED();
setupUSB();
setupBUTTON();
setupFLASH();
strobePin(LED_BANK,LED,STARTUP_BLINKS,BLINK_FAST);
/* wait for host to upload program or halt bootloader */
bool no_user_jump = !checkUserCode(USER_CODE_FLASH) && !checkUserCode(USER_CODE_RAM) || readPin(BUTTON_BANK,BUTTON);
int delay_count = 0;
while ((delay_count++ < BOOTLOADER_WAIT)
|| no_user_jump) {
strobePin(LED_BANK,LED,1,BLINK_SLOW);
if (dfuUploadStarted()) {
dfuFinishUpload(); // systemHardReset from DFU once done
}
}
if (checkUserCode(USER_CODE_RAM)) {
jumpToUser(USER_CODE_RAM);
} else if (checkUserCode(USER_CODE_FLASH)) {
jumpToUser(USER_CODE_FLASH);
} else {
// some sort of fault occurred, hard reset
strobePin(LED_BANK,LED,5,BLINK_FAST);
systemHardReset();
}
}
void systemResetToBootloader(void)
{
// 1FFFF000 -> 20000200 -> SP
// 1FFFF004 -> 1FFFF021 -> PC
*((uint32_t *)0x20004FF0) = 0xDEADBEEF; // 20KB STM32F103
systemReset();
}
static void cliSave(char *cmdline)
{
cliPrint("Saving...");
writeEEPROM(0, true);
cliPrint("\r\nRebooting...");
delay(10);
systemReset(false);
}
static void cliDefaults(char *cmdline)
{
cliPrint("Resetting to defaults...\r\n");
checkFirstTime(true);
cliPrint("Rebooting...");
delay(10);
systemReset(false);
}
static void cliSave(char *cmdline)
{
uartPrint("Saving...");
writeParams(0);
uartPrint("\r\nRebooting...");
delay(10);
systemReset(false);
}
void systemResetToBootloader(void)
{
// 1FFFF000 -> 20000200 -> SP
// 1FFFF004 -> 1FFFF021 -> PC
*((uint32_t *)0x20009FFC) = 0xDEADBEEF; // 40KB SRAM STM32F30X
systemReset();
}
void serialCom(void)
{
uint8_t c;
int i;
for (i = 0; i < numTelemetryPorts; i++) {
currentPortState = &ports[i];
// in cli mode, all serial stuff goes to here. enter cli mode by sending #
if (cliMode) {
cliProcess();
return;
}
if (pendReboot)
systemReset(false); // noreturn
while (serialTotalBytesWaiting(currentPortState->port)) {
c = serialRead(currentPortState->port);
if (currentPortState->c_state == IDLE) {
currentPortState->c_state = (c == '$') ? HEADER_START : IDLE;
if (currentPortState->c_state == IDLE && !f.ARMED)
evaluateOtherData(c); // if not armed evaluate all other incoming serial data
} else if (currentPortState->c_state == HEADER_START) {
currentPortState->c_state = (c == 'M') ? HEADER_M : IDLE;
} else if (currentPortState->c_state == HEADER_M) {
currentPortState->c_state = (c == '<') ? HEADER_ARROW : IDLE;
} else if (currentPortState->c_state == HEADER_ARROW) {
if (c > INBUF_SIZE) { // now we are expecting the payload size
currentPortState->c_state = IDLE;
continue;
}
currentPortState->dataSize = c;
currentPortState->offset = 0;
currentPortState->checksum = 0;
currentPortState->indRX = 0;
currentPortState->checksum ^= c;
currentPortState->c_state = HEADER_SIZE; // the command is to follow
} else if (currentPortState->c_state == HEADER_SIZE) {
currentPortState->cmdMSP = c;
currentPortState->checksum ^= c;
currentPortState->c_state = HEADER_CMD;
} else if (currentPortState->c_state == HEADER_CMD && currentPortState->offset < currentPortState->dataSize) {
currentPortState->checksum ^= c;
currentPortState->inBuf[currentPortState->offset++] = c;
} else if (currentPortState->c_state == HEADER_CMD && currentPortState->offset >= currentPortState->dataSize) {
if (currentPortState->checksum == c) { // compare calculated and transferred checksum
evaluateCommand(); // we got a valid packet, evaluate it
}
currentPortState->c_state = IDLE;
}
}
}
}
// evaluate all other incoming serial data
static void evaluateOtherData(uint8_t sr)
{
switch (sr) {
case '#':
cliProcess();
break;
case 'R':
systemReset(true); // reboot to bootloader
break;
}
}
int main()
{
systemReset(); // peripherals but not PC
setupCLK();
setupLEDAndButton();
setupUSB();
setupFLASH();
// strobePin(LED_BANK, LED_PIN, 9999, BLINK_SLOW,LED_ON_STATE);
strobePin(LED_BANK, LED_PIN, STARTUP_BLINKS, BLINK_FAST,LED_ON_STATE);
/* wait for host to upload program or halt bootloader */
bool no_user_jump = (!checkUserCode(USER_CODE_FLASH0X8005000) && !checkUserCode(USER_CODE_FLASH0X8002000)) || readButtonState() ;
int delay_count = 0;
while ((delay_count++ < BOOTLOADER_WAIT) || no_user_jump)
{
strobePin(LED_BANK, LED_PIN, 1, BLINK_SLOW,LED_ON_STATE);
if (dfuUploadStarted())
{
dfuFinishUpload(); // systemHardReset from DFU once done
}
}
if (checkUserCode(USER_CODE_FLASH0X8002000))
{
jumpToUser(USER_CODE_FLASH0X8002000);
}
else
{
if (checkUserCode(USER_CODE_FLASH0X8005000))
{
jumpToUser(USER_CODE_FLASH0X8005000);
}
else
{
// Nothing to execute in either Flash or RAM
strobePin(LED_BANK, LED_PIN, 5, BLINK_FAST,LED_ON_STATE);
systemHardReset();
}
}
return 0;// Added to please the compiler
}
int main()
{
systemReset(); // peripherals but not PC
setupCLK();
setupLED();
setupUSB();
setupBUTTON();
setupFLASH();
strobePin(LED_BANK, LED, STARTUP_BLINKS, BLINK_FAST);
/* wait for host to upload program or halt bootloader */
bool no_user_jump = (!checkUserCode(USER_CODE_RAM) && !checkUserCode(USER_CODE_FLASH0X8005000) && !checkUserCode(USER_CODE_FLASH0X8002000)) || readPin(BUTTON_BANK,BUTTON);
int delay_count = 0;
while ((delay_count++ < BOOTLOADER_WAIT) || no_user_jump)
{
strobePin(LED_BANK, LED, 1, BLINK_SLOW);
if (dfuUploadStarted())
{
dfuFinishUpload(); // systemHardReset from DFU once done
}
}
if (checkUserCode(USER_CODE_RAM))
{
jumpToUser(USER_CODE_RAM);
}
else if (checkUserCodeRam(USER_CODE_FLASH0X8002000+4))
{
int ptr;
/*
u32 *flashAdr = (u32 *)USER_CODE_FLASH0X8002000;
u32 *ramAdr = (u32 *)USER_CODE_RAM;
for (ptr = 0; ptr < ((RAM_END-USER_CODE_RAM)-1024)/4; ptr ++) {
ramAdr[ptr] = flashAdr[ptr];
}
*/
u32 const *flashAdr = (u32 const *)USER_CODE_FLASH0X8002000;
u32 *ramAdr = (u32 *)USER_CODE_RAM;
ptr = ((RAM_END-USER_CODE_RAM)-1024)>>2;
while (ptr--)
{
*ramAdr++ = *flashAdr++;
}
jumpToUser(USER_CODE_RAM);
}
UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent)
{
m_config = new Ui_UploaderWidget();
m_config->setupUi(this);
m_currentIAPStep = IAP_STATE_READY;
m_resetOnly = false;
m_dfu = NULL;
m_autoUpdateClosing = false;
// Listen to autopilot connection events
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
TelemetryManager *telMngr = pm->getObject<TelemetryManager>();
connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect()));
connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect()));
Core::ConnectionManager *cm = Core::ICore::instance()->connectionManager();
connect(cm, SIGNAL(deviceConnected(QIODevice *)), this, SLOT(onPhysicalHWConnect()));
connect(m_config->haltButton, SIGNAL(clicked()), this, SLOT(systemHalt()));
connect(m_config->resetButton, SIGNAL(clicked()), this, SLOT(systemReset()));
connect(m_config->bootButton, SIGNAL(clicked()), this, SLOT(systemBoot()));
connect(m_config->safeBootButton, SIGNAL(clicked()), this, SLOT(systemSafeBoot()));
connect(m_config->eraseBootButton, SIGNAL(clicked()), this, SLOT(systemEraseBoot()));
connect(m_config->rescueButton, SIGNAL(clicked()), this, SLOT(systemRescue()));
getSerialPorts();
connect(m_config->autoUpdateButton, SIGNAL(clicked()), this, SLOT(startAutoUpdate()));
connect(m_config->autoUpdateEraseButton, SIGNAL(clicked()), this, SLOT(startAutoUpdateErase()));
connect(m_config->autoUpdateOkButton, SIGNAL(clicked()), this, SLOT(closeAutoUpdate()));
m_config->autoUpdateButton->setEnabled(autoUpdateCapable());
m_config->autoUpdateEraseButton->setEnabled(autoUpdateCapable());
m_config->autoUpdateGroupBox->setVisible(false);
m_config->refreshPorts->setIcon(QIcon(":uploader/images/view-refresh.svg"));
bootButtonsSetEnable(false);
connect(m_config->refreshPorts, SIGNAL(clicked()), this, SLOT(getSerialPorts()));
connect(m_config->pbHelp, SIGNAL(clicked()), this, SLOT(openHelp()));
// And check whether by any chance we are not already connected
if (telMngr->isConnected()) {
onAutopilotConnect();
}
}
void jumpToUser(u32 usrAddr) {
typedef void (*funcPtr)(void);
// The start of the bootloader references the user application location
u32 appStart = usrAddr;
funcPtr userMain;
systemReset(); // resets clocks and periphs, not core regs
// Offset by 4 where Wiring app starts. Add 1 because it will be a thumb instruction.
userMain = (funcPtr)(*(vu32 *)(appStart + 4));
// set the users stack ptr
__set_MSP(*(vu32 *)appStart);
// Go!
userMain();
}
void initialization()
{
sei();
setupMillisTimers();
initFirmata();
systemReset();
initUart(1,BAUD_115200);
#ifdef PLUS_BOARD
initUart(0,getSavedBaudRateFromEeprom());
#endif
#ifdef CLASSIC_BOARD
initUart(0,BAUD_115200);
#endif
setUnusedPinsAsOutput();
setupUartLeds();
requestBluetoothReset();
sendIsAlive();
}
void failureMode(failureMode_e mode)
{
int codeRepeatsRemaining = 10;
int codeFlashesRemaining;
int shortFlashesRemaining;
while (codeRepeatsRemaining--) {
LED1_ON;
LED0_OFF;
shortFlashesRemaining = 5;
codeFlashesRemaining = mode + 1;
uint8_t flashDuration = SHORT_FLASH_DURATION;
while (shortFlashesRemaining || codeFlashesRemaining) {
LED1_TOGGLE;
LED0_TOGGLE;
BEEP_ON;
delay(flashDuration);
LED1_TOGGLE;
LED0_TOGGLE;
BEEP_OFF;
delay(flashDuration);
if (shortFlashesRemaining) {
shortFlashesRemaining--;
if (shortFlashesRemaining == 0) {
delay(500);
flashDuration = CODE_FLASH_DURATION;
}
} else {
codeFlashesRemaining--;
}
}
delay(1000);
}
#ifdef DEBUG
systemReset();
#else
systemResetToBootloader();
#endif
}
static void Acc_Calibrate(void) // Removed Sphere Algo, wasn't really better, sorry.
{
uint16_t i;
Acc_500Hz_AVG(cfg.accZero, ACCdiscardcnt); // Discard some values for warmup abuse accZero[3]
Acc_500Hz_AVG(cfg.accZero, ACCavgcount);
cfg.sens_1G = 16384; // preset 2^14 that is the 16G Scale of MPU
for (i = 0; i < 9; i++) // Eval Bitresolution of ACC. BitScale is recognized here
{
if((float)abs((int16_t)cfg.accZero[2]) > ((float)cfg.sens_1G * 0.85f)) break;
else cfg.sens_1G >>= 1;
}
cfg.accZero[2] -= cfg.sens_1G;
for (i = 0; i < 3; i++) cfg.ShakyGyroZero[i] = gyroZero[i];
for (i = 0; i < 2; i++) cfg.angleTrim[i] = 0.0f;
cfg.ShakyDataAvail = 1;
cfg.acc_calibrated = 1;
writeParams(1);
systemReset(false);
}
void osdExitMenu(void *ptr)
{
max7456ClearScreen();
max7456Write(5, 3, "RESTARTING IMU...");
max7456RefreshAll();
stopMotors();
stopPwmAllMotors();
delay(200);
if (ptr) {
// save local variables to configuration
if (featureBlackbox)
featureSet(FEATURE_BLACKBOX);
else
featureClear(FEATURE_BLACKBOX);
if (featureLedstrip)
featureSet(FEATURE_LED_STRIP);
else
featureClear(FEATURE_LED_STRIP);
#if defined(VTX) || defined(USE_RTC6705)
if (featureVtx)
featureSet(FEATURE_VTX);
else
featureClear(FEATURE_VTX);
#endif // VTX || USE_RTC6705
#ifdef VTX
masterConfig.vtxBand = vtxBand;
masterConfig.vtx_channel = vtxChannel - 1;
#endif // VTX
#ifdef USE_RTC6705
masterConfig.vtx_channel = vtxBand * 8 + vtxChannel - 1;
#endif // USE_RTC6705
saveConfigAndNotify();
}
systemReset();
}
UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent)
{
m_config = new Ui_UploaderWidget();
m_config->setupUi(this);
currentStep = IAP_STATE_READY;
rescueStep = RESCUE_STEP0;
resetOnly=false;
dfu = NULL;
// Listen to autopilot connection events
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
TelemetryManager* telMngr = pm->getObject<TelemetryManager>();
connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect()));
connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect()));
// Note: remove listening to the connection manager, it overlaps with
// listening to the telemetry manager, we should only listen to one, not both.
// Also listen to disconnect actions from the user:
// Core::ConnectionManager *cm = Core::ICore::instance()->connectionManager();
// connect(cm, SIGNAL(deviceDisconnected()), this, SLOT(onAutopilotDisconnect()));
connect(m_config->haltButton, SIGNAL(clicked()), this, SLOT(goToBootloader()));
connect(m_config->resetButton, SIGNAL(clicked()), this, SLOT(systemReset()));
connect(m_config->bootButton, SIGNAL(clicked()), this, SLOT(systemBoot()));
connect(m_config->rescueButton, SIGNAL(clicked()), this, SLOT(systemRescue()));
getSerialPorts();
QIcon rbi;
rbi.addFile(QString(":uploader/images/view-refresh.svg"));
m_config->refreshPorts->setIcon(rbi);
connect(m_config->refreshPorts, SIGNAL(clicked()), this, SLOT(getSerialPorts()));
}
int main() {
systemReset(); // peripherals but not PC
boardInit();
strobePin(LED_BANK,LED,STARTUP_BLINKS,BLINK_FAST);
pRCC->APB1ENR |= 0x00800000;
usbAppInit();
setupFLASH();
#ifdef CONFIG_EXTRA_MAIN_CODE
CONFIG_EXTRA_MAIN_CODE
#endif
/* wait for host to upload program or halt bootloader */
while (bootloaderCondition) {
strobePin(LED_BANK,LED,1,BLINK_SLOW);
if (dfuUploadStarted()) {
dfuFinishUpload(); // systemHardReset from DFU once done
}
#ifdef bootloaderExitCondition
if (bootloaderExitCondition)
break;
#endif
}
if (checkUserCode(USER_CODE_RAM)) {
jumpToUser(USER_CODE_RAM);
} else if (checkUserCode(USER_CODE_FLASH)) {
jumpToUser(USER_CODE_FLASH);
} else {
// some sort of fault occurred, hard reset
strobePin(LED_BANK,LED,5,BLINK_FAST);
systemHardReset();
}
return 0;
}
void receiverCLI()
{
char rcOrderString[13];
float tempFloat;
uint8_t tempChannels = 0;
uint16_t tempMax = 0;
uint16_t tempMin = 0;
uint8_t tempPin = 0;
uint8_t tempWarn = 0;
uint8_t index;
uint8_t receiverQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering Receiver CLI....\n\n");
while(true)
{
cliPortPrint("Receiver CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
receiverQuery = cliPortRead();
cliPortPrint("\n");
switch(receiverQuery)
{
///////////////////////////
case 'a': // Receiver Configuration
cliPortPrint("\nReceiver Type: ");
if (eepromConfig.receiverType == PPM)
cliPortPrint(" PPM\n");
else if (eepromConfig.receiverType == PWM)
cliPortPrint(" PWM\n");
else if (eepromConfig.receiverType == SPEKTRUM)
cliPortPrint("Spektrum\n");
else
cliPortPrint("Error...\n");
if (eepromConfig.receiverType == PPM)
tempChannels = eepromConfig.ppmChannels;
else
tempChannels = 8;
for (index = 0; index < tempChannels; index++)
rcOrderString[eepromConfig.rcMap[index]] = rcChannelLetters[index];
rcOrderString[index] = '\0';
cliPortPrintF("Current RC Channel Assignment: %12s\n", rcOrderString);
if (eepromConfig.receiverType == PPM)
cliPortPrintF("Number of serial PWM channels %2d\n", eepromConfig.ppmChannels);
cliPortPrintF("Mid Command: %4ld\n", (uint16_t)eepromConfig.midCommand);
cliPortPrintF("Min Check: %4ld\n", (uint16_t)eepromConfig.minCheck);
cliPortPrintF("Max Check: %4ld\n", (uint16_t)eepromConfig.maxCheck);
cliPortPrintF("Min Throttle: %4ld\n", (uint16_t)eepromConfig.minThrottle);
cliPortPrintF("Max Thottle: %4ld\n\n", (uint16_t)eepromConfig.maxThrottle);
tempFloat = eepromConfig.rollAndPitchRateScaling * 180000.0 / PI;
cliPortPrintF("Max Roll and Pitch Rate Cmd: %6.2f DPS\n", tempFloat);
tempFloat = eepromConfig.yawRateScaling * 180000.0 / PI;
cliPortPrintF("Max Yaw Rate Cmd: %6.2f DPS\n", tempFloat);
tempFloat = eepromConfig.attitudeScaling * 180000.0 / PI;
cliPortPrintF("Max Attitude Cmd: %6.2f Degrees\n\n", tempFloat);
cliPortPrintF("Arm Delay Count: %3d Frames\n", eepromConfig.armCount);
cliPortPrintF("Disarm Delay Count: %3d Frames\n\n", eepromConfig.disarmCount);
cliPortPrintF("RSSI via PPM or ADC: %s", eepromConfig.rssiPPM ? "PPM\n" : "ADC\n");
if (eepromConfig.rssiPPM == true)
cliPortPrintF("RSSI PPM Channel: %1d\n", eepromConfig.rssiPin);
else
cliPortPrintF("RSSI ADC Pin: %1d\n", eepromConfig.rssiPin);
cliPortPrintF("RSSI Min: %4d\n", eepromConfig.rssiMin);
cliPortPrintF("RSSI Max: %4d\n", eepromConfig.rssiMax);
cliPortPrintF("RSSI Warning %%: %2d\n\n", eepromConfig.rssiWarning);
validQuery = false;
break;
///////////////////////////
case 'b': // Read Max Rate Values
eepromConfig.rollAndPitchRateScaling = readFloatCLI() / 180000.0f * PI;
eepromConfig.yawRateScaling = readFloatCLI() / 180000.0f * PI;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Read Max Attitude Value
eepromConfig.attitudeScaling = readFloatCLI() / 180000 * PI;
receiverQuery = 'a';
validQuery = true;
break;
case 'r': // Toggle RSSI between ADC and PPM
eepromConfig.rssiPPM = !eepromConfig.rssiPPM;
if (eepromConfig.rssiPPM)
{ // automatically adjust the settings
eepromConfig.rssiPin = 9;
eepromConfig.rssiMin = eepromConfig.minCheck;
eepromConfig.rssiMax = eepromConfig.maxCheck;
}
else
{
eepromConfig.rssiPin = 5; // default from config.c
eepromConfig.rssiMin = 10;
eepromConfig.rssiMax = 3450;
}
eepromConfig.rssiWarning = 25;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Receiver CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Read RX Input Type
eepromConfig.receiverType = (uint8_t)readFloatCLI();
cliPortPrint( "\nReceiver Type Changed....\n");
cliPortPrint("\nSystem Resetting....\n");
delay(100);
writeEEPROM();
systemReset(false);
break;
///////////////////////////
case 'B': // Read RC Control Order
readStringCLI( rcOrderString, 12 );
parseRcChannels( rcOrderString );
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 'G': // Read number of PPM Channels
tempChannels = (uint8_t)readFloatCLI();
if ((tempChannels < 8) || (tempChannels > 12))
{
cliPortPrintF("\nValid number of channels are 8 to 12\n");
cliPortPrintF("You entered %2d\n\n", tempChannels);
}
else
eepromConfig.ppmChannels = tempChannels;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'R': // RSSI pin/min/max/warning
tempPin = readFloatCLI();
tempMin = readFloatCLI();
tempMax = readFloatCLI();
tempWarn = readFloatCLI();
if (eepromConfig.rssiPPM)
{
if ((tempPin < 0) || (tempPin > eepromConfig.ppmChannels))
{
cliPortPrintF("Invalid RSSI PPM channel number, valid numbers are 0-%2d\n", eepromConfig.ppmChannels);
cliPortPrintF("You entered %2d, please try again\n", tempPin);
receiverQuery = '?';
validQuery = false;
break;
}
}
else
{
if ((tempPin < 1) || (tempPin > 6))
{
cliPortPrintF("Invalid RSSI Pin number, valid numbers are 1-6\n");
cliPortPrintF("You entered %2d, please try again\n", tempPin);
receiverQuery = '?';
validQuery = false;
break;
}
}
eepromConfig.rssiPin = tempPin;
eepromConfig.rssiMin = tempMin;
eepromConfig.rssiMax = tempMax;
eepromConfig.rssiWarning = tempWarn;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPortPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPortPrint("\n");
cliPortPrint("'a' Receiver Configuration Data 'A' Set RX Input Type AX, 0=PPM, 1=PWM, 2=Spektrum\n");
cliPortPrint("'b' Set Maximum Rate Commands 'B' Set RC Control Order BTAER12345678\n");
cliPortPrint("'c' Set Maximum Attitude Command\n");
cliPortPrint(" 'E' Set RC Control Points EmidCmd;minChk;maxChk;minThrot;maxThrot\n");
cliPortPrint(" 'F' Set Arm/Disarm Counts FarmCount;disarmCount\n");
cliPortPrint(" 'G' Set number of serial PWM channels GnumChannels\n");
cliPortPrint("'r' Toggle RSSI between PPM/ADC 'R' Set RSSI Config RPin;Min;Max;Warning\n");
cliPortPrint(" 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Exit Receiver CLI '?' Command Summary\n\n");
break;
///////////////////////////
}
}
}
void reset(void)
{
systemReset();
}
void init(void)
{
#ifdef USE_HAL_DRIVER
HAL_Init();
#endif
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
systemInit();
//i2cSetOverclock(masterConfig.i2c_overclock);
// initialize IO (needed for all IO operations)
IOInitGlobal();
debugMode = masterConfig.debug_mode;
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
#ifdef ALIENFLIGHTF3
ledInit(hardwareRevision == AFF3_REV_1 ? false : true);
#else
ledInit(false);
#endif
LED2_ON;
#ifdef USE_EXTI
EXTIInit();
#endif
#if defined(BUTTONS)
gpio_config_t buttonAGpioConfig = {
BUTTON_A_PIN,
Mode_IPU,
Speed_2MHz
};
gpioInit(BUTTON_A_PORT, &buttonAGpioConfig);
gpio_config_t buttonBGpioConfig = {
BUTTON_B_PIN,
Mode_IPU,
Speed_2MHz
};
gpioInit(BUTTON_B_PORT, &buttonBGpioConfig);
// Check status of bind plug and exit if not active
delayMicroseconds(10); // allow GPIO configuration to settle
if (!isMPUSoftReset()) {
uint8_t secondsRemaining = 5;
bool bothButtonsHeld;
do {
bothButtonsHeld = !digitalIn(BUTTON_A_PORT, BUTTON_A_PIN) && !digitalIn(BUTTON_B_PORT, BUTTON_B_PIN);
if (bothButtonsHeld) {
if (--secondsRemaining == 0) {
resetEEPROM();
systemReset();
}
delay(1000);
LED0_TOGGLE;
}
} while (bothButtonsHeld);
}
#endif
#ifdef SPEKTRUM_BIND
if (feature(FEATURE_RX_SERIAL)) {
switch (masterConfig.rxConfig.serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
// Spektrum satellite binding if enabled on startup.
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
// The rest of Spektrum initialization will happen later - via spektrumInit()
spektrumBind(&masterConfig.rxConfig);
break;
}
}
#endif
delay(100);
timerInit(); // timer must be initialized before any channel is allocated
#if !defined(USE_HAL_DRIVER)
dmaInit();
#endif
#if defined(AVOID_UART1_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART2_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART3_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
#else
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
#endif
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
#ifdef USE_SERVOS
servoMixerInit(masterConfig.customServoMixer);
#endif
uint16_t idlePulse = masterConfig.motorConfig.mincommand;
if (feature(FEATURE_3D)) {
idlePulse = masterConfig.flight3DConfig.neutral3d;
}
if (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED) {
featureClear(FEATURE_3D);
idlePulse = 0; // brushed motors
}
#ifdef USE_QUAD_MIXER_ONLY
motorInit(&masterConfig.motorConfig, idlePulse, QUAD_MOTOR_COUNT);
#else
motorInit(&masterConfig.motorConfig, idlePulse, mixers[masterConfig.mixerMode].motorCount);
#endif
#ifdef USE_SERVOS
if (isMixerUsingServos()) {
//pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
servoInit(&masterConfig.servoConfig);
}
#endif
#ifndef SKIP_RX_PWM_PPM
if (feature(FEATURE_RX_PPM)) {
ppmRxInit(&masterConfig.ppmConfig, masterConfig.motorConfig.motorPwmProtocol);
} else if (feature(FEATURE_RX_PARALLEL_PWM)) {
pwmRxInit(&masterConfig.pwmConfig);
}
pwmRxSetInputFilteringMode(masterConfig.inputFilteringMode);
#endif
mixerConfigureOutput();
#ifdef USE_SERVOS
servoConfigureOutput();
#endif
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
beeperInit(&masterConfig.beeperConfig);
#endif
/* temp until PGs are implemented. */
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_BST
bstInit(BST_DEVICE);
#endif
#ifdef USE_SPI
#ifdef USE_SPI_DEVICE_1
spiInit(SPIDEV_1);
#endif
#ifdef USE_SPI_DEVICE_2
spiInit(SPIDEV_2);
#endif
#ifdef USE_SPI_DEVICE_3
#ifdef ALIENFLIGHTF3
if (hardwareRevision == AFF3_REV_2) {
spiInit(SPIDEV_3);
}
#else
spiInit(SPIDEV_3);
#endif
#endif
#ifdef USE_SPI_DEVICE_4
spiInit(SPIDEV_4);
#endif
#endif
#ifdef VTX
vtxInit();
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
#if defined(NAZE)
if (hardwareRevision == NAZE32_SP) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
} else {
serialRemovePort(SERIAL_PORT_USART3);
}
#endif
#if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
}
#endif
#if defined(SPRACINGF3MINI) || defined(OMNIBUS) || defined(X_RACERSPI)
#if defined(SONAR) && defined(USE_SOFTSERIAL1)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
}
#endif
#endif
#ifdef USE_I2C
#if defined(NAZE)
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
} else {
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
}
#elif defined(CC3D)
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
#else
i2cInit(I2C_DEVICE);
#endif
#endif
#ifdef USE_ADC
drv_adc_config_t adc_params;
adc_params.enableVBat = feature(FEATURE_VBAT);
adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
adc_params.enableExternal1 = false;
#ifdef OLIMEXINO
adc_params.enableExternal1 = true;
#endif
#ifdef NAZE
// optional ADC5 input on rev.5 hardware
adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5);
#endif
adcInit(&adc_params);
#endif
initBoardAlignment(&masterConfig.boardAlignment);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
displayInit(&masterConfig.rxConfig);
}
#endif
#ifdef USE_RTC6705
if (feature(FEATURE_VTX)) {
rtc6705_soft_spi_init();
current_vtx_channel = masterConfig.vtx_channel;
rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power);
}
#endif
#ifdef OSD
if (feature(FEATURE_OSD)) {
osdInit();
}
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
masterConfig.acc_hardware,
masterConfig.mag_hardware,
masterConfig.baro_hardware,
masterConfig.mag_declination,
masterConfig.gyro_lpf,
masterConfig.gyro_sync_denom)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
LED1_ON;
LED0_OFF;
LED2_OFF;
for (int i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
imuInit();
mspFcInit();
mspSerialInit();
#ifdef USE_CLI
cliInit(&masterConfig.serialConfig);
#endif
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rxInit(&masterConfig.rxConfig, masterConfig.modeActivationConditions);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
navigationInit(
&masterConfig.gpsProfile,
¤tProfile->pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(&masterConfig.sonarConfig);
}
#endif
#ifdef LED_STRIP
ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors);
if (feature(FEATURE_LED_STRIP)) {
ledStripEnable();
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
telemetryInit();
}
#endif
#ifdef USB_CABLE_DETECTION
usbCableDetectInit();
#endif
#ifdef TRANSPONDER
if (feature(FEATURE_TRANSPONDER)) {
transponderInit(masterConfig.transponderData);
transponderEnable();
transponderStartRepeating();
systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
}
#endif
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init(IO_TAG_NONE);
}
#elif defined(USE_FLASH_M25P16)
m25p16_init(IO_TAG_NONE);
#endif
flashfsInit();
#endif
#ifdef USE_SDCARD
bool sdcardUseDMA = false;
sdcardInsertionDetectInit();
#ifdef SDCARD_DMA_CHANNEL_TX
#if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL)
// Ensure the SPI Tx DMA doesn't overlap with the led strip
#if defined(STM32F4) || defined(STM32F7)
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_STREAM;
#else
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
#endif
#else
sdcardUseDMA = true;
#endif
#endif
sdcard_init(sdcardUseDMA);
afatfs_init();
#endif
if (masterConfig.gyro_lpf > 0 && masterConfig.gyro_lpf < 7) {
masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
masterConfig.gyro_sync_denom = 1;
}
setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime
#ifdef BLACKBOX
initBlackbox();
#endif
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles();
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
// start all timers
// TODO - not implemented yet
timerStart();
ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
if (!loopbackPort->vTable) {
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
}
serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif
// Now that everything has powered up the voltage and cell count be determined.
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
batteryInit(&masterConfig.batteryConfig);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
displayShowFixedPage(PAGE_GPS);
#else
displayResetPageCycling();
displayEnablePageCycling();
#endif
}
#endif
#ifdef CJMCU
LED2_ON;
#endif
// Latch active features AGAIN since some may be modified by init().
latchActiveFeatures();
motorControlEnable = true;
fcTasksInit();
systemState |= SYSTEM_STATE_READY;
}
byte systemExecuteCmdstring(String& cmdString){
/*
This function processes the $ system commands
This is taken heavily from grbl. https://github.com/grbl/grbl
*/
byte char_counter = 1;
// byte helper_var = 0; // Helper variable
float parameter, value;
if (cmdString.length() == 1){
reportMaslowHelp();
}
else {
switch( cmdString[char_counter] ) {
case '$': // case 'G': case 'C': case 'X':
if ( cmdString.length() > 2 ) { return(STATUS_INVALID_STATEMENT); }
switch( cmdString[char_counter] ) {
case '$' : // Prints Maslow settings
// if ( sys.state & (STATE_CYCLE | STATE_HOLD) ) { return(STATUS_IDLE_ERROR); } // Block during cycle. Takes too long to print.
// else {
reportMaslowSettings();
// }
break;
// case 'G' : // Prints gcode parser state
// report_gcode_modes();
// break;
// case 'C' : // Set check g-code mode [IDLE/CHECK]
// // Perform reset when toggling off. Check g-code mode should only work if Grbl
// // is idle and ready, regardless of alarm locks. This is mainly to keep things
// // simple and consistent.
// if ( sys.state == STATE_CHECK_MODE ) {
// mc_reset();
// report_feedback_message(MESSAGE_DISABLED);
// } else {
// if (sys.state) { return(STATUS_IDLE_ERROR); } // Requires no alarm mode.
// sys.state = STATE_CHECK_MODE;
// report_feedback_message(MESSAGE_ENABLED);
// }
// break;
// case 'X' : // Disable alarm lock [ALARM]
// if (sys.state == STATE_ALARM) {
// report_feedback_message(MESSAGE_ALARM_UNLOCK);
// sys.state = STATE_IDLE;
// // Don't run startup script. Prevents stored moves in startup from causing accidents.
// if (system_check_safety_door_ajar()) { // Check safety door switch before returning.
// bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR);
// protocol_execute_realtime(); // Enter safety door mode.
// }
// } // Otherwise, no effect.
// break;
}
break;
//case 'J' : break; // Jogging methods
// TODO: Here jogging can be placed for execution as a seperate subprogram. It does not need to be
// susceptible to other realtime commands except for e-stop. The jogging function is intended to
// be a basic toggle on/off with controlled acceleration and deceleration to prevent skipped
// steps. The user would supply the desired feedrate, axis to move, and direction. Toggle on would
// start motion and toggle off would initiate a deceleration to stop. One could 'feather' the
// motion by repeatedly toggling to slow the motion to the desired location. Location data would
// need to be updated real-time and supplied to the user through status queries.
// More controlled exact motions can be taken care of by inputting G0 or G1 commands, which are
// handled by the planner. It would be possible for the jog subprogram to insert blocks into the
// block buffer without having the planner plan them. It would need to manage de/ac-celerations
// on its own carefully. This approach could be effective and possibly size/memory efficient.
// break;
// }
//break;
default :
// Block any system command that requires the state as IDLE/ALARM. (i.e. EEPROM, homing)
// if ( !(sys.state == STATE_IDLE || sys.state == STATE_ALARM) ) { return(STATUS_IDLE_ERROR); }
switch( cmdString[char_counter] ) {
// case '#' : // Print Grbl NGC parameters
// if ( line[++char_counter] != 0 ) { return(STATUS_INVALID_STATEMENT); }
// else { report_ngc_parameters(); }
// break;
// case 'H' : // Perform homing cycle [IDLE/ALARM]
// if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) {
// sys.state = STATE_HOMING; // Set system state variable
// // Only perform homing if Grbl is idle or lost.
//
// // TODO: Likely not required.
// if (system_check_safety_door_ajar()) { // Check safety door switch before homing.
// bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR);
// protocol_execute_realtime(); // Enter safety door mode.
// }
//
//
// mc_homing_cycle();
// if (!sys.abort) { // Execute startup scripts after successful homing.
// sys.state = STATE_IDLE; // Set to IDLE when complete.
// st_go_idle(); // Set steppers to the settings idle state before returning.
// system_execute_startup(line);
// }
// } else { return(STATUS_SETTING_DISABLED); }
// break;
// case 'I' : // Print or store build info. [IDLE/ALARM]
// if ( line[++char_counter] == 0 ) {
// settings_read_build_info(line);
// report_build_info(line);
// } else { // Store startup line [IDLE/ALARM]
// if(line[char_counter++] != '=') { return(STATUS_INVALID_STATEMENT); }
// helper_var = char_counter; // Set helper variable as counter to start of user info line.
// do {
// line[char_counter-helper_var] = line[char_counter];
// } while (line[char_counter++] != 0);
// settings_store_build_info(line);
// }
// break;
case 'R' : // Restore defaults [IDLE/ALARM]
if (cmdString[++char_counter] != 'S') { return(STATUS_INVALID_STATEMENT); }
if (cmdString[++char_counter] != 'T') { return(STATUS_INVALID_STATEMENT); }
if (cmdString[++char_counter] != '=') { return(STATUS_INVALID_STATEMENT); }
if (cmdString.length() != 6) { return(STATUS_INVALID_STATEMENT); }
switch (cmdString[++char_counter]) {
case '$': settingsWipe(SETTINGS_RESTORE_SETTINGS); break;
case '#': settingsWipe(SETTINGS_RESTORE_MASLOW); break;
case '*': settingsWipe(SETTINGS_RESTORE_ALL); break;
default: return(STATUS_INVALID_STATEMENT);
}
reportFeedbackMessage(MESSAGE_RESTORE_DEFAULTS);
systemReset(); // Force reset to ensure settings are initialized correctly.
break;
// case 'N' : // Startup lines. [IDLE/ALARM]
// if ( line[++char_counter] == 0 ) { // Print startup lines
// for (helper_var=0; helper_var < N_STARTUP_LINE; helper_var++) {
// if (!(settings_read_startup_line(helper_var, line))) {
// report_status_message(STATUS_SETTING_READ_FAIL);
// } else {
// report_startup_line(helper_var,line);
// }
// }
// break;
// } else { // Store startup line [IDLE Only] Prevents motion during ALARM.
// if (sys.state != STATE_IDLE) { return(STATUS_IDLE_ERROR); } // Store only when idle.
// helper_var = true; // Set helper_var to flag storing method.
// // No break. Continues into default: to read remaining command characters.
// }
default : // Storing setting methods [IDLE/ALARM]
if(!readFloat(cmdString, char_counter, parameter)) { return(STATUS_BAD_NUMBER_FORMAT); }
if(cmdString[char_counter++] != '=') { return(STATUS_INVALID_STATEMENT); }
// if (helper_var) { // Store startup line
// // Prepare sending gcode block to gcode parser by shifting all characters
// helper_var = char_counter; // Set helper variable as counter to start of gcode block
// do {
// line[char_counter-helper_var] = line[char_counter];
// } while (line[char_counter++] != 0);
// // Execute gcode block to ensure block is valid.
// helper_var = gc_execute_line(line); // Set helper_var to returned status code.
// if (helper_var) { return(helper_var); }
// else {
// helper_var = trunc(parameter); // Set helper_var to int value of parameter
// settings_store_startup_line(helper_var,line);
// }
// } else { // Store global setting.
if(!readFloat(cmdString, char_counter, value)) { return(STATUS_BAD_NUMBER_FORMAT); }
if((cmdString[char_counter] != 0) || (parameter > 255)) { return(STATUS_INVALID_STATEMENT); }
return(settingsStoreGlobalSetting((byte)parameter, value));
// }
}
}
}
return(STATUS_OK);
}
/**
* Parse data from the input stream.
* @param inputData A single byte to be added to the parser.
*/
void FirmataParser::parse(uint8_t inputData)
{
uint8_t command;
if (parsingSysex) {
if (inputData == END_SYSEX) {
//stop sysex byte
parsingSysex = false;
//fire off handler function
processSysexMessage();
} else {
//normal data byte - add to buffer
storedInputData[sysexBytesRead] = inputData;
sysexBytesRead++;
}
} else if ( (waitForData > 0) && (inputData < 128) ) {
waitForData--;
storedInputData[waitForData] = inputData;
if ( (waitForData == 0) && executeMultiByteCommand ) { // got the whole message
switch (executeMultiByteCommand) {
case ANALOG_MESSAGE:
if (currentAnalogCallback) {
(*currentAnalogCallback)(multiByteChannel,
(storedInputData[0] << 7)
+ storedInputData[1]);
}
break;
case DIGITAL_MESSAGE:
if (currentDigitalCallback) {
(*currentDigitalCallback)(multiByteChannel,
(storedInputData[0] << 7)
+ storedInputData[1]);
}
break;
case SET_PIN_MODE:
if (currentPinModeCallback)
(*currentPinModeCallback)(storedInputData[1], storedInputData[0]);
break;
case SET_DIGITAL_PIN_VALUE:
if (currentPinValueCallback)
(*currentPinValueCallback)(storedInputData[1], storedInputData[0]);
break;
case REPORT_ANALOG:
if (currentReportAnalogCallback)
(*currentReportAnalogCallback)(multiByteChannel, storedInputData[0]);
break;
case REPORT_DIGITAL:
if (currentReportDigitalCallback)
(*currentReportDigitalCallback)(multiByteChannel, storedInputData[0]);
break;
}
executeMultiByteCommand = 0;
}
} else {
// remove channel info from command byte if less than 0xF0
if (inputData < 0xF0) {
command = inputData & 0xF0;
multiByteChannel = inputData & 0x0F;
} else {
command = inputData;
// commands in the 0xF* range don't use channel data
}
switch (command) {
case ANALOG_MESSAGE:
case DIGITAL_MESSAGE:
case SET_PIN_MODE:
case SET_DIGITAL_PIN_VALUE:
waitForData = 2; // two data bytes needed
executeMultiByteCommand = command;
break;
case REPORT_ANALOG:
case REPORT_DIGITAL:
waitForData = 1; // one data byte needed
executeMultiByteCommand = command;
break;
case START_SYSEX:
parsingSysex = true;
sysexBytesRead = 0;
break;
case SYSTEM_RESET:
systemReset();
break;
case REPORT_VERSION:
if (currentReportVersionCallback)
(*currentReportVersionCallback)();
break;
}
}
}
void cliCom(void)
{
uint16_t index;
char mvlkToggleString[5] = { 0, 0, 0, 0, 0 };
if ((cliPortAvailable() && !validCliCommand))
{
cliQuery = cliPortRead();
if (cliQuery == '#') // Check to see if we should toggle mavlink msg state
{
while (cliPortAvailable == false);
readStringCLI(mvlkToggleString, 5);
if ((mvlkToggleString[0] == '#') &&
(mvlkToggleString[1] == '#') &&
(mvlkToggleString[2] == '#') &&
(mvlkToggleString[3] == '#'))
{
if (systemConfig.mavlinkEnabled == false)
{
systemConfig.mavlinkEnabled = true;
systemConfig.activeTelemetry = 0x0000;
}
else
{
systemConfig.mavlinkEnabled = false;
}
if (mvlkToggleString[4] == 'W')
{
cliPortPrint("\nWriting EEPROM Parameters....\n");
writeSystemEEPROM();
}
}
}
}
validCliCommand = false;
if ((systemConfig.mavlinkEnabled == false) && (cliQuery != '#'))
{
switch (cliQuery)
{
///////////////////////////////
case 'a': // Rate PIDs
cliPortPrintF("\nRoll Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[ROLL_RATE_PID].P,
systemConfig.PID[ROLL_RATE_PID].I,
systemConfig.PID[ROLL_RATE_PID].D,
systemConfig.PID[ROLL_RATE_PID].N);
cliPortPrintF( "Pitch Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[PITCH_RATE_PID].P,
systemConfig.PID[PITCH_RATE_PID].I,
systemConfig.PID[PITCH_RATE_PID].D,
systemConfig.PID[PITCH_RATE_PID].N);
cliPortPrintF( "Yaw Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[YAW_RATE_PID].P,
systemConfig.PID[YAW_RATE_PID].I,
systemConfig.PID[YAW_RATE_PID].D,
systemConfig.PID[YAW_RATE_PID].N);
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'b': // Attitude PIDs
cliPortPrintF("\nRoll Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[ROLL_ATT_PID].P,
systemConfig.PID[ROLL_ATT_PID].I,
systemConfig.PID[ROLL_ATT_PID].D,
systemConfig.PID[ROLL_ATT_PID].N);
cliPortPrintF( "Pitch Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[PITCH_ATT_PID].P,
systemConfig.PID[PITCH_ATT_PID].I,
systemConfig.PID[PITCH_ATT_PID].D,
systemConfig.PID[PITCH_ATT_PID].N);
cliPortPrintF( "Heading PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[HEADING_PID].P,
systemConfig.PID[HEADING_PID].I,
systemConfig.PID[HEADING_PID].D,
systemConfig.PID[HEADING_PID].N);
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'c': // Velocity PIDs
cliPortPrintF("\nnDot PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[NDOT_PID].P,
systemConfig.PID[NDOT_PID].I,
systemConfig.PID[NDOT_PID].D,
systemConfig.PID[NDOT_PID].N);
cliPortPrintF( "eDot PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[EDOT_PID].P,
systemConfig.PID[EDOT_PID].I,
systemConfig.PID[EDOT_PID].D,
systemConfig.PID[EDOT_PID].N);
cliPortPrintF( "hDot PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[HDOT_PID].P,
systemConfig.PID[HDOT_PID].I,
systemConfig.PID[HDOT_PID].D,
systemConfig.PID[HDOT_PID].N);
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'd': // Position PIDs
cliPortPrintF("\nN PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[N_PID].P,
systemConfig.PID[N_PID].I,
systemConfig.PID[N_PID].D,
systemConfig.PID[N_PID].N);
cliPortPrintF( "E PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[E_PID].P,
systemConfig.PID[E_PID].I,
systemConfig.PID[E_PID].D,
systemConfig.PID[E_PID].N);
cliPortPrintF( "h PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[H_PID].P,
systemConfig.PID[H_PID].I,
systemConfig.PID[H_PID].D,
systemConfig.PID[H_PID].N);
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'e': // Loop Delta Times
cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", deltaTime1000Hz,
deltaTime500Hz,
deltaTime100Hz,
deltaTime50Hz,
deltaTime10Hz,
deltaTime5Hz,
deltaTime1Hz);
validCliCommand = false;
break;
///////////////////////////////
case 'f': // Loop Execution Times
cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", executionTime1000Hz,
executionTime500Hz,
executionTime100Hz,
executionTime50Hz,
executionTime10Hz,
executionTime5Hz,
executionTime1Hz);
validCliCommand = false;
break;
///////////////////////////////
case 'g': // 500 Hz Accels
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS],
sensors.accel500Hz[YAXIS],
sensors.accel500Hz[ZAXIS]);
validCliCommand = false;
break;
///////////////////////////////
case 'h': // 100 hz Earth Axis Accels
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", earthAxisAccels[XAXIS],
earthAxisAccels[YAXIS],
earthAxisAccels[ZAXIS]);
validCliCommand = false;
break;
///////////////////////////////
case 'i': // 500 hz Gyros
cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ] * R2D,
sensors.gyro500Hz[PITCH] * R2D,
sensors.gyro500Hz[YAW ] * R2D,
mpu6000Temperature);
validCliCommand = false;
break;
///////////////////////////////
case 'j': // 10 Hz Mag Data
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.mag10Hz[XAXIS],
sensors.mag10Hz[YAXIS],
sensors.mag10Hz[ZAXIS]);
validCliCommand = false;
break;
///////////////////////////////
case 'k': // Vertical Axis Variables
cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %4ld, %9.4f\n", earthAxisAccels[ZAXIS],
sensors.pressureAlt50Hz,
hDotEstimate,
hEstimate,
ms5611Temperature,
aglRead());
validCliCommand = false;
break;
///////////////////////////////
case 'l': // Attitudes
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ] * R2D,
sensors.attitude500Hz[PITCH] * R2D,
sensors.attitude500Hz[YAW ] * R2D);
validCliCommand = false;
break;
///////////////////////////////
case 'm': // Axis PIDs
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", ratePID[ROLL ],
ratePID[PITCH],
ratePID[YAW ]);
validCliCommand = false;
break;
///////////////////////////////
case 'n': // GPS Data
switch (gpsDataType)
{
///////////////////////
case 0:
cliPortPrintF("%12ld, %12ld, %12ld, %12ld, %12ld, %12ld, %4d, %4d\n", gps.latitude,
gps.longitude,
gps.hMSL,
gps.velN,
gps.velE,
gps.velD,
gps.fix,
gps.numSats);
break;
///////////////////////
case 1:
cliPortPrintF("%3d: ", gps.numCh);
for (index = 0; index < gps.numCh; index++)
cliPortPrintF("%3d ", gps.chn[index]);
cliPortPrint("\n");
break;
///////////////////////
case 2:
cliPortPrintF("%3d: ", gps.numCh);
for (index = 0; index < gps.numCh; index++)
cliPortPrintF("%3d ", gps.svid[index]);
cliPortPrint("\n");
break;
///////////////////////
case 3:
cliPortPrintF("%3d: ", gps.numCh);
for (index = 0; index < gps.numCh; index++)
cliPortPrintF("%3d ", gps.cno[index]);
cliPortPrint("\n");
break;
///////////////////////
}
validCliCommand = false;
break;
///////////////////////////////
case 'o':
cliPortPrintF("%9.4f\n", batteryVoltage);
validCliCommand = false;
break;
///////////////////////////////
case 'p': // Primary Spektrum Raw Data
cliPortPrintF("%04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X\n", primarySpektrumState.lostFrameCnt,
primarySpektrumState.rcAvailable,
primarySpektrumState.values[0],
primarySpektrumState.values[1],
primarySpektrumState.values[2],
primarySpektrumState.values[3],
primarySpektrumState.values[4],
primarySpektrumState.values[5],
primarySpektrumState.values[6],
primarySpektrumState.values[7]);
validCliCommand = false;
break;
///////////////////////////////
case 'q': // Not Used
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'r':
if (flightMode == RATE)
cliPortPrint("Flight Mode:RATE ");
else if (flightMode == ATTITUDE)
cliPortPrint("Flight Mode:ATTITUDE ");
else if (flightMode == GPS)
cliPortPrint("Flight Mode:GPS ");
if (headingHoldEngaged == true)
cliPortPrint("Heading Hold:ENGAGED ");
else
cliPortPrint("Heading Hold:DISENGAGED ");
switch (verticalModeState)
{
case ALT_DISENGAGED_THROTTLE_ACTIVE:
cliPortPrint("Alt:Disenaged Throttle Active ");
break;
case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT:
cliPortPrint("Alt:Hold Fixed at Engagement Alt ");
break;
case ALT_HOLD_AT_REFERENCE_ALTITUDE:
cliPortPrint("Alt:Hold at Reference Alt ");
break;
case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY:
cliPortPrint("Alt:Velocity Hold at Reference Vel ");
break;
case ALT_DISENGAGED_THROTTLE_INACTIVE:
cliPortPrint("Alt:Disengaged Throttle Inactive ");
break;
}
if (rxCommand[AUX3] > MIDCOMMAND)
cliPortPrint("Mode:Simple ");
else
cliPortPrint("Mode:Normal ");
if (rxCommand[AUX4] > MIDCOMMAND)
cliPortPrint("Emergency Bail:Active\n");
else
cliPortPrint("Emergency Bail:Inactive\n");
validCliCommand = false;
break;
///////////////////////////////
case 's': // Raw Receiver Commands
if ((systemConfig.receiverType == SPEKTRUM) && (maxChannelNum > 0))
{
for (index = 0; index < maxChannelNum - 1; index++)
cliPortPrintF("%4ld, ", spektrumBuf[index]);
cliPortPrintF("%4ld\n", spektrumBuf[maxChannelNum - 1]);
}
else if ((systemConfig.receiverType == SPEKTRUM) && (maxChannelNum == 0))
cliPortPrint("Invalid Number of Spektrum Channels....\n");
else
{
for (index = 0; index < 7; index++)
cliPortPrintF("%4i, ", ppmRxRead(index));
cliPortPrintF("%4i\n", ppmRxRead(7));
}
validCliCommand = false;
break;
///////////////////////////////
case 't': // Processed Receiver Commands
for (index = 0; index < 7; index++)
cliPortPrintF("%8.2f, ", rxCommand[index]);
cliPortPrintF("%8.2f\n", rxCommand[7]);
validCliCommand = false;
break;
///////////////////////////////
case 'u': // Command in Detent Discretes
cliPortPrintF("%s, ", commandInDetent[ROLL ] ? " true" : "false");
cliPortPrintF("%s, ", commandInDetent[PITCH] ? " true" : "false");
cliPortPrintF("%s\n", commandInDetent[YAW ] ? " true" : "false");
validCliCommand = false;
break;
///////////////////////////////
case 'v': // ESC PWM Outputs
cliPortPrintF("%4ld, ", TIM2->CCR1 );
cliPortPrintF("%4ld, ", TIM2->CCR2 );
cliPortPrintF("%4ld, ", TIM15->CCR1);
cliPortPrintF("%4ld, ", TIM15->CCR2);
cliPortPrintF("%4ld, ", TIM3->CCR1 );
cliPortPrintF("%4ld\n", TIM3->CCR2 );
validCliCommand = false;
break;
///////////////////////////////
case 'w': // Servo PWM Outputs
cliPortPrintF("%4ld, ", TIM4->CCR1);
cliPortPrintF("%4ld, ", TIM4->CCR2);
cliPortPrintF("%4ld, ", TIM4->CCR3);
cliPortPrintF("%4ld\n", TIM4->CCR4);
validCliCommand = false;
break;
///////////////////////////////
case 'x':
validCliCommand = false;
break;
///////////////////////////////
case 'y': // ESC Calibration
escCalibration();
cliQuery = 'x';
break;
///////////////////////////////
case 'z':
cliPortPrintF("%5.2f, %5.2f\n", voltageMonitor(),
adcChannel());
break;
///////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////
case 'A': // Read Roll Rate PID Values
readCliPID(ROLL_RATE_PID);
cliPortPrint( "\nRoll Rate PID Received....\n" );
cliQuery = 'a';
validCliCommand = false;
break;
///////////////////////////////
case 'B': // Read Pitch Rate PID Values
readCliPID(PITCH_RATE_PID);
cliPortPrint( "\nPitch Rate PID Received....\n" );
cliQuery = 'a';
validCliCommand = false;
break;
///////////////////////////////
case 'C': // Read Yaw Rate PID Values
readCliPID(YAW_RATE_PID);
cliPortPrint( "\nYaw Rate PID Received....\n" );
cliQuery = 'a';
validCliCommand = false;
break;
///////////////////////////////
case 'D': // Read Roll Attitude PID Values
readCliPID(ROLL_ATT_PID);
cliPortPrint( "\nRoll Attitude PID Received....\n" );
cliQuery = 'b';
validCliCommand = false;
break;
///////////////////////////////
case 'E': // Read Pitch Attitude PID Values
readCliPID(PITCH_ATT_PID);
cliPortPrint( "\nPitch Attitude PID Received....\n" );
cliQuery = 'b';
validCliCommand = false;
break;
///////////////////////////////
case 'F': // Read Heading Hold PID Values
readCliPID(HEADING_PID);
cliPortPrint( "\nHeading PID Received....\n" );
cliQuery = 'b';
validCliCommand = false;
break;
///////////////////////////////
case 'G': // Read nDot PID Values
readCliPID(NDOT_PID);
cliPortPrint( "\nnDot PID Received....\n" );
cliQuery = 'c';
validCliCommand = false;
break;
///////////////////////////////
case 'H': // Read eDot PID Values
readCliPID(EDOT_PID);
cliPortPrint( "\neDot PID Received....\n" );
cliQuery = 'c';
validCliCommand = false;
break;
///////////////////////////////
case 'I': // Read hDot PID Values
readCliPID(HDOT_PID);
cliPortPrint( "\nhDot PID Received....\n" );
cliQuery = 'c';
validCliCommand = false;
break;
///////////////////////////////
case 'J': // Read n PID Values
readCliPID(N_PID);
cliPortPrint( "\nn PID Received....\n" );
cliQuery = 'd';
validCliCommand = false;
break;
///////////////////////////////
case 'K': // Read e PID Values
readCliPID(E_PID);
cliPortPrint( "\ne PID Received....\n" );
cliQuery = 'd';
validCliCommand = false;
break;
///////////////////////////////
case 'L': // Read h PID Values
readCliPID(H_PID);
cliPortPrint( "\nh PID Received....\n" );
cliQuery = 'd';
validCliCommand = false;
break;
///////////////////////////////
case 'N': // Mixer CLI
mixerCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'O': // Receiver CLI
receiverCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'P': // Sensor CLI
sensorCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'Q': // GPS Data Selection
gpsDataType = (uint8_t)readFloatCLI();
cliPortPrint("\n");
cliQuery = 'n';
validCliCommand = false;
break;
///////////////////////////////
case 'R': // Reset to Bootloader
cliPortPrint("Entering Bootloader....\n\n");
delay(100);
systemReset(true);
break;
///////////////////////////////
case 'S': // Reset System
cliPortPrint("\nSystem Reseting....\n\n");
delay(100);
systemReset(false);
break;
///////////////////////////////
case 'T': // Telemetry CLI
telemetryCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'U': // EEPROM CLI
eepromCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'V': // Write Sensor EEPROM Parameters
cliPortPrint("\nWriting Sensor EEPROM Parameters....\n\n");
cliBusy = true;
writeSensorEEPROM();
cliBusy = false;
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'W': // Write System EEPROM Parameters
cliPortPrint("\nWriting System EEPROM Parameters....\n\n");
cliBusy = true;
writeSystemEEPROM();
cliBusy = false;
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'X': // Not Used
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'Y': // Not Used
computeGeoMagElements();
cliQuery = 'x';
break;
///////////////////////////////
case 'Z': // Not Used
if (usbIsConfigured() == true)
cliPortPrint("\nUSB Configured TRUE\n");
else
cliPortPrint("\nUSB Configured FALSE\n");
if (usbIsConnected() == true)
cliPortPrint("\nUSB Connected TRUE\n");
else
cliPortPrint("\nUSB Connected FALSE\n");
cliQuery = 'x';
break;
///////////////////////////////
case '?': // Command Summary
cliBusy = true;
cliPortPrint("\n");
cliPortPrint("'a' Rate PIDs 'A' Set Roll Rate PID Data AB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'b' Attitude PIDs 'B' Set Pitch Rate PID Data BB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'c' Velocity PIDs 'C' Set Yaw Rate PID Data CB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'d' Position PIDs 'D' Set Roll Att PID Data DB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'e' Loop Delta Times 'E' Set Pitch Att PID Data EB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'f' Loop Execution Times 'F' Set Hdg Hold PID Data FB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'g' 500 Hz Accels 'G' Set nDot PID Data GB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'h' 100 Hz Earth Axis Accels 'H' Set eDot PID Data HB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'i' 500 Hz Gyros 'I' Set hDot PID Data IB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'j' 10 hz Mag Data 'J' Set n PID Data JB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'k' Vertical Axis Variable 'K' Set e PID Data KB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'l' Attitudes 'L' Set h PID Data LB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("\n");
cliPortPrint("Press space bar for more, or enter a command....\n");
while (cliPortAvailable() == false);
cliQuery = cliPortRead();
if (cliQuery != ' ')
{
validCliCommand = true;
cliBusy = false;
return;
}
cliPortPrint("\n");
cliPortPrint("'m' Axis PIDs 'M' Not Used\n");
cliPortPrint("'n' GPS Data 'N' Mixer CLI\n");
cliPortPrint("'o' Battery Voltage 'O' Receiver CLI\n");
cliPortPrint("'p' Primary Spektrum Raw Data 'P' Sensor CLI\n");
cliPortPrint("'q' Not Used 'Q' GPS Data Selection\n");
cliPortPrint("'r' Mode States 'R' Reset and Enter Bootloader\n");
cliPortPrint("'s' Raw Receiver Commands 'S' Reset\n");
cliPortPrint("'t' Processed Receiver Commands 'T' Telemetry CLI\n");
cliPortPrint("'u' Command In Detent Discretes 'U' EEPROM CLI\n");
cliPortPrint("'v' Motor PWM Outputs 'V' Write Sensor EEPROM Parameters\n");
cliPortPrint("'w' Servo PWM Outputs 'W' Write System EEPROM Parameters\n");
cliPortPrint("'x' Terminate Serial Communication 'X' Not Used\n");
cliPortPrint("\n");
cliPortPrint("Press space bar for more, or enter a command....\n");
while (cliPortAvailable() == false);
cliQuery = cliPortRead();
if (cliQuery != ' ')
{
validCliCommand = true;
cliBusy = false;
return;
}
cliPortPrint("\n");
cliPortPrint("'y' ESC Calibration 'Y' Not Used\n");
cliPortPrint("'z' ADC Values 'Z' Not Used\n");
cliPortPrint(" '?' Command Summary\n");
cliPortPrint("\n");
cliQuery = 'x';
cliBusy = false;
break;
///////////////////////////////
}
}
}
void cliCom(void)
{
uint8_t index;
char mvlkToggleString[5] = { 0, 0, 0, 0, 0 };
if ((cliPortAvailable() && !validCliCommand))
{
cliQuery = cliPortRead();
if (cliQuery == '#') // Check to see if we should toggle mavlink msg state
{
while (cliPortAvailable == false);
readStringCLI(mvlkToggleString, 5);
if ((mvlkToggleString[0] == '#') &&
(mvlkToggleString[1] == '#') &&
(mvlkToggleString[2] == '#') &&
(mvlkToggleString[3] == '#'))
{
if (eepromConfig.mavlinkEnabled == false)
{
eepromConfig.mavlinkEnabled = true;
eepromConfig.activeTelemetry = 0x0000;
}
else
{
eepromConfig.mavlinkEnabled = false;
}
if (mvlkToggleString[4] == 'W')
{
cliPortPrint("\nWriting EEPROM Parameters....\n");
writeEEPROM();
}
}
}
}
validCliCommand = false;
if ((eepromConfig.mavlinkEnabled == false) && (cliQuery != '#'))
{
switch (cliQuery)
{
///////////////////////////////
case 'a': // Rate PIDs
cliPortPrintF("\nRoll Rate PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[ROLL_RATE_PID].B,
eepromConfig.PID[ROLL_RATE_PID].P,
eepromConfig.PID[ROLL_RATE_PID].I,
eepromConfig.PID[ROLL_RATE_PID].D,
eepromConfig.PID[ROLL_RATE_PID].windupGuard,
eepromConfig.PID[ROLL_RATE_PID].dErrorCalc ? "Error" : "State");
cliPortPrintF("Pitch Rate PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[PITCH_RATE_PID].B,
eepromConfig.PID[PITCH_RATE_PID].P,
eepromConfig.PID[PITCH_RATE_PID].I,
eepromConfig.PID[PITCH_RATE_PID].D,
eepromConfig.PID[PITCH_RATE_PID].windupGuard,
eepromConfig.PID[PITCH_RATE_PID].dErrorCalc ? "Error" : "State");
cliPortPrintF("Yaw Rate PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[YAW_RATE_PID].B,
eepromConfig.PID[YAW_RATE_PID].P,
eepromConfig.PID[YAW_RATE_PID].I,
eepromConfig.PID[YAW_RATE_PID].D,
eepromConfig.PID[YAW_RATE_PID].windupGuard,
eepromConfig.PID[YAW_RATE_PID].dErrorCalc ? "Error" : "State");
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'b': // Attitude PIDs
cliPortPrintF("\nRoll Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[ROLL_ATT_PID].B,
eepromConfig.PID[ROLL_ATT_PID].P,
eepromConfig.PID[ROLL_ATT_PID].I,
eepromConfig.PID[ROLL_ATT_PID].D,
eepromConfig.PID[ROLL_ATT_PID].windupGuard,
eepromConfig.PID[ROLL_ATT_PID].dErrorCalc ? "Error" : "State");
cliPortPrintF("Pitch Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[PITCH_ATT_PID].B,
eepromConfig.PID[PITCH_ATT_PID].P,
eepromConfig.PID[PITCH_ATT_PID].I,
eepromConfig.PID[PITCH_ATT_PID].D,
eepromConfig.PID[PITCH_ATT_PID].windupGuard,
eepromConfig.PID[PITCH_ATT_PID].dErrorCalc ? "Error" : "State");
cliPortPrintF("Heading PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[HEADING_PID].B,
eepromConfig.PID[HEADING_PID].P,
eepromConfig.PID[HEADING_PID].I,
eepromConfig.PID[HEADING_PID].D,
eepromConfig.PID[HEADING_PID].windupGuard,
eepromConfig.PID[HEADING_PID].dErrorCalc ? "Error" : "State");
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'c': // Velocity PIDs
cliPortPrintF("\nhDot PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[HDOT_PID].B,
eepromConfig.PID[HDOT_PID].P,
eepromConfig.PID[HDOT_PID].I,
eepromConfig.PID[HDOT_PID].D,
eepromConfig.PID[HDOT_PID].windupGuard,
eepromConfig.PID[HDOT_PID].dErrorCalc ? "Error" : "State");
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'd': // Position PIDs
cliPortPrintF("\nh PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[H_PID].B,
eepromConfig.PID[H_PID].P,
eepromConfig.PID[H_PID].I,
eepromConfig.PID[H_PID].D,
eepromConfig.PID[H_PID].windupGuard,
eepromConfig.PID[H_PID].dErrorCalc ? "Error" : "State");
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'e': // Loop Delta Times
cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", deltaTime1000Hz,
deltaTime500Hz,
deltaTime100Hz,
deltaTime50Hz,
deltaTime10Hz,
deltaTime5Hz,
deltaTime1Hz);
validCliCommand = false;
break;
///////////////////////////////
case 'f': // Loop Execution Times
cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", executionTime1000Hz,
executionTime500Hz,
executionTime100Hz,
executionTime50Hz,
executionTime10Hz,
executionTime5Hz,
executionTime1Hz);
validCliCommand = false;
break;
///////////////////////////////
case 'g': // 500 Hz Accels
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS],
sensors.accel500Hz[YAXIS],
sensors.accel500Hz[ZAXIS]);
validCliCommand = false;
break;
///////////////////////////////
case 'h': // 100 hz Earth Axis Accels
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", earthAxisAccels[XAXIS],
earthAxisAccels[YAXIS],
earthAxisAccels[ZAXIS]);
validCliCommand = false;
break;
///////////////////////////////
case 'i': // 500 hz Gyros
cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ] * R2D,
sensors.gyro500Hz[PITCH] * R2D,
sensors.gyro500Hz[YAW ] * R2D,
mpuTemperature);
validCliCommand = false;
break;
///////////////////////////////
case 'j': // 10 Hz Mag Data
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.mag10Hz[XAXIS],
sensors.mag10Hz[YAXIS],
sensors.mag10Hz[ZAXIS]);
validCliCommand = false;
break;
///////////////////////////////
case 'k': // Vertical Axis Variables
cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", earthAxisAccels[ZAXIS],
sensors.pressureAlt50Hz,
hDotEstimate,
hEstimate);
validCliCommand = false;
break;
///////////////////////////////
case 'l': // Attitudes
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ] * R2D,
sensors.attitude500Hz[PITCH] * R2D,
sensors.attitude500Hz[YAW ] * R2D);
validCliCommand = false;
break;
///////////////////////////////
case 'm': // Axis PIDs
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", axisPID[ROLL ],
axisPID[PITCH],
axisPID[YAW ]);
validCliCommand = false;
break;
///////////////////////////////
case 'o':
cliPortPrintF("%9.4f\n", batteryVoltage);
validCliCommand = false;
break;
///////////////////////////////
case 'p': // Primary Spektrum Raw Data
cliPortPrintF("%04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X\n", primarySpektrumState.lostFrameCnt,
primarySpektrumState.rcAvailable,
primarySpektrumState.values[0],
primarySpektrumState.values[1],
primarySpektrumState.values[2],
primarySpektrumState.values[3],
primarySpektrumState.values[4],
primarySpektrumState.values[5],
primarySpektrumState.values[6],
primarySpektrumState.values[7]);
validCliCommand = false;
break;
///////////////////////////////
case 'q': // Not Used
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'r':
if (flightMode == RATE)
cliPortPrint("Flight Mode = RATE ");
else if (flightMode == ATTITUDE)
cliPortPrint("Flight Mode = ATTITUDE ");
if (headingHoldEngaged == true)
cliPortPrint("Heading Hold = ENGAGED ");
else
cliPortPrint("Heading Hold = DISENGAGED ");
cliPortPrint("Alt Hold = ");
switch (verticalModeState)
{
case ALT_DISENGAGED_THROTTLE_ACTIVE:
cliPortPrint("Alt Disenaged Throttle Active\n");
break;
case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT:
cliPortPrint("Alt Hold Fixed at Engagement Alt\n");
break;
case ALT_HOLD_AT_REFERENCE_ALTITUDE:
cliPortPrint("Alt Hold at Reference Alt\n");
break;
case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY:
cliPortPrint("V Velocity Hold at Reference Vel\n");
break;
case ALT_DISENGAGED_THROTTLE_INACTIVE:
cliPortPrint("Alt Disengaged Throttle Inactive\n");
break;
}
validCliCommand = false;
break;
///////////////////////////////
case 's': // Raw Receiver Commands
if ((eepromConfig.receiverType == SPEKTRUM) && (maxChannelNum > 0))
{
for (index = 0; index < maxChannelNum - 1; index++)
cliPortPrintF("%4ld, ", spektrumBuf[index]);
cliPortPrintF("%4ld\n", spektrumBuf[maxChannelNum - 1]);
}
else if ((eepromConfig.receiverType == SPEKTRUM) && (maxChannelNum == 0))
cliPortPrint("Invalid Number of Spektrum Channels....\n");
else
{
for (index = 0; index < 7; index++)
cliPortPrintF("%4i, ", ppmRxRead(index));
cliPortPrintF("%4i\n", ppmRxRead(7));
}
validCliCommand = false;
break;
///////////////////////////////
case 't': // Processed Receiver Commands
for (index = 0; index < 7; index++)
cliPortPrintF("%8.2f, ", rxCommand[index]);
cliPortPrintF("%8.2f\n", rxCommand[7]);
validCliCommand = false;
break;
///////////////////////////////
case 'u': // Command in Detent Discretes
cliPortPrintF("%s, ", commandInDetent[ROLL ] ? " true" : "false");
cliPortPrintF("%s, ", commandInDetent[PITCH] ? " true" : "false");
cliPortPrintF("%s\n", commandInDetent[YAW ] ? " true" : "false");
validCliCommand = false;
break;
///////////////////////////////
case 'v': // ESC PWM Outputs
cliPortPrintF("%4ld, ", TIM4->CCR4);
cliPortPrintF("%4ld, ", TIM4->CCR3);
cliPortPrintF("%4ld, ", TIM4->CCR2);
cliPortPrintF("%4ld, ", TIM4->CCR1);
cliPortPrintF("%4ld, ", TIM1->CCR4);
cliPortPrintF("%4ld\n", TIM1->CCR1);
validCliCommand = false;
break;
///////////////////////////////
case 'x':
validCliCommand = false;
break;
///////////////////////////////
case 'y': // ESC Calibration
escCalibration();
cliQuery = 'x';
break;
///////////////////////////////
case 'z': // Voltage monitor ADC, Battery voltage
cliPortPrintF("%7.2f, %5.2f\n", voltageMonitor(), batteryVoltage);
break;
///////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////
case 'A': // Read Roll Rate PID Values
readCliPID(ROLL_RATE_PID);
cliPortPrint( "\nRoll Rate PID Received....\n" );
cliQuery = 'a';
validCliCommand = false;
break;
///////////////////////////////
case 'B': // Read Pitch Rate PID Values
readCliPID(PITCH_RATE_PID);
cliPortPrint( "\nPitch Rate PID Received....\n" );
cliQuery = 'a';
validCliCommand = false;
break;
///////////////////////////////
case 'C': // Read Yaw Rate PID Values
readCliPID(YAW_RATE_PID);
cliPortPrint( "\nYaw Rate PID Received....\n" );
cliQuery = 'a';
validCliCommand = false;
break;
///////////////////////////////
case 'D': // Read Roll Attitude PID Values
readCliPID(ROLL_ATT_PID);
cliPortPrint( "\nRoll Attitude PID Received....\n" );
cliQuery = 'b';
validCliCommand = false;
break;
///////////////////////////////
case 'E': // Read Pitch Attitude PID Values
readCliPID(PITCH_ATT_PID);
cliPortPrint( "\nPitch Attitude PID Received....\n" );
cliQuery = 'b';
validCliCommand = false;
break;
///////////////////////////////
case 'F': // Read Heading Hold PID Values
readCliPID(HEADING_PID);
cliPortPrint( "\nHeading PID Received....\n" );
cliQuery = 'b';
validCliCommand = false;
break;
///////////////////////////////
case 'I': // Read hDot PID Values
readCliPID(HDOT_PID);
cliPortPrint( "\nhDot PID Received....\n" );
cliQuery = 'c';
validCliCommand = false;
break;
///////////////////////////////
case 'L': // Read h PID Values
readCliPID(H_PID);
cliPortPrint( "\nh PID Received....\n" );
cliQuery = 'd';
validCliCommand = false;
break;
///////////////////////////////
case 'N': // Mixer CLI
mixerCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'O': // Receiver CLI
receiverCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'P': // Sensor CLI
sensorCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'R': // Reset to Bootloader
cliPortPrint("Entering Bootloader....\n\n");
delay(100);
systemReset(true);
break;
///////////////////////////////
case 'S': // Reset System
cliPortPrint("\nSystem Reseting....\n\n");
delay(100);
systemReset(false);
break;
///////////////////////////////
case 'T': // Telemetry CLI
telemetryCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'U': // EEPROM CLI
eepromCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'V': // Reset EEPROM Parameters
cliPortPrint( "\nEEPROM Parameters Reset....\n" );
checkFirstTime(true);
cliPortPrint("\nSystem Resetting....\n\n");
delay(100);
systemReset(false);
break;
///////////////////////////////
case 'W': // Write EEPROM Parameters
cliPortPrint("\nWriting EEPROM Parameters....\n");
writeEEPROM();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'X': // Not Used
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'Y': // Not Used
cliQuery = 'x';
break;
///////////////////////////////
case 'Z': // Not Used
cliQuery = 'x';
break;
///////////////////////////////
case '?': // Command Summary
cliBusy = true;
cliPortPrint("\n");
cliPortPrint("'a' Rate PIDs 'A' Set Roll Rate PID Data AB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'b' Attitude PIDs 'B' Set Pitch Rate PID Data BB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'c' Velocity PIDs 'C' Set Yaw Rate PID Data CB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'d' Position PIDs 'D' Set Roll Att PID Data DB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'e' Loop Delta Times 'E' Set Pitch Att PID Data EB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'f' Loop Execution Times 'F' Set Hdg Hold PID Data FB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'g' 500 Hz Accels 'G' Not Used\n");
cliPortPrint("'h' 100 Hz Earth Axis Accels 'H' Not Used\n");
cliPortPrint("'i' 500 Hz Gyros 'I' Set hDot PID Data IB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'j' 10 hz Mag Data 'J' Not Used\n");
cliPortPrint("'k' Vertical Axis Variable 'K' Not Used\n");
cliPortPrint("'l' Attitudes 'L' Set h PID Data LB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("\n");
cliPortPrint("Press space bar for more, or enter a command....\n");
while (cliPortAvailable() == false);
cliQuery = cliPortRead();
if (cliQuery != ' ')
{
validCliCommand = true;
cliBusy = false;
return;
}
cliPortPrint("\n");
cliPortPrint("'m' Axis PIDs 'M' Not Used\n");
cliPortPrint("'n' Not Used 'N' Mixer CLI\n");
cliPortPrint("'o' Battery Voltage 'O' Receiver CLI\n");
cliPortPrint("'p' Not Used 'P' Sensor CLI\n");
cliPortPrint("'q' Primary Spektrum Raw Data 'Q' Not Used\n");
cliPortPrint("'r' Mode States 'R' Reset and Enter Bootloader\n");
cliPortPrint("'s' Raw Receiver Commands 'S' Reset\n");
cliPortPrint("'t' Processed Receiver Commands 'T' Telemetry CLI\n");
cliPortPrint("'u' Command In Detent Discretes 'U' EEPROM CLI\n");
cliPortPrint("'v' Motor PWM Outputs 'V' Reset EEPROM Parameters\n");
cliPortPrint("'w' Not Used 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Terminate Serial Communication 'X' Not Used\n");
cliPortPrint("\n");
cliPortPrint("Press space bar for more, or enter a command....\n");
while (cliPortAvailable() == false);
cliQuery = cliPortRead();
if (cliQuery != ' ')
{
validCliCommand = true;
cliBusy = false;
return;
}
cliPortPrint("\n");
cliPortPrint("'y' ESC Calibration 'Y' Not Used\n");
cliPortPrint("'z' ADC Values 'Z' Not Used\n");
cliPortPrint("'#####' Toggle MavLink Msg State '?' Command Summary\n");
cliPortPrint("\n");
cliQuery = 'x';
cliBusy = false;
break;
///////////////////////////////
}
}
}
