void CGameClient::WriteViewAngleUpdate() { // // send the current viewpos offset from the view entity // // a fixangle might get lost in a dropped packet. Oh well. if ( IsFakeClient() ) return; Assert( serverGameClients ); CPlayerState *pl = serverGameClients->GetPlayerState( edict ); Assert( pl ); if ( pl && pl->fixangle != FIXANGLE_NONE ) { if ( pl->fixangle == FIXANGLE_RELATIVE ) { SVC_FixAngle fixAngle( true, pl->anglechange ); m_NetChannel->SendNetMsg( fixAngle ); pl->anglechange.Init(); // clear } else { SVC_FixAngle fixAngle(false, pl->v_angle ); m_NetChannel->SendNetMsg( fixAngle ); } pl->fixangle = FIXANGLE_NONE; } }
void RobotMove::DriveToVector(Vector &whereToGo, Position2dProxy * position){ startingAngle = whereToGo.getAngle(); startingAngle = fixAngle(dtor(startingAngle)); distanceToGo = whereToGo.getDistance(); speed = whereToGo.getDistance(); angularDistence = fixAngle(startingAngle - (*position).GetYaw()); speed = abs(speed - abs(angularDistence / dtor(90))); if(speed > maxSpeed) speed = maxSpeed; (*position).SetSpeed(speed, angularDistence / angleDampener); }
/** * The idea of this method is to execute all heavy calculations in a lower-priority thread, * so that trigger event handler/IO scheduler tasks are faster. */ void Engine::periodicFastCallback(DECLARE_ENGINE_PARAMETER_F) { int rpm = rpmCalculator.rpmValue; if (isValidRpm(rpm)) { MAP_sensor_config_s * c = &engineConfiguration->map; angle_t start = interpolate2d(rpm, c->samplingAngleBins, c->samplingAngle, MAP_ANGLE_SIZE); angle_t offsetAngle = TRIGGER_SHAPE(eventAngles[CONFIG(mapAveragingSchedulingAtIndex)]); for (int i = 0; i < engineConfiguration->specs.cylindersCount; i++) { angle_t cylinderOffset = getEngineCycle(engineConfiguration->operationMode) * i / engineConfiguration->specs.cylindersCount; float cylinderStart = start + cylinderOffset - offsetAngle + tdcPosition(); fixAngle(cylinderStart, "cylinderStart"); engine->engineState.mapAveragingStart[i] = cylinderStart; } engine->engineState.mapAveragingDuration = interpolate2d(rpm, c->samplingWindowBins, c->samplingWindow, MAP_WINDOW_SIZE); } else { for (int i = 0; i < engineConfiguration->specs.cylindersCount; i++) { engine->engineState.mapAveragingStart[i] = NAN; } engine->engineState.mapAveragingDuration = NAN; } engineState.periodicFastCallback(PASS_ENGINE_PARAMETER_F); engine->m.beforeFuelCalc = GET_TIMESTAMP(); ENGINE(fuelMs) = getInjectionDuration(rpm PASS_ENGINE_PARAMETER) * engineConfiguration->globalFuelCorrection; engine->m.fuelCalcTime = GET_TIMESTAMP() - engine->m.beforeFuelCalc; }
void moveClock(void) { /* Bounce the clock off the walls */ if (extent.left + stepx < 0) stepx = -stepx; if (extent.right + stepx > maxx) stepx = -stepx; if (extent.top + stepy < 0) stepy = -stepy; if (extent.bottom + stepy > maxy) stepy = -stepy; MGL_offsetRect(extent,stepx,stepy); /* Update the hand movement */ secAngle = fixAngle(secAngle - 5); if (secAngle == 90) minAngle = fixAngle(minAngle - 5); }
float getAdvance(int rpm, float engineLoad) { float angle; if (isCrankingR(rpm)) { angle = engineConfiguration->crankingTimingAngle; } else { angle = getBaseAdvance(rpm, engineLoad); } return fixAngle(angle + engineConfiguration->ignitionOffset); }
static void auxValveTriggerCallback(trigger_event_e ckpSignalType, uint32_t index DECLARE_ENGINE_PARAMETER_SUFFIX) { UNUSED(ckpSignalType); #if EFI_PROD_CODE || EFI_SIMULATOR if (index != SCHEDULING_TRIGGER_INDEX) { return; } int rpm = GET_RPM_VALUE; if (!isValidRpm(rpm)) { return; } for (int valveIndex = 0; valveIndex < AUX_DIGITAL_VALVE_COUNT; valveIndex++) { NamedOutputPin *output = &enginePins.auxValve[valveIndex]; for (int phaseIndex = 0; phaseIndex < 2; phaseIndex++) { /* I believe a more correct implementation is the following: * here we properly account for trigger angle position in engine cycle coordinates // todo: at the moment this logic is assuming four-stroke 720-degree engine cycle angle_t extra = phaseIndex * 360 // cycle opens twice per 720 engine cycle + valveIndex * 180 // 2nd valve is operating at 180 offset to first + tdcPosition() // engine cycle position to trigger cycle position conversion - ENGINE(triggerCentral.triggerShape.eventAngles[SCHEDULING_TRIGGER_INDEX]) ; */ angle_t extra = phaseIndex * 360 + valveIndex * 180; angle_t onTime = extra + engine->engineState.auxValveStart; fixAngle(onTime, "onTime", CUSTOM_ERR_6556); scheduleByAngle(rpm, &turnOnEvent[valveIndex][phaseIndex], onTime, (schfunc_t) &turnOn, output, &engine->rpmCalculator); angle_t offTime = extra + engine->engineState.auxValveEnd; fixAngle(offTime, "offTime", CUSTOM_ERR_6557); scheduleByAngle(rpm, &turnOffEvent[valveIndex][phaseIndex], offTime, (schfunc_t) &turnOff, output, &engine->rpmCalculator); } } #endif /* EFI_PROD_CODE || EFI_SIMULATOR */ }
static void reportWave(Logging *logging, int index) { if (readers[index].hw == NULL) { return; } if (readers[index].hw->started) { // int counter = getEventCounter(index); // debugInt2(logging, "ev", index, counter); float dwellMs = getSignalOnTime(index); float periodMs = getSignalPeriodMs(index); appendPrintf(logging, "duty%d%s", index, DELIMETER); appendFloat(logging, 100.0f * dwellMs / periodMs, 2); appendPrintf(logging, "%s", DELIMETER); /** * that's the ON time of the LAST signal */ appendPrintf(logging, "dwell%d%s", index, DELIMETER); appendFloat(logging, dwellMs, 2); appendPrintf(logging, "%s", DELIMETER); /** * that's the total ON time during the previous engine cycle */ appendPrintf(logging, "total_dwell%d%s", index, DELIMETER); appendFloat(logging, readers[index].prevTotalOnTimeUs / 1000.0f, 2); appendPrintf(logging, "%s", DELIMETER); appendPrintf(logging, "period%d%s", index, DELIMETER); appendFloat(logging, periodMs, 2); appendPrintf(logging, "%s", DELIMETER); uint32_t offsetUs = getWaveOffset(index); int rpm = GET_RPM(); if (rpm != 0) { float oneDegreeUs = getOneDegreeTimeUs(rpm); appendPrintf(logging, "advance%d%s", index, DELIMETER); float angle = (offsetUs / oneDegreeUs) - tdcPosition(); fixAngle(angle, "waveAn", CUSTOM_ERR_6564); appendFloat(logging, angle, 3); appendPrintf(logging, "%s", DELIMETER); } } }
void findTriggerPosition(engine_configuration_s const *engineConfiguration, trigger_shape_s * s, event_trigger_position_s *position, float angleOffset) { angleOffset = fixAngle(angleOffset + engineConfiguration->globalTriggerAngleOffset); int engineCycleEventCount = getEngineCycleEventCount(engineConfiguration, s); int middle; int left = 0; int right = engineCycleEventCount - 1; /** * Let's find the last trigger angle which is less or equal to the desired angle * todo: extract binary search as template method? */ while (true) { middle = (left + right) / 2; if (middle == left) { break; } if (angleOffset < s->eventAngles[middle]) { right = middle; } else if (angleOffset > s->eventAngles[middle]) { left = middle; } else { break; } } float eventAngle = s->eventAngles[middle]; if (angleOffset < eventAngle) { firmwareError("angle constraint violation in registerActuatorEventExt(): %f/%f", angleOffset, eventAngle); return; } position->eventIndex = middle; position->eventAngle = eventAngle; position->angleOffset = angleOffset - eventAngle; }
static void reportWave(Logging *logging, int index) { if (readers[index].hw.started) { // int counter = getEventCounter(index); // debugInt2(logging, "ev", index, counter); float dwellMs = getSignalOnTime(index); float periodMs = getSignalPeriodMs(index); appendPrintf(logging, "duty%d%s", index, DELIMETER); appendFloat(logging, 100.0f * dwellMs / periodMs, 2); appendPrintf(logging, "%s", DELIMETER); /** * that's the ON time of the LAST signal */ appendPrintf(logging, "dwell%d%s", index, DELIMETER); appendFloat(logging, dwellMs, 2); appendPrintf(logging, "%s", DELIMETER); /** * that's the total ON time during the previous engine cycle */ appendPrintf(logging, "total_dwell%d%s", index, DELIMETER); appendFloat(logging, readers[index].prevTotalOnTimeUs / 1000.0f, 2); appendPrintf(logging, "%s", DELIMETER); appendPrintf(logging, "period%d%s", index, DELIMETER); appendFloat(logging, periodMs, 2); appendPrintf(logging, "%s", DELIMETER); uint32_t offsetUs = getWaveOffset(index); float oneDegreeUs = getOneDegreeTimeUs(getRpm()); appendPrintf(logging, "advance%d%s", index, DELIMETER); appendFloat(logging, fixAngle((offsetUs / oneDegreeUs) - engineConfiguration->globalTriggerAngleOffset), 3); appendPrintf(logging, "%s", DELIMETER); } }
void TriggerShape::calculateTriggerSynchPoint(DECLARE_ENGINE_PARAMETER_F) { trigger_config_s const*triggerConfig = &engineConfiguration->trigger; triggerShapeSynchPointIndex = findTriggerZeroEventIndex(this, triggerConfig PASS_ENGINE_PARAMETER); engine->engineCycleEventCount = getLength(); float firstAngle = getAngle(triggerShapeSynchPointIndex); int frontOnlyIndex = 0; for (int eventIndex = 0; eventIndex < engine->engineCycleEventCount; eventIndex++) { if (eventIndex == 0) { // explicit check for zero to avoid issues where logical zero is not exactly zero due to float nature eventAngles[0] = 0; // this value would be used in case of front-only eventAngles[1] = 0; frontOnlyIndexes[0] = 0; } else { int triggerDefinitionCoordinate = (triggerShapeSynchPointIndex + eventIndex) % engine->engineCycleEventCount; int triggerDefinitionIndex = triggerDefinitionCoordinate >= size ? triggerDefinitionCoordinate - size : triggerDefinitionCoordinate; float angle = getAngle(triggerDefinitionCoordinate) - firstAngle; fixAngle(angle); if (engineConfiguration->useOnlyFrontForTrigger) { if (isFrontEvent[triggerDefinitionIndex]) { frontOnlyIndex += 2; eventAngles[frontOnlyIndex] = angle; eventAngles[frontOnlyIndex + 1] = angle; } } else { eventAngles[eventIndex] = angle; } frontOnlyIndexes[eventIndex] = frontOnlyIndex; } } }
/** * @brief Trigger decoding happens here * This method is invoked every time we have a fall or rise on one of the trigger sensors. * This method changes the state of trigger_state_s data structure according to the trigger event * @param signal type of event which just happened * @param nowNt current time */ void TriggerState::decodeTriggerEvent(trigger_event_e const signal, efitime_t nowNt DECLARE_ENGINE_PARAMETER_S) { efiAssertVoid(signal <= SHAFT_3RD_UP, "unexpected signal"); trigger_wheel_e triggerWheel = eventIndex[signal]; if (!engineConfiguration->useOnlyFrontForTrigger && curSignal == prevSignal) { orderingErrorCounter++; } prevSignal = curSignal; curSignal = signal; currentCycle.eventCount[triggerWheel]++; efitime_t currentDurationLong = getCurrentGapDuration(nowNt); /** * For performance reasons, we want to work with 32 bit values. If there has been more then * 10 seconds since previous trigger event we do not really care. */ currentDuration = currentDurationLong > 10 * US2NT(US_PER_SECOND_LL) ? 10 * US2NT(US_PER_SECOND_LL) : currentDurationLong; bool isPrimary = triggerWheel == T_PRIMARY; if (isLessImportant(signal)) { #if EFI_UNIT_TEST || defined(__DOXYGEN__) if (printTriggerDebug) { printf("%s isLessImportant %s %d\r\n", getTrigger_type_e(engineConfiguration->trigger.type), getTrigger_event_e(signal), nowNt); } #endif /** * For less important events we simply increment the index. */ nextTriggerEvent() ; if (TRIGGER_SHAPE(gapBothDirections) && considerEventForGap()) { isFirstEvent = false; thirdPreviousDuration = durationBeforePrevious; durationBeforePrevious = toothed_previous_duration; toothed_previous_duration = currentDuration; toothed_previous_time = nowNt; } } else { #if EFI_UNIT_TEST || defined(__DOXYGEN__) if (printTriggerDebug) { printf("%s event %s %d\r\n", getTrigger_type_e(engineConfiguration->trigger.type), getTrigger_event_e(signal), nowNt); } #endif isFirstEvent = false; // todo: skip a number of signal from the beginning #if EFI_PROD_CODE || defined(__DOXYGEN__) // scheduleMsg(&logger, "from %f to %f %d %d", triggerConfig->syncRatioFrom, triggerConfig->syncRatioTo, currentDuration, shaftPositionState->toothed_previous_duration); // scheduleMsg(&logger, "ratio %f", 1.0 * currentDuration/ shaftPositionState->toothed_previous_duration); #else if (toothed_previous_duration != 0) { // printf("ratio %f: cur=%d pref=%d\r\n", 1.0 * currentDuration / shaftPositionState->toothed_previous_duration, // currentDuration, shaftPositionState->toothed_previous_duration); } #endif bool isSynchronizationPoint; if (TRIGGER_SHAPE(isSynchronizationNeeded)) { /** * Here I prefer to have two multiplications instead of one division, that's a micro-optimization */ isSynchronizationPoint = currentDuration > toothed_previous_duration * TRIGGER_SHAPE(syncRatioFrom) && currentDuration < toothed_previous_duration * TRIGGER_SHAPE(syncRatioTo) && toothed_previous_duration > durationBeforePrevious * TRIGGER_SHAPE(secondSyncRatioFrom) && toothed_previous_duration < durationBeforePrevious * TRIGGER_SHAPE(secondSyncRatioTo) // this is getting a little out of hand, any ideas? && durationBeforePrevious > thirdPreviousDuration * TRIGGER_SHAPE(thirdSyncRatioFrom) && durationBeforePrevious < thirdPreviousDuration * TRIGGER_SHAPE(thirdSyncRatioTo) ; #if EFI_PROD_CODE || defined(__DOXYGEN__) if (engineConfiguration->isPrintTriggerSynchDetails || someSortOfTriggerError) { #else if (printTriggerDebug) { #endif /* EFI_PROD_CODE */ float gap = 1.0 * currentDuration / toothed_previous_duration; float prevGap = 1.0 * toothed_previous_duration / durationBeforePrevious; float gap3 = 1.0 * durationBeforePrevious / thirdPreviousDuration; #if EFI_PROD_CODE || defined(__DOXYGEN__) scheduleMsg(logger, "gap=%f/%f/%f @ %d while expected %f/%f and %f/%f error=%d", gap, prevGap, gap3, currentCycle.current_index, TRIGGER_SHAPE(syncRatioFrom), TRIGGER_SHAPE(syncRatioTo), TRIGGER_SHAPE(secondSyncRatioFrom), TRIGGER_SHAPE(secondSyncRatioTo), someSortOfTriggerError); #else actualSynchGap = gap; print("current gap %f/%f/%f c=%d prev=%d\r\n", gap, prevGap, gap3, currentDuration, toothed_previous_duration); #endif /* EFI_PROD_CODE */ } } else { /** * in case of noise the counter could be above the expected number of events */ int d = engineConfiguration->useOnlyFrontForTrigger ? 2 : 1; isSynchronizationPoint = !shaft_is_synchronized || (currentCycle.current_index >= TRIGGER_SHAPE(size) - d); } #if EFI_UNIT_TEST || defined(__DOXYGEN__) if (printTriggerDebug) { printf("%s isSynchronizationPoint=%d index=%d %s\r\n", getTrigger_type_e(engineConfiguration->trigger.type), isSynchronizationPoint, currentCycle.current_index, getTrigger_event_e(signal)); } #endif if (isSynchronizationPoint) { /** * We can check if things are fine by comparing the number of events in a cycle with the expected number of event. */ bool isDecodingError = currentCycle.eventCount[0] != TRIGGER_SHAPE(expectedEventCount[0]) || currentCycle.eventCount[1] != TRIGGER_SHAPE(expectedEventCount[1]) || currentCycle.eventCount[2] != TRIGGER_SHAPE(expectedEventCount[2]); triggerDecoderErrorPin.setValue(isDecodingError); if (isDecodingError) { lastDecodingErrorTime = getTimeNowNt(); someSortOfTriggerError = true; totalTriggerErrorCounter++; if (engineConfiguration->isPrintTriggerSynchDetails || someSortOfTriggerError) { #if EFI_PROD_CODE || defined(__DOXYGEN__) scheduleMsg(logger, "error: synchronizationPoint @ index %d expected %d/%d/%d got %d/%d/%d", currentCycle.current_index, TRIGGER_SHAPE(expectedEventCount[0]), TRIGGER_SHAPE(expectedEventCount[1]), TRIGGER_SHAPE(expectedEventCount[2]), currentCycle.eventCount[0], currentCycle.eventCount[1], currentCycle.eventCount[2]); #endif /* EFI_PROD_CODE */ } } errorDetection.add(isDecodingError); if (isTriggerDecoderError()) { warning(OBD_PCM_Processor_Fault, "trigger decoding issue. expected %d/%d/%d got %d/%d/%d", TRIGGER_SHAPE(expectedEventCount[0]), TRIGGER_SHAPE(expectedEventCount[1]), TRIGGER_SHAPE(expectedEventCount[2]), currentCycle.eventCount[0], currentCycle.eventCount[1], currentCycle.eventCount[2]); } shaft_is_synchronized = true; // this call would update duty cycle values nextTriggerEvent() ; nextRevolution(); } else { nextTriggerEvent() ; } thirdPreviousDuration = durationBeforePrevious; durationBeforePrevious = toothed_previous_duration; toothed_previous_duration = currentDuration; toothed_previous_time = nowNt; } if (!isValidIndex(PASS_ENGINE_PARAMETER_F)) { warning(OBD_PCM_Processor_Fault, "unexpected eventIndex=%d while size %d", currentCycle.current_index, TRIGGER_SHAPE(size)); lastDecodingErrorTime = getTimeNowNt(); someSortOfTriggerError = true; } if (someSortOfTriggerError) { if (getTimeNowNt() - lastDecodingErrorTime > US2NT(US_PER_SECOND_LL)) { someSortOfTriggerError = false; } } if (ENGINE(sensorChartMode) == SC_RPM_ACCEL || ENGINE(sensorChartMode) == SC_DETAILED_RPM) { angle_t currentAngle = TRIGGER_SHAPE(eventAngles[currentCycle.current_index]); // todo: make this '90' depend on cylinder count? angle_t prevAngle = currentAngle - 90; fixAngle(prevAngle); // todo: prevIndex should be pre-calculated int prevIndex = TRIGGER_SHAPE(triggerIndexByAngle[(int)prevAngle]); // now let's get precise angle for that event prevAngle = TRIGGER_SHAPE(eventAngles[prevIndex]); // todo: re-implement this as a subclass. we need two instances of // uint32_t time = nowNt - timeOfLastEvent[prevIndex]; angle_t angleDiff = currentAngle - prevAngle; // todo: angle diff should be pre-calculated fixAngle(angleDiff); // float r = (60000000.0 / 360 * US_TO_NT_MULTIPLIER) * angleDiff / time; #if EFI_SENSOR_CHART || defined(__DOXYGEN__) if (boardConfiguration->sensorChartMode == SC_DETAILED_RPM) { // scAddData(currentAngle, r); } else { // scAddData(currentAngle, r / instantRpmValue[prevIndex]); } #endif // instantRpmValue[currentCycle.current_index] = r; // timeOfLastEvent[currentCycle.current_index] = nowNt; } } angle_t getEngineCycle(operation_mode_e operationMode) { return operationMode == TWO_STROKE ? 360 : 720; } void addSkippedToothTriggerEvents(trigger_wheel_e wheel, TriggerShape *s, int totalTeethCount, int skippedCount, float toothWidth, float offset, float engineCycle, float filterLeft, float filterRight) { efiAssertVoid(totalTeethCount > 0, "total count"); efiAssertVoid(skippedCount >= 0, "skipped count"); for (int i = 0; i < totalTeethCount - skippedCount - 1; i++) { float angleDown = engineCycle / totalTeethCount * (i + (1 - toothWidth)); float angleUp = engineCycle / totalTeethCount * (i + 1); s->addEvent(offset + angleDown, wheel, TV_RISE, filterLeft, filterRight); s->addEvent(offset + angleUp, wheel, TV_FALL, filterLeft, filterRight); } float angleDown = engineCycle / totalTeethCount * (totalTeethCount - skippedCount - 1 + (1 - toothWidth)); s->addEvent(offset + angleDown, wheel, TV_RISE, filterLeft, filterRight); s->addEvent(offset + engineCycle, wheel, TV_FALL, filterLeft, filterRight); }