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);
}
}
