/**
* @brief Sends all pending data to dev console
*/
void updateDevConsoleState(Engine *engine) {
if (!isConsoleReady()) {
return;
}
// looks like this is not needed anymore
// checkIfShouldHalt();
printPending();
/**
* this should go before the firmware error so that console can detect connection
*/
printSensors(&logger, false);
#if EFI_PROD_CODE || defined(__DOXYGEN__)
// todo: unify with simulator!
if (hasFirmwareError()) {
scheduleMsg(&logger, "FATAL error: %s", errorMessageBuffer);
warningEnabled = false;
scheduleLogging(&logger);
return;
}
#endif
#if (EFI_PROD_CODE && HAL_USE_ADC) || defined(__DOXYGEN__)
pokeAdcInputs();
#endif
if (!fullLog) {
return;
}
systime_t nowSeconds = getTimeNowSeconds();
printInfo(nowSeconds);
#if EFI_ENGINE_CONTROL || defined(__DOXYGEN__)
int currentCkpEventCounter = getCrankEventCounter();
if (prevCkpEventCounter == currentCkpEventCounter && timeOfPreviousReport == nowSeconds) {
return;
}
timeOfPreviousReport = nowSeconds;
prevCkpEventCounter = currentCkpEventCounter;
#else
chThdSleepMilliseconds(200);
#endif
printState();
#if EFI_WAVE_ANALYZER
printWave(&logger);
#endif
scheduleLogging(&logger);
}
void writeLogLine(void) {
#if EFI_FILE_LOGGING || defined(__DOXYGEN__)
if (!main_loop_started)
return;
resetLogging(&fileLogger);
printSensors(&fileLogger, true);
if (isSdCardAlive()) {
appendPrintf(&fileLogger, "\r\n");
appendToLog(fileLogger.buffer);
logFileLineIndex++;
}
#endif /* EFI_FILE_LOGGING */
}
/**
* Main loop.
*/
int main(void) {
int32_t lastSpeedCalc = 0;
int32_t lastDebug = 0;
int32_t lastDebug2 = 0;
// initalize all modules
initAll();
// enable interrupts (i.e timers start from here)
sei();
// The LOOP
while(1) {
// read serial port
readCommand(); // communication.c
// apply speed calculation and run odometer
if (uptime() >= lastSpeedCalc + SPEED_CALC_PERIOD) {
lastSpeedCalc += SPEED_CALC_PERIOD;
calculateSpeeds(); // motors.c
if (pidOn) {
runPID(); // motors.c
}
//if (debug1) printSpeed(CPUCHAR);
runOdometer(); // odometer.c
navigator();
}
// read sensors and apply position correction
positionCorrection(); // odometer.c
// print debug loop 2
if (uptime() >= (lastDebug2 + 20)) {
lastDebug2 = uptime();
if (debug6) printf("%d, %d\r\n", p_LsensVal, p_RsensVal);
}
// print debug
if ((debugPeriod) && uptime() >= (lastDebug + debugPeriod)) {
lastDebug += debugPeriod;
//if (debug1) printf(">21%02x\r", speed0 >> 4);
//if (debug1) printf(">21%04x\r>22%04x\r", readADC(0), readADC(1));
//if (debug2) printf("%ld\t%ld\r\n", timer, timer20);
//if (debug2) printf_P(PSTR("%d\t%d\r\n"), p_L, p_R);
if (debug3) printf_P(PSTR("%d, %d / %d, %d / %d, %d / %u, %u\r\n"),
p_Ltrans, p_Lrel, p_Rtrans, p_Rrel, p_Lerr, p_Rerr, posCorrLeftFailed, posCorrRightFailed);
//if (debug4) printf_P(PSTR("%ld\t%ld\r\n"), ticks0, ticks1);
//if (debug5) printSpeed(CPUCHAR);
//if (debug5) printf_P(PSTR("%d\t%d\r\n"), speed0, speed1);
//if (debug6) printf_P(PSTR("%d\t%d\r\n"), accel0, accel1);
if (debug7) printf_P(PSTR("%u\t%u\r\n"), power0, power1);
// debug for GUI
printTicks(CPUCHAR);
printSpeed(CPUCHAR);
printAccel(CPUCHAR);
printAbsDist(CPUCHAR);
printRelDist(CPUCHAR);
printSensors(CPUCHAR);
}
}
while(1);
}
int Arduilink::handleInput() {
while (Serial.available() > 0) {
while (write) {}
write = true;
String str = Serial.readString();
char buf[str.length() + 1];
str.toCharArray(buf, str.length() + 1);
char* opcode = NULL;
int node = -1;
int sensorId = -1;
char* pch;
pch = strtok(buf, ";");
while (pch != NULL)
{
if (opcode == NULL) {
if (strcmp(pch, "PRESENT") == 0) {
printSensors();
write = false;
return 0;
}
if (strcmp(pch, "GET") != 0 && strcmp(pch, "INFO") != 0 && strcmp(pch, "SET") != 0) {
Serial.print("400;OPCODE;");
Serial.println(pch);
write = false;
return 2;
}
opcode = pch;
}
else if (node == -1) {
node = atoi(pch);
}
else if (sensorId == -1) {
sensorId = atoi(pch);
break;
}
pch = strtok(NULL, ";");
}
/*Serial.print("Opcode: ");
Serial.print(opcode);
Serial.print(" NodeId: ");
Serial.print(node);
Serial.print(" SensorId: ");
Serial.println(sensorId);*/
if (nodeId != node) {
Serial.print("404;NODE;");
Serial.println(node);
write = false;
return 3;
}
SensorItem* sensor = getSensor(sensorId);
if (sensor == NULL) {
Serial.print("404;SENSOR;");
Serial.println(sensorId);
write = false;
return 4;
}
// INFO - Récupérer la description d'un capteur
if (strcmp(opcode, "INFO") == 0) {
printSensor(sensor, node);
}
// GET - Récupérer la valeur d'un capteur
if (strcmp(opcode, "GET") == 0) {
char buff[256];
sprintf(buff, "200;%d;%d;", node, sensor->id);
Serial.print(buff);
Serial.println(sensor->value);
}
// SET - Modifier un attribut d'un capteur
else if (strcmp(opcode, "SET") == 0) {
// Choose attribute mode
pch = strtok(NULL, ";");
int mode = 0;
if (strcmp(pch, "VERBOSE") == 0) mode = 1;
else if (strcmp(pch, "VAL") == 0) mode = 2;
else {
Serial.print("400;ATTR;");
Serial.println(pch);
write = false;
return 5;
}
// Ack
pch = strtok(NULL, ";");
bool ack = strcmp(pch, "1") == 0;
// Value
pch = strtok(NULL, ";");
// Handle Set Verbose
if (mode == 1) {
if (strcmp(pch, "1") == 0) {
if (ack) {
Serial.println("201;VERBOSE;1");
Serial.flush();
}
sensor->verbose = true;
}
else if (strcmp(pch, "0") == 0) {
sensor->verbose = false;
if (ack) {
Serial.println("201;VERBOSE;0"); // TODO Ameliorer avec ids
Serial.flush();
}
}
else {
Serial.print("400;OPT;VERBOSE;"); // TODO Ameliorer avec ids
Serial.println(pch);
write = false;
return 6;
}
}
// Handle Set Value
else if (mode == 2) {
sensor->value = pch;
if (sensor->writter != NULL)
sensor->writter(pch);
if (ack) {
Serial.print("201;SET;");
Serial.print(node);
Serial.print(";");
Serial.print(sensor->id);
Serial.print(";VAL;");
Serial.println(pch);
}
}
}
Serial.flush();
write = false;
return 0;
}
return 1;
}
