static ALWAYS_INLINE void handleFuelInjectionEvent(int eventIndex, bool limitedFuel, InjectionEvent *event, int rpm DECLARE_ENGINE_PARAMETER_S) { if (limitedFuel) return; // todo: move this check up /** * todo: this is a bit tricky with batched injection. is it? Does the same * wetting coefficient works the same way for any injection mode, or is something * x2 or /2? */ floatms_t injectionDuration = ENGINE(wallFuel).adjust(event->injectorIndex, ENGINE(fuelMs) PASS_ENGINE_PARAMETER); ENGINE(actualLastInjection) = injectionDuration; if (cisnan(injectionDuration)) { warning(OBD_PCM_Processor_Fault, "NaN injection pulse"); return; } if (injectionDuration < 0) { warning(OBD_PCM_Processor_Fault, "Negative injection pulse %f", injectionDuration); return; } if (engine->isCylinderCleanupMode) return; floatus_t injectionStartDelayUs = ENGINE(rpmCalculator.oneDegreeUs) * event->injectionStart.angleOffset; OutputSignal *signal = &ENGINE(engineConfiguration2)->fuelActuators[eventIndex]; if (event->isSimultanious) { if (injectionDuration < 0) { firmwareError("duration cannot be negative: %d", injectionDuration); return; } if (cisnan(injectionDuration)) { firmwareError("NaN in scheduleOutput", injectionDuration); return; } /** * this is pretty much copy-paste of 'scheduleOutput' * 'scheduleOutput' is currently only used for injection, so maybe it should be * changed into 'scheduleInjection' and unified? todo: think about it. */ efiAssertVoid(signal!=NULL, "signal is NULL"); int index = getRevolutionCounter() % 2; scheduling_s * sUp = &signal->signalTimerUp[index]; scheduling_s * sDown = &signal->signalTimerDown[index]; scheduleTask("out up", sUp, (int) injectionStartDelayUs, (schfunc_t) &startSimultaniousInjection, engine); scheduleTask("out down", sDown, (int) injectionStartDelayUs + MS2US(injectionDuration), (schfunc_t) &endSimultaniousInjection, engine); } else { #if EFI_UNIT_TEST || defined(__DOXYGEN__) printf("scheduling injection angle=%f/delay=%f injectionDuration=%f\r\n", event->injectionStart.angleOffset, injectionStartDelayUs, injectionDuration); #endif scheduleOutput(signal, getTimeNowUs(), injectionStartDelayUs, MS2US(injectionDuration), event->output); } }
static void testCallback(void *arg) { /** * 0.1ms from now please squirt for 1.6ms */ float delayMs = 0.1; float durationMs = 1.6; efitimeus_t nowUs = getTimeNowUs(); scheduleOutput(&outSignals[0], nowUs, delayMs, durationMs); scheduleOutput(&outSignals[1], nowUs, delayMs, durationMs); scheduleOutput(&outSignals[2], nowUs, delayMs, durationMs); scheduleOutput(&outSignals[3], nowUs, delayMs, durationMs); scheduleOutput(&outSignals[4], nowUs, delayMs, durationMs); scheduleOutput(&outSignals[5], nowUs, delayMs, durationMs); scheduleOutput(&outSignals[6], nowUs, delayMs, durationMs); scheduleOutput(&outSignals[7], nowUs, delayMs, durationMs); /** * this would re-schedule another callback in 2ms from now */ scheduleTask("test", &ioTest, MS2US(2), testCallback, NULL); }
/** * * @param delay the number of ticks before the output signal * immediate output if delay is zero * @param dwell the number of ticks of output duration * */ void scheduleOutput(OutputSignal *signal, float delayMs, float durationMs) { if (durationMs < 0) { firmwareError("duration cannot be negative: %d", durationMs); return; } if (cisnan(durationMs)) { firmwareError("NaN in scheduleOutput", durationMs); return; } int index = getRevolutionCounter() % 2; scheduling_s * sUp = &signal->signalTimerUp[index]; scheduling_s * sDown = &signal->signalTimerDown[index]; scheduleTask("out up", sUp, (int)MS2US(delayMs), (schfunc_t) &turnPinHigh, (void *) signal->io_pin); scheduleTask("out down", sDown, (int)MS2US(delayMs + durationMs), (schfunc_t) &turnPinLow, (void*) signal->io_pin); }
void output_rx_throughput(packet_t *pktt) { static dllist_node_t rx_tpt_list = { .prev = &rx_tpt_list, .next = &rx_tpt_list }; if (pktt && pktt->ptt != RX_OK) return; if (pktt == NULL) { /* output the result */ while (!DLLIST_EMPTY(&rx_tpt_list)) { rx_throughput_t *ttt = (rx_throughput_t*) DLLIST_HEAD(&rx_tpt_list); ZLOG_INFO("rx tpt: at [%d, %d)s, throughput %d bps\n", ttt->time, ttt->time + 1, ttt->throughput /* bps */); dllist_remove(&rx_tpt_list, &(ttt->node)); free(ttt); } return; } /* pktt != NULL, calcuate the rx *delivery* throughput */ u32 remainder = pktt->rx_deliver_timestamp % (u32) MS2US(S2MS(1)); u32 integer = (pktt->rx_deliver_timestamp - remainder) / MS2US(S2MS(1)); rx_throughput_t * ttt = (rx_throughput_t*) DLLIST_TAIL(&rx_tpt_list); if (DLLIST_IS_HEAD(&rx_tpt_list, ttt) || ttt->time != integer) { /* new one */ rx_throughput_t *new_ttt = (rx_throughput_t*) malloc(sizeof(rx_throughput_t)); assert(new_ttt); new_ttt->time = integer; new_ttt->throughput = pktt->packet_size; dllist_append(&rx_tpt_list, &(new_ttt->node)); } else { /* already existed, update it */ ttt->throughput += (pktt->packet_size * OCTET); } }
static void tachSignalCallback(trigger_event_e ckpSignalType, uint32_t index DECLARE_ENGINE_PARAMETER_SUFFIX) { if (index != engineConfiguration->tachPulseTriggerIndex) { return; } enginePins.tachOut.setHigh(); float durationMs; if (engineConfiguration->tachPulseDurationAsDutyCycle) { // todo: implement tachPulseDurationAsDutyCycle durationMs = engineConfiguration->tachPulseDuractionMs; } else { durationMs = engineConfiguration->tachPulseDuractionMs; } engine->executor.scheduleForLater(&tachTurnSignalOff, (int)MS2US(durationMs), (schfunc_t) &turnTachPinLow, NULL); }
/** * @return Next time for signal toggle */ efitimeus_t PwmConfig::togglePwmState() { #if DEBUG_PWM scheduleMsg(&logger, "togglePwmState phaseIndex=%d iteration=%d", safe.phaseIndex, safe.iteration); scheduleMsg(&logger, "period=%.2f safe.period=%.2f", period, safe.period); #endif if (cisnan(periodNt)) { /** * NaN period means PWM is paused */ return getTimeNowUs() + MS2US(100); } handleCycleStart(); /** * Here is where the 'business logic' - the actual pin state change is happening */ // callback state index is offset by one. todo: why? can we simplify this? int cbStateIndex = safe.phaseIndex == 0 ? phaseCount - 1 : safe.phaseIndex - 1; stateChangeCallback(this, cbStateIndex); efitimeus_t nextSwitchTimeUs = getNextSwitchTimeUs(this); #if DEBUG_PWM scheduleMsg(&logger, "%s: nextSwitchTime %d", state->name, nextSwitchTime); #endif /* DEBUG_PWM */ // signed value is needed here // int64_t timeToSwitch = nextSwitchTimeUs - getTimeNowUs(); // if (timeToSwitch < 1) { // /** // * We are here if we are late for a state transition. // * At 12000RPM=200Hz with a 60 toothed wheel we need to change state every // * 1000000 / 200 / 120 = ~41 uS. We are kind of OK. // * // * We are also here after a flash write. Flash write freezes the whole chip for a couple of seconds, // * so PWM generation and trigger simulation generation would have to recover from this time lag. // */ // //todo: introduce error and test this error handling warning(OBD_PCM_Processor_Fault, "PWM: negative switch time"); // timeToSwitch = 10; // } safe.phaseIndex++; if (safe.phaseIndex == phaseCount) { safe.phaseIndex = 0; // restart safe.iteration++; } return nextSwitchTimeUs; }
static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs) { const float swingVelocity = (ABS(imuMeasuredRotationBF.A[Z]) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.A[Y] / imuMeasuredRotationBF.A[Z]) : 0; const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.A[X] > navConfig()->fw.launch_accel_thresh); const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle))); const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.A[X] > 0); if (isBungeeLaunched || isSwingLaunched) { launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviosUpdate); launchState.launchDetectorPreviosUpdate = currentTimeUs; if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)navConfig()->fw.launch_time_thresh)) { launchState.launchDetected = true; } } else { launchState.launchDetectorPreviosUpdate = currentTimeUs; launchState.launchDetectionTimeAccum = 0; } }
static msg_t seThread(void *arg) { (void)arg; chRegSetThreadName("servo"); while (true) { OutputPin *pin = &pins[0]; pin->setValue(1); percent_t position = (currentTimeMillis() / 5) % 200; if (position > 100) position = 200 - position; float durationMs = 0 + position * 0.02f; scheduleForLater(&servoTurnSignalOff, (int)MS2US(durationMs), (schfunc_t) &servoTachPinLow, pin); chThdSleepMilliseconds(19); } return 0; }
void initPwmTester(void) { initLogging(&logger, "pwm test"); addConsoleActionI("pwmtest", startPwmTest); startPwmTest(1000); /** * injector channels #4-#8 are used for individual squirt test */ // todo: yet, it's some horrible code duplication outSignals[0].output = &enginePins.injectors[4]; outSignals[1].output = &enginePins.injectors[5]; outSignals[2].output = &enginePins.injectors[6]; outSignals[3].output = &enginePins.injectors[7]; outSignals[4].output = &enginePins.injectors[8]; outSignals[5].output = &enginePins.injectors[9]; outSignals[6].output = &enginePins.injectors[10]; outSignals[7].output = &enginePins.injectors[11]; /** * this would schedule a callback in 2ms from now */ scheduleTask("test", &ioTest, MS2US(2), testCallback, NULL); }
/** * Calculate next estimate using IMU and apply corrections from reference sensors (GPS, BARO etc) * Function is called at main loop rate */ static void updateEstimatedTopic(uint32_t currentTime) { t_fp_vector accelBiasCorr; float dt = US2S(currentTime - posEstimator.est.lastUpdateTime); posEstimator.est.lastUpdateTime = currentTime; /* If IMU is not ready we can't estimate anything */ if (!isImuReady()) { posEstimator.est.eph = posControl.navConfig->inav.max_eph_epv + 0.001f; posEstimator.est.epv = posControl.navConfig->inav.max_eph_epv + 0.001f; return; } /* increase EPH/EPV on each iteration */ if (posEstimator.est.eph <= posControl.navConfig->inav.max_eph_epv) { posEstimator.est.eph *= 1.0f + dt; } if (posEstimator.est.epv <= posControl.navConfig->inav.max_eph_epv) { posEstimator.est.epv *= 1.0f + dt; } /* Figure out if we have valid position data from our data sources */ bool isGPSValid = sensors(SENSOR_GPS) && posControl.gpsOrigin.valid && ((currentTime - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)); bool isBaroValid = sensors(SENSOR_BARO) && ((currentTime - posEstimator.baro.lastUpdateTime) <= MS2US(INAV_BARO_TIMEOUT_MS)); bool isSonarValid = sensors(SENSOR_SONAR) && ((currentTime - posEstimator.sonar.lastUpdateTime) <= MS2US(INAV_SONAR_TIMEOUT_MS)); /* Do some preparations to data */ if (isBaroValid) { if (!ARMING_FLAG(ARMED)) { posEstimator.state.baroGroundAlt = posEstimator.est.pos.V.Z; posEstimator.state.isBaroGroundValid = true; posEstimator.state.baroGroundTimeout = currentTime + 250000; // 0.25 sec } else { if (posEstimator.est.vel.V.Z > 15) { if (currentTime > posEstimator.state.baroGroundTimeout) { posEstimator.state.isBaroGroundValid = false; } } else { posEstimator.state.baroGroundTimeout = currentTime + 250000; // 0.25 sec } } } else { posEstimator.state.isBaroGroundValid = false; } /* We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it */ bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) && ((isSonarValid && posEstimator.sonar.alt < 20.0f && posEstimator.state.isBaroGroundValid) || (isBaroValid && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt)); #if defined(NAV_GPS_GLITCH_DETECTION) //isGPSValid = isGPSValid && !posEstimator.gps.glitchDetected; #endif /* Apply GPS altitude corrections only on fixed wing aircrafts */ bool useGpsZ = STATE(FIXED_WING) && isGPSValid; /* Pre-calculate history index for GPS delay compensation */ int gpsHistoryIndex = (posEstimator.history.index - 1) - constrain(((int)posControl.navConfig->inav.gps_delay_ms / (1000 / INAV_POSITION_PUBLISH_RATE_HZ)), 0, INAV_HISTORY_BUF_SIZE - 1); if (gpsHistoryIndex < 0) { gpsHistoryIndex += INAV_HISTORY_BUF_SIZE; } /* Correct accelerometer bias */ if (posControl.navConfig->inav.w_acc_bias > 0) { accelBiasCorr.V.X = 0; accelBiasCorr.V.Y = 0; accelBiasCorr.V.Z = 0; /* accelerometer bias correction for GPS */ if (isGPSValid) { accelBiasCorr.V.X -= (posEstimator.gps.pos.V.X - posEstimator.history.pos[gpsHistoryIndex].V.X) * sq(posControl.navConfig->inav.w_xy_gps_p); accelBiasCorr.V.X -= (posEstimator.gps.vel.V.X - posEstimator.history.vel[gpsHistoryIndex].V.X) * posControl.navConfig->inav.w_xy_gps_v; accelBiasCorr.V.Y -= (posEstimator.gps.pos.V.Y - posEstimator.history.pos[gpsHistoryIndex].V.Y) * sq(posControl.navConfig->inav.w_xy_gps_p); accelBiasCorr.V.Y -= (posEstimator.gps.vel.V.Y - posEstimator.history.vel[gpsHistoryIndex].V.Y) * posControl.navConfig->inav.w_xy_gps_v; if (useGpsZ) { accelBiasCorr.V.Z -= (posEstimator.gps.pos.V.Z - posEstimator.history.pos[gpsHistoryIndex].V.Z) * sq(posControl.navConfig->inav.w_z_gps_p); accelBiasCorr.V.Z -= (posEstimator.gps.vel.V.Z - posEstimator.history.vel[gpsHistoryIndex].V.Z) * posControl.navConfig->inav.w_z_gps_v; } } /* accelerometer bias correction for baro */ if (isBaroValid && !isAirCushionEffectDetected) { accelBiasCorr.V.Z -= (posEstimator.baro.alt - posEstimator.est.pos.V.Z) * sq(posControl.navConfig->inav.w_z_baro_p); } /* transform error vector from NEU frame to body frame */ imuTransformVectorEarthToBody(&accelBiasCorr); /* Correct accel bias */ posEstimator.imu.accelBias.V.X += accelBiasCorr.V.X * posControl.navConfig->inav.w_acc_bias * dt; posEstimator.imu.accelBias.V.Y += accelBiasCorr.V.Y * posControl.navConfig->inav.w_acc_bias * dt; posEstimator.imu.accelBias.V.Z += accelBiasCorr.V.Z * posControl.navConfig->inav.w_acc_bias * dt; } /* Estimate Z-axis */ if ((posEstimator.est.epv < posControl.navConfig->inav.max_eph_epv) || useGpsZ || isBaroValid) { /* Predict position/velocity based on acceleration */ inavFilterPredict(Z, dt, posEstimator.imu.accelNEU.V.Z); #if defined(BARO) if (isBaroValid) { float baroError = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.V.Z; /* Apply only baro correction, no sonar */ inavFilterCorrectPos(Z, dt, baroError, posControl.navConfig->inav.w_z_baro_p); /* Adjust EPV */ posEstimator.est.epv = MIN(posEstimator.est.epv, posEstimator.baro.epv); } #endif /* Apply GPS correction to altitude */ if (useGpsZ) { inavFilterCorrectPos(Z, dt, posEstimator.gps.pos.V.Z - posEstimator.history.pos[gpsHistoryIndex].V.Z, posControl.navConfig->inav.w_z_gps_p); inavFilterCorrectVel(Z, dt, posEstimator.gps.vel.V.Z - posEstimator.history.vel[gpsHistoryIndex].V.Z, posControl.navConfig->inav.w_z_gps_v); /* Adjust EPV */ posEstimator.est.epv = MIN(posEstimator.est.epv, posEstimator.gps.epv); } } else { inavFilterCorrectVel(Z, dt, 0.0f - posEstimator.est.vel.V.Z, posControl.navConfig->inav.w_z_res_v); } /* Estimate XY-axis only if heading is valid (X-Y acceleration is North-East)*/ if ((posEstimator.est.eph < posControl.navConfig->inav.max_eph_epv) || isGPSValid) { if (isImuHeadingValid()) { inavFilterPredict(X, dt, posEstimator.imu.accelNEU.V.X); inavFilterPredict(Y, dt, posEstimator.imu.accelNEU.V.Y); } /* Correct position from GPS - always if GPS is valid */ if (isGPSValid) { inavFilterCorrectPos(X, dt, posEstimator.gps.pos.V.X - posEstimator.history.pos[gpsHistoryIndex].V.X, posControl.navConfig->inav.w_xy_gps_p); inavFilterCorrectPos(Y, dt, posEstimator.gps.pos.V.Y - posEstimator.history.pos[gpsHistoryIndex].V.Y, posControl.navConfig->inav.w_xy_gps_p); inavFilterCorrectVel(X, dt, posEstimator.gps.vel.V.X - posEstimator.history.vel[gpsHistoryIndex].V.X, posControl.navConfig->inav.w_xy_gps_v); inavFilterCorrectVel(Y, dt, posEstimator.gps.vel.V.Y - posEstimator.history.vel[gpsHistoryIndex].V.Y, posControl.navConfig->inav.w_xy_gps_v); /* Adjust EPH */ posEstimator.est.eph = MIN(posEstimator.est.eph, posEstimator.gps.eph); } } else { inavFilterCorrectVel(X, dt, 0.0f - posEstimator.est.vel.V.X, posControl.navConfig->inav.w_xy_res_v); inavFilterCorrectVel(Y, dt, 0.0f - posEstimator.est.vel.V.Y, posControl.navConfig->inav.w_xy_res_v); } /* Surface offset */ #if defined(SONAR) if (isSonarValid) { posEstimator.est.surface = posEstimator.sonar.alt; posEstimator.est.surfaceVel = posEstimator.sonar.vel; } else { posEstimator.est.surface = -1; posEstimator.est.surfaceVel = 0; } #else posEstimator.est.surface = -1; posEstimator.est.surfaceVel = 0; #endif }
/** * Update GPS topic * Function is called on each GPS update */ void onNewGPSData(void) { static uint32_t lastGPSNewDataTime; static int32_t previousLat; static int32_t previousLon; static int32_t previousAlt; static bool isFirstGPSUpdate = true; gpsLocation_t newLLH; uint32_t currentTime = micros(); newLLH.lat = gpsSol.llh.lat; newLLH.lon = gpsSol.llh.lon; newLLH.alt = gpsSol.llh.alt; if (sensors(SENSOR_GPS)) { if (!(STATE(GPS_FIX) && gpsSol.numSat >= posControl.navConfig->inav.gps_min_sats)) { isFirstGPSUpdate = true; return; } if ((currentTime - lastGPSNewDataTime) > MS2US(INAV_GPS_TIMEOUT_MS)) { isFirstGPSUpdate = true; } #if defined(NAV_AUTO_MAG_DECLINATION) /* Automatic magnetic declination calculation - do this once */ static bool magDeclinationSet = false; if (posControl.navConfig->inav.automatic_mag_declination && !magDeclinationSet) { magneticDeclination = geoCalculateMagDeclination(&newLLH) * 10.0f; // heading is in 0.1deg units magDeclinationSet = true; } #endif /* Process position update if GPS origin is already set, or precision is good enough */ // FIXME: use HDOP here if ((posControl.gpsOrigin.valid) || (gpsSol.numSat >= posControl.navConfig->inav.gps_min_sats)) { /* Convert LLH position to local coordinates */ geoConvertGeodeticToLocal(&posControl.gpsOrigin, &newLLH, & posEstimator.gps.pos, GEO_ALT_ABSOLUTE); /* If not the first update - calculate velocities */ if (!isFirstGPSUpdate) { float dT = US2S(getGPSDeltaTimeFilter(currentTime - lastGPSNewDataTime)); /* Use VELNED provided by GPS if available, calculate from coordinates otherwise */ float gpsScaleLonDown = constrainf(cos_approx((ABS(gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f), 0.01f, 1.0f); if (posControl.navConfig->inav.use_gps_velned && gpsSol.flags.validVelNE) { posEstimator.gps.vel.V.X = gpsSol.velNED[0]; posEstimator.gps.vel.V.Y = gpsSol.velNED[1]; } else { posEstimator.gps.vel.V.X = (posEstimator.gps.vel.V.X + (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * (gpsSol.llh.lat - previousLat) / dT)) / 2.0f; posEstimator.gps.vel.V.Y = (posEstimator.gps.vel.V.Y + (gpsScaleLonDown * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * (gpsSol.llh.lon - previousLon) / dT)) / 2.0f; } if (posControl.navConfig->inav.use_gps_velned && gpsSol.flags.validVelD) { posEstimator.gps.vel.V.Z = - gpsSol.velNED[2]; // NEU } else { posEstimator.gps.vel.V.Z = (posEstimator.gps.vel.V.Z + (gpsSol.llh.alt - previousAlt) / dT) / 2.0f; } #if defined(NAV_GPS_GLITCH_DETECTION) /* GPS glitch protection. We have local coordinates and local velocity for current GPS update. Check if they are sane */ if (detectGPSGlitch(currentTime)) { posEstimator.gps.glitchRecovery = false; posEstimator.gps.glitchDetected = true; } else { /* Store previous glitch flag in glitchRecovery to indicate a valid reading after a glitch */ posEstimator.gps.glitchRecovery = posEstimator.gps.glitchDetected; posEstimator.gps.glitchDetected = false; } #endif /* FIXME: use HDOP/VDOP */ posEstimator.gps.eph = INAV_GPS_EPH; posEstimator.gps.epv = INAV_GPS_EPV; /* Indicate a last valid reading of Pos/Vel */ posEstimator.gps.lastUpdateTime = currentTime; } previousLat = gpsSol.llh.lat; previousLon = gpsSol.llh.lon; previousAlt = gpsSol.llh.alt; isFirstGPSUpdate = false; lastGPSNewDataTime = currentTime; } } else { posEstimator.gps.lastUpdateTime = 0; } }
TEST(cranking, testFasterEngineSpinningUp) { printf("*************************************************** testFasterEngineSpinningUp\r\n"); WITH_ENGINE_TEST_HELPER(TEST_ENGINE); // turn on FasterEngineSpinUp mode engineConfiguration->bc.isFasterEngineSpinUpEnabled = true; // set ignition mode engineConfiguration->ignitionMode = IM_INDIVIDUAL_COILS; // set cranking threshold (used below) engineConfiguration->cranking.rpm = 999; // set sequential injection mode to test auto-change to simultaneous when spinning-up setupSimpleTestEngineWithMafAndTT_ONE_trigger(ð, IM_SEQUENTIAL); ASSERT_EQ(IM_INDIVIDUAL_COILS, getCurrentIgnitionMode(PASS_ENGINE_PARAMETER_SIGNATURE)); eth.moveTimeForwardMs(1000 /*ms*/); eth.firePrimaryTriggerRise(); // check if it's true ASSERT_EQ(IM_SEQUENTIAL, engine->getCurrentInjectionMode(PASS_ENGINE_PARAMETER_SIGNATURE)); ASSERT_EQ(IM_WASTED_SPARK, getCurrentIgnitionMode(PASS_ENGINE_PARAMETER_SIGNATURE)); // check if the engine has the right state ASSERT_EQ(SPINNING_UP, engine->rpmCalculator.getState()); // check RPM ASSERT_EQ( 0, GET_RPM()) << "RPM=0"; // the queue should be empty, no trigger events yet ASSERT_EQ(0, engine->executor.size()) << "plain#1"; // check all events starting from now int timeStartUs = eth.getTimeNowUs(); // advance 1 revolution // because we have trivial TT_ONE trigger here synchronization would happen with just one rise front eth.fireRise(200); // check if the mode is changed ASSERT_EQ(SPINNING_UP, engine->rpmCalculator.getState()); // due to isFasterEngineSpinUp=true, we should have already detected RPM! ASSERT_EQ( 300, GET_RPM()) << "spinning-RPM#1"; // two simultaneous injections ASSERT_EQ(4, engine->executor.size()) << "plain#2"; // test if they are simultaneous ASSERT_EQ(IM_SIMULTANEOUS, engine->getCurrentInjectionMode(PASS_ENGINE_PARAMETER_SIGNATURE)); // test if ignition mode is temporary changed to wasted spark, if set to individual coils ASSERT_EQ(IM_WASTED_SPARK, getCurrentIgnitionMode(PASS_ENGINE_PARAMETER_SIGNATURE)); // check real events eth.assertEvent5(&engine->executor, "inj start#1", 0, (void*)startSimultaniousInjection, timeStartUs, MS2US(200) + 81250); eth.assertEvent5(&engine->executor, "inj end#1", 1, (void*)endSimultaniousInjection, timeStartUs, MS2US(200) + 100000); // skip the rest of the cycle eth.fireFall(200); // now clear and advance more eth.clearQueue(); eth.fireRise(200); // check if the mode is changed when fully synched ASSERT_EQ(CRANKING, engine->rpmCalculator.getState()); // check RPM ASSERT_EQ( 200, GET_RPM()) << "RPM#2"; // test if they are simultaneous in cranking mode too ASSERT_EQ(IM_SIMULTANEOUS, engine->getCurrentInjectionMode(PASS_ENGINE_PARAMETER_SIGNATURE)); // test if ignition mode is restored to ind.coils ASSERT_EQ(IM_INDIVIDUAL_COILS, getCurrentIgnitionMode(PASS_ENGINE_PARAMETER_SIGNATURE)); // two simultaneous injections ASSERT_EQ( 4, engine->executor.size()) << "plain#2"; // check real events eth.assertEvent5(&engine->executor, "inj start#2", 0, (void*)startSimultaniousInjection, eth.getTimeNowUs(), 131250); eth.assertEvent5(&engine->executor, "inj end#2", 1, (void*)endSimultaniousInjection, eth.getTimeNowUs(), 150000); // skip, clear & advance 1 more revolution at higher RPM eth.fireFall(60); eth.clearQueue(); timeStartUs = eth.getTimeNowUs(); eth.fireTriggerEventsWithDuration(60); // check if the mode is now changed to 'running' at higher RPM ASSERT_EQ(RUNNING, engine->rpmCalculator.getState()); // check RPM ASSERT_EQ( 1000, GET_RPM()) << "RPM#3"; // check if the injection mode is back to sequential now ASSERT_EQ(IM_SEQUENTIAL, engine->getCurrentInjectionMode(PASS_ENGINE_PARAMETER_SIGNATURE)); // 4 sequential injections for the full cycle ASSERT_EQ( 8, engine->executor.size()) << "plain#3"; // check real events for sequential injection // Note: See addFuelEvents() fix inside setRpmValue()! eth.assertEvent5(&engine->executor, "inj start#3", 0, (void*)seTurnPinHigh, timeStartUs, MS2US(60) + 11250); eth.assertEvent5(&engine->executor, "inj end#3", 1, (void*)seTurnPinLow, timeStartUs, MS2US(60) + 11250 + 3000); }
void startIdleBench(void) { timeToStopIdleTest = getTimeNowUs() + MS2US(3000); // 3 seconds scheduleMsg(logger, "idle valve bench test"); showIdleInfo(); }
int main (int argc, char *argv[]) { /* 1. acquire all simulation parameters or use the default values 2. generate the simulation begin event @time = 0 3. add this event to the timer queue 4. while (simulatio not finished) { advance the simu time by 1 time unit (ms) } */ /* 1. */ simu_paras_t spt; spt.t.packet_size = PKT_SIZE; /* bytes */ spt.rl.link_distance = 0; spt.rl.prop_delay = MS2US(270); /* 270 ms */ spt.rl.link_bandwidth = BANDWIDTH; /* bps */ spt.rl.per = PER; /* x/10000 */ /* this should be set when the mac pdu is build (at the tx_begin_event) */ /* spt.tx_delay = ( (1e3 * OCTET * (spt.t.packet_size + MAC_HEADER_SIZE + PHY_HEADER_SIZE)) / spt.rl.link_bandwidth ); */ /* rlc params */ spt.rlc_paras.ump.t_Reordering = MS2US(T_REORDERING); /* ms */ spt.rlc_paras.ump.UM_Window_Size = UWSize; /* window size */ spt.rlc_paras.ump.sn_FieldLength = SN_LEN; /* sn length */ #define PTIMER_MEM_MAX 4096*16 spt.g_mem_ptimer_t = fastalloc_create(sizeof(ptimer_t), PTIMER_MEM_MAX, 0, 100); assert(spt.g_mem_ptimer_t); /* #define PACKET_MEM_MAX (4096*16*16*8) */ /* spt.g_mem_packet_t = fastalloc_create(sizeof(packet_t), PACKET_MEM_MAX, 0, 100); */ /* assert(spt.g_mem_packet_t); */ /* @transmitter */ /* @receiver */ /* leave these to be done at the simulatioin begin event */ /* init log */ zlog_default = openzlog(ZLOG_STDOUT); zlog_set_pri(zlog_default, LOG_INFO); zlog_reset_flag(zlog_default, ZLOG_LOGTIME); /* no time display */ ZLOG_DEBUG("pkt size = %d, prop delay = %d, link bandwidth = %d\n", spt.t.packet_size, spt.rl.prop_delay, spt.rl.link_bandwidth); /* 2. */ /* FIXME: simu time unit: 1us! */ ptimer_t simu_begin_timer = { .duration = 0, .onexpired_func = simu_begin_event, .param[0] = (void*) &spt, .param[1] = (void*) SIMU_BEGIN, }; time_t t; srand((unsigned) time(&t)); /* 3. */ rlc_init(); rlc_timer_start(&simu_begin_timer); /* 4. */ int step_in_us = 1; while (g_is_finished == NOT_FINISHED && g_time_elasped_in_us <= MS2US(S2MS(SIMU_TIME)) ) { rlc_timer_push(step_in_us); /* us */ g_time_elasped_in_us += step_in_us; if ( g_time_elasped_in_us % (int)MS2US(S2MS(1)) == 0 ) { ZLOG_INFO("simu time = %f\n", g_time_elasped_in_us/MS2US(S2MS(1))); } } /* output the simu result */ ZLOG_DEBUG("time_elasped_in_us = %d\n", g_time_elasped_in_us); int cnt = 0; int output_e2e_delay = 1; int n_rxok = 0, n_rxerr = 0; while (!DLLIST_EMPTY(&g_sink_packet_list)) { packet_t *pktt = (packet_t*) DLLIST_HEAD(&g_sink_packet_list); /* 1. tx throughput */ // output_tx_throughput(pktt); // output_rx_throughput(pktt); /* 2. e2e delay */ switch (pktt->ptt) { case RX_OK: n_rxok++; break; case RX_ERR: n_rxerr++; break; default: assert(0); break; } if( output_e2e_delay ) { if (pktt->ptt == RX_OK ) ZLOG_INFO("SN, buffering: %u, %u\n", pktt->sequence_no, pktt->rx_deliver_timestamp - pktt->rx_end_timestamp); } dllist_remove(&g_sink_packet_list, &(pktt->node)); cnt++; // FASTFREE(spt.g_mem_packet_t, pktt); free(pktt); pktt = NULL; } output_tx_throughput(NULL); output_rx_throughput(NULL); ZLOG_INFO("n_txed = %d, n_rxok = %d, n_rxerr = %d\n", cnt, n_rxok, n_rxerr); return 0; }
void output_tx_throughput(packet_t *pktt) { /* unit: second, based on the pktt->tx_begin_timestamp, the pktt->tx_end_timestamp */ static dllist_node_t tx_tpt_list = { .prev = &tx_tpt_list, .next = &tx_tpt_list }; if (pktt == NULL) { /* output the result */ while (!DLLIST_EMPTY(&tx_tpt_list)) { tx_throughput_t *ttt = (tx_throughput_t*) DLLIST_HEAD(&tx_tpt_list); ZLOG_INFO("tx tpt: at [%d, %d)s, throughput %d bps\n", ttt->time, ttt->time + 1, ttt->throughput /* bps */); dllist_remove(&tx_tpt_list, &(ttt->node)); free(ttt); } return; } /* pktt != NULL, calcuate the tx throughput */ u32 remainder_begin = pktt->tx_begin_timestamp % (u32) MS2US(S2MS(1)); u32 remainder_end = pktt->tx_end_timestamp % (u32) (MS2US(S2MS(1))); u32 integer_begin = (pktt->tx_begin_timestamp - remainder_begin) / MS2US(S2MS(1)); u32 integer_end = (pktt->tx_end_timestamp - remainder_end) / MS2US(S2MS(1)); tx_throughput_t * ttt = (tx_throughput_t*) DLLIST_TAIL(&tx_tpt_list); if ( integer_begin == integer_end ) { /* in the same time range */ /* if have, get it */ if (DLLIST_IS_HEAD(&tx_tpt_list, ttt) || ttt->time != integer_begin) { /* new one */ tx_throughput_t *new_ttt = (tx_throughput_t*) malloc(sizeof(tx_throughput_t)); assert(new_ttt); new_ttt->time = integer_begin; new_ttt->throughput = pktt->mac_pdu_size * OCTET; dllist_append(&tx_tpt_list, &(new_ttt->node)); } else { /* already existed, update it */ ttt->throughput += (pktt->mac_pdu_size * OCTET); } } else { /* not in the same time range, split it based on the pktt->mac_pdu_size */ assert(integer_end - integer_begin == 1); u32 total = pktt->tx_end_timestamp - pktt->tx_begin_timestamp; if (DLLIST_IS_HEAD(&tx_tpt_list, ttt)) { ttt = (tx_throughput_t*) malloc(sizeof(tx_throughput_t)); dllist_append(&tx_tpt_list, &(ttt->node)); } u32 part = (pktt->mac_pdu_size * OCTET) * remainder_end / total; tx_throughput_t *new_ttt = (tx_throughput_t*) malloc(sizeof(tx_throughput_t)); assert(new_ttt); new_ttt->time = integer_end; new_ttt->throughput = (pktt->mac_pdu_size * OCTET) * remainder_end / total; dllist_append(&tx_tpt_list, &(new_ttt->node)); ttt->time = integer_begin; ttt->throughput += (pktt->mac_pdu_size * OCTET) * (1 - remainder_end / total); ZLOG_DEBUG("begin %d, remainder %d, tpt: %d, end %d, remainder %d, tpt: %d\n", integer_begin, remainder_begin, pktt->mac_pdu_size * OCTET - part, integer_end, remainder_end, part); } }