Example #1
0
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); 
		
	
	}
Example #3
0
/**
 * 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;

}
Example #4
0
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);
}
Example #5
0
float getAdvance(int rpm, float engineLoad) {
	float angle;
	if (isCrankingR(rpm)) {
		angle = engineConfiguration->crankingTimingAngle;
	} else {
		angle = getBaseAdvance(rpm, engineLoad);
	}
	return fixAngle(angle + engineConfiguration->ignitionOffset);
}
Example #6
0
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 */
}
Example #7
0
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);
		}
	}
}
Example #8
0
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;
}
Example #9
0
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);
	}
}
Example #10
0
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;
		}
	}
}
Example #11
0
/**
 * @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);
}