void LRoverNavigator::loop() {
if (_state == LRoverStateGoalReached) return;
_gps->readGPSData();
if (_state == LRoverStateInitialization) {
initializationLoop();
return;
}
unsigned long currentTime = millis();
if (currentTime - _mainLoopTime20Hz >= 50) {
_mainLoopTime20Hz = currentTime;
mainLoopAt20Hz();
}
if (currentTime - _loggingAndTuningTime1Hz >= 1000) {
_loggingAndTuningTime1Hz = currentTime;
loggingAndTuningLoopAt1Hz();
}
if (currentTime - _loggingAndTuningTime5s >= 5000) {
_loggingAndTuningTime5s = currentTime;
loggingAndTuningLoopAt5s();
}
}
bool MainLoop::processLoop(){
while(sControl->read()){};
if(sControl->isCtrlCommandReady()){
fCtrlCommand = sControl->getCtrlCommand();
cout << "Control command received " << (int)fCtrlCommand << endl;
delay(100);
if(fCtrlCommand==Constants::CtrlCommand::kDODEBUG) bigDebug = !bigDebug;
}
if(sControl->isSimpleKFactorsReady()){
sAltitude->setKs(sControl->getSimpleKP(), sControl->getSimpleKD());
}
if(fState==kINITIALIZING) initializationLoop();
else if(fState==kIDLE) idleLoop();
else if(fState==kCALIBRATING) calibrationLoop();
else if(fState==kFLYING) flightLoop();
else if(fState==kSTILL) stillLoop();
else{
//Error, do something
}
return true;
}
