fresh version

This commit is contained in:
Andrey B 2014-08-23 18:37:21 -04:00
parent fa6f88dc03
commit 34f2334652
201 changed files with 5174 additions and 1853 deletions

View File

@ -78,7 +78,7 @@
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/chibios/os/ports/common/ARMCMx}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/chibios/os/ports/common/ARMCMx}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/chibios/os/ports/common/ARMCMx/CMSIS/include}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/chibios/os/ports/common/ARMCMx/CMSIS/include}&quot;"/>
</option> </option>
<option id="org.eclipse.cdt.cross.arm.gnu.c.compiler.option.other.otherflags.720321025" name="Other flags" superClass="org.eclipse.cdt.cross.arm.gnu.c.compiler.option.other.otherflags" value="-fgnu89-inline -c -fmessage-length=0 -Werror=type-limits -Werror-implicit-function-declaration -Werror -Wno-error=pointer-sign -Wno-error=unused-function -Wno-error=unused-variable" valueType="string"/> <option id="org.eclipse.cdt.cross.arm.gnu.c.compiler.option.other.otherflags.720321025" name="Other flags" superClass="org.eclipse.cdt.cross.arm.gnu.c.compiler.option.other.otherflags" value="-fgnu89-inline -c -fmessage-length=0 -Werror=switch -Werror=type-limits -Werror-implicit-function-declaration -Werror -Wno-error=pointer-sign -Wno-error=unused-function -Wno-error=unused-variable" valueType="string"/>
<option id="org.eclipse.cdt.cross.arm.gnu.c.compiler.option.misc.std.32943169" name="Language Standard" superClass="org.eclipse.cdt.cross.arm.gnu.c.compiler.option.misc.std" value="org.eclipse.cdt.cross.arm.gnu.c.compiler.option.misc.std.gnu99" valueType="enumerated"/> <option id="org.eclipse.cdt.cross.arm.gnu.c.compiler.option.misc.std.32943169" name="Language Standard" superClass="org.eclipse.cdt.cross.arm.gnu.c.compiler.option.misc.std" value="org.eclipse.cdt.cross.arm.gnu.c.compiler.option.misc.std.gnu99" valueType="enumerated"/>
<option id="org.eclipse.cdt.cross.arm.gnu.c.compiler.option.preprocessor.def.1323722868" name="Defined symbols (-D)" superClass="org.eclipse.cdt.cross.arm.gnu.c.compiler.option.preprocessor.def" valueType="definedSymbols"> <option id="org.eclipse.cdt.cross.arm.gnu.c.compiler.option.preprocessor.def.1323722868" name="Defined symbols (-D)" superClass="org.eclipse.cdt.cross.arm.gnu.c.compiler.option.preprocessor.def" valueType="definedSymbols">
<listOptionValue builtIn="false" value="CORTEX_USE_FPU=TRUE"/> <listOptionValue builtIn="false" value="CORTEX_USE_FPU=TRUE"/>
@ -115,6 +115,7 @@
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/emulation/hw_layer}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/emulation/hw_layer}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/hw_layer}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/hw_layer}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/hw_layer/flash}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/hw_layer/flash}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/hw_layer/stm32f4}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/russianefi}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/russianefi}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/russianefi/algo}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/russianefi/algo}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/russianefi/ckp}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/russianefi/ckp}&quot;"/>
@ -139,7 +140,7 @@
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/chibios/os/ports/common/ARMCMx}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/chibios/os/ports/common/ARMCMx}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/chibios/os/ports/common/ARMCMx/CMSIS/include}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/chibios/os/ports/common/ARMCMx/CMSIS/include}&quot;"/>
</option> </option>
<option id="org.eclipse.cdt.cross.arm.gnu.cpp.compiler.option.other.otherflags.1439013659" name="Other flags" superClass="org.eclipse.cdt.cross.arm.gnu.cpp.compiler.option.other.otherflags" value="-c -fmessage-length=0 -std=c++11 -Werror=write-strings" valueType="string"/> <option id="org.eclipse.cdt.cross.arm.gnu.cpp.compiler.option.other.otherflags.1439013659" name="Other flags" superClass="org.eclipse.cdt.cross.arm.gnu.cpp.compiler.option.other.otherflags" value="-c -fmessage-length=0 -std=c++11 -Werror=write-strings -Werror=switch" valueType="string"/>
<option id="org.eclipse.cdt.cross.arm.gnu.cpp.compiler.option.preprocessor.def.1500938905" name="Defined symbols (-D)" superClass="org.eclipse.cdt.cross.arm.gnu.cpp.compiler.option.preprocessor.def" valueType="definedSymbols"> <option id="org.eclipse.cdt.cross.arm.gnu.cpp.compiler.option.preprocessor.def.1500938905" name="Defined symbols (-D)" superClass="org.eclipse.cdt.cross.arm.gnu.cpp.compiler.option.preprocessor.def" valueType="definedSymbols">
<listOptionValue builtIn="false" value="CORTEX_USE_FPU=TRUE"/> <listOptionValue builtIn="false" value="CORTEX_USE_FPU=TRUE"/>
</option> </option>
@ -303,10 +304,10 @@
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/chibios/os/ports/common/ARMCMx}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/chibios/os/ports/common/ARMCMx}&quot;"/>
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/chibios/os/ports/common/ARMCMx/CMSIS/include}&quot;"/> <listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/chibios/os/ports/common/ARMCMx/CMSIS/include}&quot;"/>
</option> </option>
<option id="org.eclipse.cdt.cross.arm.gnu.cpp.compiler.option.preprocessor.def.104404165" superClass="org.eclipse.cdt.cross.arm.gnu.cpp.compiler.option.preprocessor.def" valueType="definedSymbols"> <option id="org.eclipse.cdt.cross.arm.gnu.cpp.compiler.option.preprocessor.def.104404165" name="Defined symbols (-D)" superClass="org.eclipse.cdt.cross.arm.gnu.cpp.compiler.option.preprocessor.def" valueType="definedSymbols">
<listOptionValue builtIn="false" value="CORTEX_USE_FPU=TRUE"/> <listOptionValue builtIn="false" value="CORTEX_USE_FPU=TRUE"/>
</option> </option>
<option id="org.eclipse.cdt.cross.arm.gnu.cpp.compiler.option.optimization.flags.514837677" superClass="org.eclipse.cdt.cross.arm.gnu.cpp.compiler.option.optimization.flags" value="-mfloat-abi=softfp -mfpu=fpv4-sp-d16 -fsingle-precision-constant" valueType="string"/> <option id="org.eclipse.cdt.cross.arm.gnu.cpp.compiler.option.optimization.flags.514837677" name="Other optimization flags" superClass="org.eclipse.cdt.cross.arm.gnu.cpp.compiler.option.optimization.flags" value="-mfloat-abi=softfp -mfpu=fpv4-sp-d16 -fsingle-precision-constant" valueType="string"/>
<inputType id="org.eclipse.cdt.cross.arm.gnu.sourcery.windows.cpp.compiler.base.input.815685204" superClass="org.eclipse.cdt.cross.arm.gnu.sourcery.windows.cpp.compiler.base.input"/> <inputType id="org.eclipse.cdt.cross.arm.gnu.sourcery.windows.cpp.compiler.base.input.815685204" superClass="org.eclipse.cdt.cross.arm.gnu.sourcery.windows.cpp.compiler.base.input"/>
</tool> </tool>
<tool id="org.eclipse.cdt.cross.arm.gnu.sourcery.windows.elf.c.linker.release.1692652942" name="ARM Sourcery Windows GCC C Linker" superClass="org.eclipse.cdt.cross.arm.gnu.sourcery.windows.elf.c.linker.release"/> <tool id="org.eclipse.cdt.cross.arm.gnu.sourcery.windows.elf.c.linker.release.1692652942" name="ARM Sourcery Windows GCC C Linker" superClass="org.eclipse.cdt.cross.arm.gnu.sourcery.windows.elf.c.linker.release"/>

View File

@ -196,6 +196,7 @@ INCDIR = $(PORTINC) $(KERNINC) $(TESTINC) \
hw_layer/serial_over_usb \ hw_layer/serial_over_usb \
hw_layer/algo \ hw_layer/algo \
hw_layer/lcd \ hw_layer/lcd \
hw_layer/stm32f4 \
emulation \ emulation \
emulation/hw_layer \ emulation/hw_layer \
emulation/test \ emulation/test \

View File

@ -220,14 +220,10 @@
#define EFI_SIGNAL_EXECUTOR_ONE_TIMER TRUE #define EFI_SIGNAL_EXECUTOR_ONE_TIMER TRUE
#define EFI_SIGNAL_EXECUTOR_HW_TIMER FALSE #define EFI_SIGNAL_EXECUTOR_HW_TIMER FALSE
// USART1 -> check defined STM32_SERIAL_USE_USART1 // USART1 -> check defined STM32_SERIAL_USE_USART1
// For GPS we have USART1. We can start with PB7 USART1_RX and PB6 USART1_TX // For GPS we have USART1. We can start with PB7 USART1_RX and PB6 USART1_TX
#define GPS_SERIAL_DEVICE &SD1 #define GPS_SERIAL_DEVICE &SD1
#define GPS_SERIAL_SPEED 38400 #define GPS_SERIAL_SPEED 38400
#define GPS_PORT GPIOB
#define GPS_SERIAL_TX_PIN 6
#define GPS_SERIAL_RX_PIN 7
#define CONSOLE_MODE_SWITCH_PORT GPIOB #define CONSOLE_MODE_SWITCH_PORT GPIOB
#define CONSOLE_MODE_SWITCH_PIN 1 #define CONSOLE_MODE_SWITCH_PIN 1

View File

@ -73,7 +73,7 @@
#define EFI_HD44780_LCD TRUE #define EFI_HD44780_LCD TRUE
#define EFI_IDLE_CONTROL FALSE #define EFI_IDLE_CONTROL TRUE
#define EFI_FUEL_PUMP TRUE #define EFI_FUEL_PUMP TRUE
@ -124,9 +124,6 @@
#define EFI_UART_GPS TRUE #define EFI_UART_GPS TRUE
//#define EFI_UART_GPS FALSE //#define EFI_UART_GPS FALSE
//#define EFI_IDLE_CONTROL TRUE
#define EFI_IDLE_CONTROL FALSE
//#define EFI_ELECTRONIC_THROTTLE_BODY TRUE //#define EFI_ELECTRONIC_THROTTLE_BODY TRUE
#define EFI_ELECTRONIC_THROTTLE_BODY FALSE #define EFI_ELECTRONIC_THROTTLE_BODY FALSE

View File

@ -16,4 +16,5 @@ ENGINES_SRC_CPP = $(PROJECT_DIR)/config/engines/ford_aspire.cpp \
$(PROJECT_DIR)/config/engines/GY6_139QMB.cpp \ $(PROJECT_DIR)/config/engines/GY6_139QMB.cpp \
$(PROJECT_DIR)/config/engines/rover_v8.cpp \ $(PROJECT_DIR)/config/engines/rover_v8.cpp \
$(PROJECT_DIR)/config/engines/mazda_323.cpp \ $(PROJECT_DIR)/config/engines/mazda_323.cpp \
$(PROJECT_DIR)/config/engines/saturn_ion.cpp $(PROJECT_DIR)/config/engines/saturn_ion.cpp \
$(PROJECT_DIR)/config/engines/mitsubishi.cpp

View File

@ -48,10 +48,10 @@ void setFordInline6(engine_configuration_s *engineConfiguration, board_configura
engineConfiguration->globalTriggerAngleOffset = 0; engineConfiguration->globalTriggerAngleOffset = 0;
engineConfiguration->ignitionOffset = 13; engineConfiguration->ignitionOffset = 13;
setThermistorConfiguration(&engineConfiguration->cltThermistorConf, -10, 160310, 60, 7700, 120.00, 1180); setThermistorConfiguration(&engineConfiguration->cltThermistorConf, -10.0, 160310.0, 60.0, 7700.0, 120.0, 1180.0);
engineConfiguration->cltThermistorConf.bias_resistor = 2700; engineConfiguration->cltThermistorConf.bias_resistor = 2700;
setThermistorConfiguration(&engineConfiguration->iatThermistorConf, -10, 160310, 60, 7700, 120.00, 1180); setThermistorConfiguration(&engineConfiguration->iatThermistorConf, -10.0, 160310.0, 60.0, 7700.0, 120.0, 1180.0);
engineConfiguration->iatThermistorConf.bias_resistor = 2700; engineConfiguration->iatThermistorConf.bias_resistor = 2700;
// 12ch analog board pinout: // 12ch analog board pinout:
@ -75,15 +75,15 @@ void setFordInline6(engine_configuration_s *engineConfiguration, board_configura
boardConfiguration->adcHwChannelEnabled[15] = ADC_FAST; boardConfiguration->adcHwChannelEnabled[15] = ADC_FAST;
engineConfiguration->tpsAdcChannel = 4; engineConfiguration->tpsAdcChannel = EFI_ADC_4;
engineConfiguration->iatAdcChannel = 2; engineConfiguration->iatAdcChannel = EFI_ADC_2;
engineConfiguration->cltAdcChannel = 1; engineConfiguration->cltAdcChannel = EFI_ADC_1;
engineConfiguration->afrSensor.afrAdcChannel = 11; engineConfiguration->afrSensor.afrAdcChannel = EFI_ADC_11;
engineConfiguration->map.sensor.sensorType = MT_MPX4250; engineConfiguration->map.sensor.sensorType = MT_MPX4250;
engineConfiguration->map.sensor.hwChannel = 15; engineConfiguration->map.sensor.hwChannel = EFI_ADC_15;
engineConfiguration->baroSensor.sensorType = MT_MPX4250; engineConfiguration->baroSensor.sensorType = MT_MPX4250;
engineConfiguration->baroSensor.hwChannel = 7; engineConfiguration->baroSensor.hwChannel = EFI_ADC_7;
// 6 channel output board // 6 channel output board
// output 1 is PB9 // output 1 is PB9
@ -105,10 +105,10 @@ void setFordInline6(engine_configuration_s *engineConfiguration, board_configura
// engineConfiguration->vBattAdcChannel = 0; // // engineConfiguration->vBattAdcChannel = 0; //
// engineConfiguration->mafAdcChannel = 1; // engineConfiguration->mafAdcChannel = 1;
boardConfiguration->primaryTriggerInputPin = GPIOA_8; boardConfiguration->triggerInputPins[0] = GPIOA_8;
boardConfiguration->secondaryTriggerInputPin = GPIOA_5; boardConfiguration->triggerInputPins[1] = GPIOA_5;
boardConfiguration->primaryLogicAnalyzerPin = GPIOC_6; boardConfiguration->logicAnalyzerPins[0] = GPIOC_6;
boardConfiguration->secondaryLogicAnalyzerPin = GPIOE_7; boardConfiguration->logicAnalyzerPins[1] = GPIOE_7;
} }

View File

@ -90,6 +90,11 @@ void setFordAspireEngineConfiguration(engine_configuration_s *engineConfiguratio
engineConfiguration->rpmHardLimit = 7000; engineConfiguration->rpmHardLimit = 7000;
/**
* 18K Ohm @ -20C
* 2.1K Ohm @ 24C
* 1K Ohm @ 49C
*/
setThermistorConfiguration(&engineConfiguration->cltThermistorConf, -20, 18000, 23.8889, 2100, 48.8889, 1000); setThermistorConfiguration(&engineConfiguration->cltThermistorConf, -20, 18000, 23.8889, 2100, 48.8889, 1000);
engineConfiguration->cltThermistorConf.bias_resistor = 3300; // that's my custom resistor value! engineConfiguration->cltThermistorConf.bias_resistor = 3300; // that's my custom resistor value!
@ -112,9 +117,12 @@ void setFordAspireEngineConfiguration(engine_configuration_s *engineConfiguratio
engineConfiguration->injectionOffset = 59; engineConfiguration->injectionOffset = 59;
setDefaultMaps(engineConfiguration); setDefaultMaps(engineConfiguration);
// set_cranking_rpm 550
engineConfiguration->crankingSettings.crankingRpm = 550; engineConfiguration->crankingSettings.crankingRpm = 550;
// set_cranking_charge_angle 70
engineConfiguration->crankingChargeAngle = 70; engineConfiguration->crankingChargeAngle = 70;
engineConfiguration->crankingTimingAngle = 26 + 11; // set_cranking_timing_angle 37
engineConfiguration->crankingTimingAngle = 37;
setSingleCoilDwell(engineConfiguration); setSingleCoilDwell(engineConfiguration);
engineConfiguration->ignitionMode = IM_ONE_COIL; engineConfiguration->ignitionMode = IM_ONE_COIL;
@ -140,11 +148,11 @@ void setFordAspireEngineConfiguration(engine_configuration_s *engineConfiguratio
// Frankenstein analog input #12: adc // Frankenstein analog input #12: adc
engineConfiguration->tpsAdcChannel = 3; engineConfiguration->tpsAdcChannel = EFI_ADC_3;
engineConfiguration->vBattAdcChannel = 0; engineConfiguration->vBattAdcChannel = EFI_ADC_0;
engineConfiguration->map.sensor.hwChannel = 4; engineConfiguration->map.sensor.hwChannel = EFI_ADC_4;
engineConfiguration->mafAdcChannel = 1; engineConfiguration->mafAdcChannel = EFI_ADC_1;
engineConfiguration->cltAdcChannel = 11; engineConfiguration->cltAdcChannel = EFI_ADC_11;
// engineConfiguration->iatAdcChannel = // engineConfiguration->iatAdcChannel =
engineConfiguration->map.sensor.sensorType = MT_DENSO183; engineConfiguration->map.sensor.sensorType = MT_DENSO183;

View File

@ -40,9 +40,9 @@ void setFordEscortGt(engine_configuration_s *engineConfiguration, board_configur
// Frankenstein analog input #10: adc // Frankenstein analog input #10: adc
// Frankenstein analog input #11: adc // Frankenstein analog input #11: adc
// Frankenstein analog input #12: adc // Frankenstein analog input #12: adc
engineConfiguration->mafAdcChannel = 1; engineConfiguration->mafAdcChannel = EFI_ADC_1;
engineConfiguration->tpsAdcChannel = 3; engineConfiguration->tpsAdcChannel = EFI_ADC_3;
engineConfiguration->cltAdcChannel = 11; engineConfiguration->cltAdcChannel = EFI_ADC_11;
// Frankenstein: high side #1 is PE8 // Frankenstein: high side #1 is PE8

View File

@ -6,19 +6,26 @@
* http://rusefi.com/forum/viewtopic.php?f=3&t=621 * http://rusefi.com/forum/viewtopic.php?f=3&t=621
* *
* engine_type 6 * engine_type 6
* engine_type 17
* *
* @date Jan 12, 2014 * @date Jan 12, 2014
* @author Andrey Belomutskiy, (c) 2012-2014 * @author Andrey Belomutskiy, (c) 2012-2014
*/ */
#include "main.h" #include "main.h"
#include "engine_configuration.h"
#include "trigger_decoder.h" #include "trigger_decoder.h"
#include "thermistors.h"
#include "honda_accord.h"
void setHondaAccordConfiguration(engine_configuration_s *engineConfiguration, board_configuration_s *boardConfiguration) { static void setHondaAccordConfigurationCommon(engine_configuration_s *engineConfiguration, board_configuration_s *boardConfiguration) {
engineConfiguration->engineType = HONDA_ACCORD; engineConfiguration->map.sensor.sensorType = MT_DENSO183;
engineConfiguration->map.sensor.sensorType = MT_HONDA3BAR; engineConfiguration->ignitionMode = IM_ONE_COIL;
engineConfiguration->injectionMode = IM_BATCH;
engineConfiguration->idleMode = IM_MANUAL;
engineConfiguration->HD44780height = 4;
engineConfiguration->cylindersCount = 4; engineConfiguration->cylindersCount = 4;
engineConfiguration->displacement = 2.156; engineConfiguration->displacement = 2.156;
@ -26,9 +33,9 @@ void setHondaAccordConfiguration(engine_configuration_s *engineConfiguration, bo
// Keihin 06164-P0A-A00 // Keihin 06164-P0A-A00
engineConfiguration->injectorFlow = 248; engineConfiguration->injectorFlow = 248;
// engineConfiguration->algorithm = LM_SPEED_DENSITY;
engineConfiguration->algorithm = LM_SPEED_DENSITY; // I want to start with a simple Alpha-N
engineConfiguration->algorithm = LM_TPS;
engineConfiguration->crankingSettings.coolantTempMaxC = 65; // 8ms at 65C engineConfiguration->crankingSettings.coolantTempMaxC = 65; // 8ms at 65C
engineConfiguration->crankingSettings.fuelAtMaxTempMs = 8; engineConfiguration->crankingSettings.fuelAtMaxTempMs = 8;
@ -36,24 +43,158 @@ void setHondaAccordConfiguration(engine_configuration_s *engineConfiguration, bo
engineConfiguration->crankingSettings.coolantTempMinC = 0; // 20ms at 0C engineConfiguration->crankingSettings.coolantTempMinC = 0; // 20ms at 0C
engineConfiguration->crankingSettings.fuelAtMinTempMs = 15; engineConfiguration->crankingSettings.fuelAtMinTempMs = 15;
/**
* 18K Ohm @ -20C
* 2.1K Ohm @ 24C
* 100 Ohm @ 120C
*/
setThermistorConfiguration(&engineConfiguration->cltThermistorConf, -20.0, 18000.0, 23.8889, 2100.0, 120.0, 100.0);
engineConfiguration->cltThermistorConf.bias_resistor = 1500; // same as OEM ECU
setThermistorConfiguration(&engineConfiguration->iatThermistorConf, -20.0, 18000.0, 23.8889, 2100.0, 120.0, 100.0);
engineConfiguration->iatThermistorConf.bias_resistor = 1500; // same as OEM ECU
// set_cranking_charge_angle 35
engineConfiguration->crankingChargeAngle = 35;
// set_cranking_timing_angle 0
engineConfiguration->crankingTimingAngle = 0;
// set_global_trigger_offset_angle 34
engineConfiguration->globalTriggerAngleOffset = 34;
// set_rpm_hard_limit 4000
engineConfiguration->rpmHardLimit = 4000; // yes, 4k. let's play it safe for now
// set_cranking_rpm 2000
engineConfiguration->crankingSettings.crankingRpm = 600;
// set_ignition_offset 350
// engineConfiguration->ignitionOffset = 350;
// set_injection_offset 510
// engineConfiguration->injectionOffset = 510;
/**
* ADC inputs:
*
* Inp1/ADC12 PC2: CLT
* Inp2/ADC11 PC1: AIT/IAT
* Inp3/ADC0 PA0: MAP
* Inp4/ADC13 PC3: AFR
* Inp6/ADC1 PA1: TPS
* Inp12/ADC14 PC4: VBatt
*/
memset(boardConfiguration->adcHwChannelEnabled, 0, sizeof(boardConfiguration->adcHwChannelEnabled)); memset(boardConfiguration->adcHwChannelEnabled, 0, sizeof(boardConfiguration->adcHwChannelEnabled));
boardConfiguration->adcHwChannelEnabled[0] = ADC_FAST; // ADC0 - PA0 - MAP boardConfiguration->adcHwChannelEnabled[0] = ADC_FAST; // ADC0 - PA0 - MAP
boardConfiguration->adcHwChannelEnabled[1] = ADC_SLOW; boardConfiguration->adcHwChannelEnabled[1] = ADC_SLOW; // TPS
boardConfiguration->adcHwChannelEnabled[2] = ADC_SLOW; boardConfiguration->adcHwChannelEnabled[2] = ADC_SLOW;
boardConfiguration->adcHwChannelEnabled[3] = ADC_SLOW; boardConfiguration->adcHwChannelEnabled[3] = ADC_SLOW;
boardConfiguration->adcHwChannelEnabled[4] = ADC_SLOW; boardConfiguration->adcHwChannelEnabled[4] = ADC_SLOW;
boardConfiguration->adcHwChannelEnabled[6] = ADC_SLOW; boardConfiguration->adcHwChannelEnabled[6] = ADC_SLOW;
boardConfiguration->adcHwChannelEnabled[7] = ADC_SLOW; boardConfiguration->adcHwChannelEnabled[11] = ADC_SLOW; // IAT
boardConfiguration->adcHwChannelEnabled[11] = ADC_SLOW; boardConfiguration->adcHwChannelEnabled[12] = ADC_SLOW; // CLT
boardConfiguration->adcHwChannelEnabled[12] = ADC_SLOW; boardConfiguration->adcHwChannelEnabled[13] = ADC_SLOW; // AFR
boardConfiguration->adcHwChannelEnabled[13] = ADC_SLOW; boardConfiguration->adcHwChannelEnabled[14] = ADC_SLOW; // VBatt
/**
* D14/W10 O2 Sensor
*/
engineConfiguration->afrSensor.afrAdcChannel = EFI_ADC_13;
/**
* VBatt
*/
engineConfiguration->vBattAdcChannel = EFI_ADC_14;
engineConfiguration->vbattDividerCoeff = ((float) (8.2 + 33)) / 8.2 * 2;
// todo engineConfiguration->afrSensor.afrAdcChannel = 14;
engineConfiguration->map.sensor.sensorType = MT_MPX4250; /**
engineConfiguration->map.sensor.hwChannel = 0; * MAP D17/W5
*/
engineConfiguration->map.sensor.hwChannel = EFI_ADC_0;
/**
* TPS D11/W11
*/
engineConfiguration->tpsAdcChannel = EFI_ADC_1;
/**
* IAT D15/W7
*/
engineConfiguration->iatAdcChannel = EFI_ADC_11;
/**
* CLT D13/W9
*/
engineConfiguration->cltAdcChannel = EFI_ADC_12;
/**
* Outputs
*/
// Frankenso low out #:
// Frankenso low out #:
// Frankenso low out #:
// Frankenso low out #:
// Frankenso low out #5: PE3
// Frankenso low out #6: PE4
// Frankenso low out #7: PE1 (do not use with discovery!)
// Frankenso low out #:
// Frankenso low out #9: PB9
// Frankenso low out #10: PE0 (do not use with discovery!)
// Frankenso low out #11: PB8
// Frankenso low out #12: PB7
boardConfiguration->idleValvePin = GPIOE_5;
boardConfiguration->o2heaterPin = GPIOC_13;
boardConfiguration->injectionPins[0] = GPIOB_8;
boardConfiguration->injectionPins[1] = GPIOB_9;
boardConfiguration->injectionPins[2] = GPIOE_1;
boardConfiguration->injectionPins[3] = GPIOB_7;
boardConfiguration->ignitionPins[0] = GPIOE_4;
boardConfiguration->ignitionPins[1] = GPIO_NONE;
boardConfiguration->ignitionPins[2] = GPIO_NONE;
boardConfiguration->ignitionPins[3] = GPIO_NONE;
boardConfiguration->fuelPumpPin = GPIOE_3;
boardConfiguration->fuelPumpPinMode = OM_DEFAULT;
boardConfiguration->gps_rx_pin = GPIO_NONE;
boardConfiguration->gps_tx_pin = GPIO_NONE;
boardConfiguration->HD44780_rs = GPIOE_7;
boardConfiguration->HD44780_e = GPIOE_9;
boardConfiguration->HD44780_db4 = GPIOE_11;
boardConfiguration->HD44780_db5 = GPIOE_13;
boardConfiguration->HD44780_db6 = GPIOE_15;
boardConfiguration->HD44780_db7 = GPIOB_10;
boardConfiguration->logicAnalyzerPins[1] = GPIO_NONE;
boardConfiguration->idleSolenoidFrequency = 500;
} }
void setHondaAccordConfigurationTwoWires(engine_configuration_s *engineConfiguration, board_configuration_s *boardConfiguration) {
engineConfiguration->engineType = HONDA_ACCORD_CD_TWO_WIRES;
engineConfiguration->triggerConfig.triggerType = TT_HONDA_ACCORD_CD_TWO_WIRES;
setHondaAccordConfigurationCommon(engineConfiguration, boardConfiguration);
}
void setHondaAccordConfigurationThreeWires(engine_configuration_s *engineConfiguration, board_configuration_s *boardConfiguration) {
engineConfiguration->engineType = HONDA_ACCORD_CD;
engineConfiguration->triggerConfig.triggerType = TT_HONDA_ACCORD_CD;
setHondaAccordConfigurationCommon(engineConfiguration, boardConfiguration);
}
void setHondaAccordConfigurationDip(engine_configuration_s *engineConfiguration, board_configuration_s *boardConfiguration) {
engineConfiguration->engineType = HONDA_ACCORD_CD_DIP;
engineConfiguration->triggerConfig.triggerType = TT_HONDA_ACCORD_CD_DIP;
setHondaAccordConfigurationCommon(engineConfiguration, boardConfiguration);
}

View File

@ -10,6 +10,8 @@
#include "engine_configuration.h" #include "engine_configuration.h"
void setHondaAccordConfiguration(engine_configuration_s *engineConfiguration, board_configuration_s *boardConfiguration); void setHondaAccordConfigurationTwoWires(engine_configuration_s *engineConfiguration, board_configuration_s *boardConfiguration);
void setHondaAccordConfigurationThreeWires(engine_configuration_s *engineConfiguration, board_configuration_s *boardConfiguration);
void setHondaAccordConfigurationDip(engine_configuration_s *engineConfiguration, board_configuration_s *boardConfiguration);
#endif /* HONDA_ACCORD_H_ */ #endif /* HONDA_ACCORD_H_ */

View File

@ -34,13 +34,13 @@ void setMazdaMiataNbEngineConfiguration(engine_configuration_s *engineConfigurat
setThermistorConfiguration(&engineConfiguration->iatThermistorConf, -10, 160310, 60, 7700, 120.00, 1180); setThermistorConfiguration(&engineConfiguration->iatThermistorConf, -10, 160310, 60, 7700, 120.00, 1180);
engineConfiguration->iatThermistorConf.bias_resistor = 2700; engineConfiguration->iatThermistorConf.bias_resistor = 2700;
engineConfiguration->tpsAdcChannel = 3; // 15 is the old value engineConfiguration->tpsAdcChannel = EFI_ADC_3; // 15 is the old value
engineConfiguration->vBattAdcChannel = 0; // 1 is the old value engineConfiguration->vBattAdcChannel = EFI_ADC_0; // 1 is the old value
// engineConfiguration->map.channel = 1; // engineConfiguration->map.channel = 1;
engineConfiguration->mafAdcChannel = 1; engineConfiguration->mafAdcChannel = EFI_ADC_1;
engineConfiguration->cltAdcChannel = 11; engineConfiguration->cltAdcChannel = EFI_ADC_11;
engineConfiguration->iatAdcChannel = 13; engineConfiguration->iatAdcChannel = EFI_ADC_13;
engineConfiguration->afrSensor.afrAdcChannel = 2; engineConfiguration->afrSensor.afrAdcChannel = EFI_ADC_2;
boardConfiguration->idleValvePin = GPIOE_0; boardConfiguration->idleValvePin = GPIOE_0;
boardConfiguration->idleValvePinMode = OM_DEFAULT; boardConfiguration->idleValvePinMode = OM_DEFAULT;

View File

@ -0,0 +1,95 @@
/**
* @file mitsubishi.cpp
*
* MITSU_4G93 16
*
* @date Aug 5, 2014
* @author Andrey Belomutskiy, (c) 2012-2014
*/
#include "mitsubishi.h"
#include "thermistors.h"
void setMitsubishiConfiguration(engine_configuration_s *engineConfiguration, board_configuration_s *boardConfiguration) {
engineConfiguration->engineType = MITSU_4G93;
engineConfiguration->triggerConfig.triggerType = TT_MITSU;
engineConfiguration->cylindersCount = 4;
engineConfiguration->displacement = 1.800;
// set_ignition_mode 2
engineConfiguration->ignitionMode = IM_WASTED_SPARK;
engineConfiguration->firingOrder = FO_1_THEN_3_THEN_4_THEN2;
// set_global_trigger_offset_angle 671
engineConfiguration->globalTriggerAngleOffset = 671;
// set_cranking_rpm 550
engineConfiguration->crankingSettings.crankingRpm = 550;
// set_cranking_charge_angle 70
engineConfiguration->crankingChargeAngle = 70;
// set_cranking_timing_angle 715
engineConfiguration->crankingTimingAngle = 715;
// set_whole_fuel_map 3
setWholeFuelMap(engineConfiguration, 3);
// since CLT is not wired up yet let's just use same value for min and max
// set_cranking_fuel_max 6 40
engineConfiguration->crankingSettings.coolantTempMaxC = 37.7; // 6ms at 37.7C
engineConfiguration->crankingSettings.fuelAtMaxTempMs = 6;
// set_cranking_fuel_min 6 -40
engineConfiguration->crankingSettings.coolantTempMinC = -40; // 6ms at -40C
engineConfiguration->crankingSettings.fuelAtMinTempMs = 6;
// /**
// * 29150 Ohm @ 40C
// * 10160 Ohm @ 70C
// * 1270 Ohm @ 150C
// */
// setThermistorConfiguration(&engineConfiguration->cltThermistorConf, 40, 29150, 70, 10160, 150, 1270);
/**
* 18K Ohm @ -20C
* 2.1K Ohm @ 24C
* 294 Ohm @ 80C
* http://www.rexbo.eu/hella/coolant-temperature-sensor-6pt009107121?c=100334&at=3130
*/
setThermistorConfiguration(&engineConfiguration->cltThermistorConf, -20, 18000, 23.8889, 2100, 80, 294);
engineConfiguration->cltThermistorConf.bias_resistor = 2700;
// Frankenstein: low side - inj #1: PC14
// Frankenstein: low side - inj #2: PC15
// Frankenstein: low side - inj #3: PE6
// Frankenstein: low side - inj #4: PC13
// Frankenstein: low side - inj #5: PE4
// Frankenstein: low side - inj #6: PE5
// Frankenstein: low side - inj #7: PE2
// Frankenstein: low side - inj #8: PE3
// Frankenstein: low side - inj #9: PE0
// Frankenstein: low side - inj #10: PE1
// Frankenstein: low side - inj #11: PB8
// Frankenstein: low side - inj #12: PB9
boardConfiguration->injectionPins[0] = GPIOB_9; // Frankenstein: low side - inj #12
boardConfiguration->injectionPins[1] = GPIOB_8; // Frankenstein: low side - inj #11
boardConfiguration->injectionPins[2] = GPIOE_3; // Frankenstein: low side - inj #8
boardConfiguration->injectionPins[3] = GPIOE_5; // Frankenstein: low side - inj #6
// Frankenstein: high side #1: PE8
// Frankenstein: high side #2: PE10
boardConfiguration->ignitionPins[0] = GPIOE_8; // Frankenstein: high side #1
boardConfiguration->ignitionPins[1] = GPIO_NONE;
boardConfiguration->ignitionPins[2] = GPIOE_10; // // Frankenstein: high side #2
boardConfiguration->ignitionPins[3] = GPIO_NONE;
engineConfiguration->HD44780width = 20;
engineConfiguration->HD44780height = 4;
}

View File

@ -0,0 +1,16 @@
/**
* @file mitsubishi.h
*
* @date Aug 5, 2014
* @author Andrey Belomutskiy, (c) 2012-2014
*/
#ifndef MITSUBISHI_H_
#define MITSUBISHI_H_
#include "main.h"
#include "engine_configuration.h"
void setMitsubishiConfiguration(engine_configuration_s *engineConfiguration, board_configuration_s *boardConfiguration);
#endif /* MITSUBISHI_H_ */

View File

@ -80,9 +80,7 @@ static bool getConsoleLine(BaseSequentialStream *chp, char *line, unsigned size)
continue; continue;
#endif #endif
if (c < 0) if (c < 0 || c == 4) {
return TRUE;
if (c == 4) {
return TRUE; return TRUE;
} }
if (c == 8) { if (c == 8) {
@ -101,8 +99,9 @@ static bool getConsoleLine(BaseSequentialStream *chp, char *line, unsigned size)
*p = 0; *p = 0;
return false; return false;
} }
if (c < 0x20) if (c < 0x20) {
continue; continue;
}
if (p < line + size - 1) { if (p < line + size - 1) {
consolePutChar((uint8_t) c); consolePutChar((uint8_t) c);
*p++ = (char) c; *p++ = (char) c;
@ -152,7 +151,7 @@ SerialDriver * getConsoleChannel(void) {
} }
} }
int isConsoleReady(void) { bool isConsoleReady(void) {
if (isSerialOverUart()) { if (isSerialOverUart()) {
return isSerialConsoleStarted; return isSerialConsoleStarted;
} else { } else {
@ -169,7 +168,7 @@ void consolePutChar(int x) {
// 10 seconds // 10 seconds
#define CONSOLE_WRITE_TIMEOUT 10000 #define CONSOLE_WRITE_TIMEOUT 10000
void consoleOutputBuffer(const int8_t *buf, int size) { void consoleOutputBuffer(const uint8_t *buf, int size) {
lastWriteSize = size; lastWriteSize = size;
#if !EFI_UART_ECHO_TEST_MODE #if !EFI_UART_ECHO_TEST_MODE
lastWriteActual = chnWriteTimeout(getConsoleChannel(), buf, size, CONSOLE_WRITE_TIMEOUT); lastWriteActual = chnWriteTimeout(getConsoleChannel(), buf, size, CONSOLE_WRITE_TIMEOUT);

View File

@ -32,9 +32,9 @@ extern "C"
SerialDriver * getConsoleChannel(void); SerialDriver * getConsoleChannel(void);
void consolePutChar(int x); void consolePutChar(int x);
void consoleOutputBuffer(const int8_t *buf, int size); void consoleOutputBuffer(const uint8_t *buf, int size);
void startConsole(void (*console_line_callback_p)(char *)); void startConsole(void (*console_line_callback_p)(char *));
int isConsoleReady(void); bool isConsoleReady(void);
bool isSerialOverUart(void); bool isSerialOverUart(void);
#ifdef __cplusplus #ifdef __cplusplus

View File

@ -130,17 +130,14 @@ static void cmd_threads(void) {
#endif #endif
} }
void sendOutConfirmation(char *value, int i) {
scheduleMsg(&logger, "%s%d", value, i);
}
/** /**
* This methods prints the message to whatever is configured as our primary console * This methods prints the message to whatever is configured as our primary console
*/ */
void print(const char *format, ...) { void print(const char *format, ...) {
#if !EFI_UART_ECHO_TEST_MODE #if !EFI_UART_ECHO_TEST_MODE
if (!isConsoleReady()) if (!isConsoleReady()) {
return; return;
}
va_list ap; va_list ap;
va_start(ap, format); va_start(ap, format);
chvprintf((BaseSequentialStream*)getConsoleChannel(), format, ap); chvprintf((BaseSequentialStream*)getConsoleChannel(), format, ap);
@ -148,7 +145,7 @@ void print(const char *format, ...) {
#endif /* EFI_UART_ECHO_TEST_MODE */ #endif /* EFI_UART_ECHO_TEST_MODE */
} }
void initializeConsole() { void initializeConsole(void) {
initIntermediateLoggingBuffer(); initIntermediateLoggingBuffer();
initConsoleLogic(); initConsoleLogic();

View File

@ -16,7 +16,6 @@ extern "C"
#endif /* __cplusplus */ #endif /* __cplusplus */
void initializeConsole(void); void initializeConsole(void);
void sendOutConfirmation(char *value, int i);
void print(const char *fmt, ...); void print(const char *fmt, ...);
#ifdef __cplusplus #ifdef __cplusplus

View File

@ -1,5 +1,5 @@
/** /**
* @file status_loop.c * @file status_loop.cpp
* @brief Human-readable protocol status messages * @brief Human-readable protocol status messages
* *
* http://rusefi.com/forum/viewtopic.php?t=263 Dev console overview * http://rusefi.com/forum/viewtopic.php?t=263 Dev console overview
@ -46,8 +46,6 @@
#include "tunerstudio.h" #include "tunerstudio.h"
#endif /* EFI_TUNER_STUDIO */ #endif /* EFI_TUNER_STUDIO */
#include "wave_math.h"
#include "fuel_math.h" #include "fuel_math.h"
#include "main_trigger_callback.h" #include "main_trigger_callback.h"
#include "engine_math.h" #include "engine_math.h"
@ -56,18 +54,22 @@
#include "rfiutil.h" #include "rfiutil.h"
#include "svnversion.h" #include "svnversion.h"
#include "engine.h" #include "engine.h"
#include "lcd_controller.h"
#include "fuel_math.h"
#if EFI_PROD_CODE #if EFI_PROD_CODE
// todo: move this logic to algo folder! // todo: move this logic to algo folder!
#include "rtc_helper.h" #include "rtc_helper.h"
#include "lcd_HD44780.h" #include "lcd_HD44780.h"
#include "rusefi.h" #include "rusefi.h"
#include "pin_repository.h"
#include "flash_main.h"
#endif #endif
extern Engine engine; extern Engine engine;
// this 'true' value is needed for simulator // this 'true' value is needed for simulator
static volatile int fullLog = TRUE; static volatile bool fullLog = true;
int warningEnabled = TRUE; int warningEnabled = TRUE;
//int warningEnabled = FALSE; //int warningEnabled = FALSE;
@ -112,10 +114,6 @@ static const char* boolean2string(int value) {
return value ? "YES" : "NO"; return value ? "YES" : "NO";
} }
void finishStatusLine(void) {
printLine(&logger);
}
void printSensors(void) { void printSensors(void) {
#if EFI_FILE_LOGGING #if EFI_FILE_LOGGING
resetLogging(&fileLogger); resetLogging(&fileLogger);
@ -186,7 +184,7 @@ void printState(int currentCkpEventCounter) {
#define INITIAL_FULL_LOG TRUE #define INITIAL_FULL_LOG TRUE
//#define INITIAL_FULL_LOG FALSE //#define INITIAL_FULL_LOG FALSE
static char LOGGING_BUFFER[500]; static char LOGGING_BUFFER[700];
volatile int needToReportStatus = FALSE; volatile int needToReportStatus = FALSE;
static int prevCkpEventCounter = -1; static int prevCkpEventCounter = -1;
@ -203,13 +201,42 @@ static void printStatus(void) {
*/ */
static systime_t timeOfPreviousPrintVersion = (systime_t) -1; static systime_t timeOfPreviousPrintVersion = (systime_t) -1;
static void printVersion(systime_t nowSeconds) { #if EFI_PROD_CODE
if (overflowDiff(nowSeconds, timeOfPreviousPrintVersion) < 4) static void printOutPin(const char *pinName, brain_pin_e hwPin) {
appendPrintf(&logger, "outpin%s%s@%s%s", DELIMETER, pinName,
hwPortname(hwPin), DELIMETER);
}
#endif /* EFI_PROD_CODE */
static void printInfo(systime_t nowSeconds) {
/**
* we report the version every 4 seconds - this way the console does not need to
* request it and we will display it pretty soon
*/
if (overflowDiff(nowSeconds, timeOfPreviousPrintVersion) < 4) {
return; return;
}
timeOfPreviousPrintVersion = nowSeconds; timeOfPreviousPrintVersion = nowSeconds;
appendPrintf(&logger, "rusEfiVersion%s%d@%s %s%s", DELIMETER, getRusEfiVersion(), VCS_VERSION, appendPrintf(&logger, "rusEfiVersion%s%d@%s %s%s", DELIMETER, getRusEfiVersion(), VCS_VERSION,
getConfigurationName(engineConfiguration), getConfigurationName(engineConfiguration),
DELIMETER); DELIMETER);
#if EFI_PROD_CODE
printOutPin(WC_CRANK1, boardConfiguration->triggerInputPins[0]);
printOutPin(WC_CRANK2, boardConfiguration->triggerInputPins[1]);
printOutPin(WA_CHANNEL_1, boardConfiguration->logicAnalyzerPins[0]);
printOutPin(WA_CHANNEL_2, boardConfiguration->logicAnalyzerPins[1]);
for (int i = 0; i < engineConfiguration->cylindersCount; i++) {
// todo: extract method?
io_pin_e pin = (io_pin_e) ((int) SPARKOUT_1_OUTPUT + i);
printOutPin(getPinName(pin), boardConfiguration->ignitionPins[i]);
pin = (io_pin_e) ((int) INJECTOR_1_OUTPUT + i);
printOutPin(getPinName(pin), boardConfiguration->injectionPins[i]);
}
#endif
} }
static systime_t timeOfPreviousReport = (systime_t) -1; static systime_t timeOfPreviousReport = (systime_t) -1;
@ -220,8 +247,9 @@ extern char errorMessageBuffer[200];
* @brief Sends all pending data to dev console * @brief Sends all pending data to dev console
*/ */
void updateDevConsoleState(void) { void updateDevConsoleState(void) {
if (!isConsoleReady()) if (!isConsoleReady()) {
return; return;
}
// looks like this is not needed anymore // looks like this is not needed anymore
// checkIfShouldHalt(); // checkIfShouldHalt();
printPending(); printPending();
@ -240,15 +268,17 @@ void updateDevConsoleState(void) {
pokeAdcInputs(); pokeAdcInputs();
#endif #endif
if (!fullLog) if (!fullLog) {
return; return;
}
systime_t nowSeconds = getTimeNowSeconds(); systime_t nowSeconds = getTimeNowSeconds();
printVersion(nowSeconds); printInfo(nowSeconds);
int currentCkpEventCounter = getCrankEventCounter(); int currentCkpEventCounter = getCrankEventCounter();
if (prevCkpEventCounter == currentCkpEventCounter && timeOfPreviousReport == nowSeconds) if (prevCkpEventCounter == currentCkpEventCounter && timeOfPreviousReport == nowSeconds) {
return; return;
}
timeOfPreviousReport = nowSeconds; timeOfPreviousReport = nowSeconds;
@ -257,10 +287,10 @@ void updateDevConsoleState(void) {
printState(currentCkpEventCounter); printState(currentCkpEventCounter);
#if EFI_WAVE_ANALYZER #if EFI_WAVE_ANALYZER
// printWave(&logger); printWave(&logger);
#endif #endif
finishStatusLine(); printLine(&logger);
} }
#if EFI_PROD_CODE #if EFI_PROD_CODE
@ -272,7 +302,7 @@ void updateDevConsoleState(void) {
*/ */
static void showFuelMap2(float rpm, float engineLoad) { static void showFuelMap2(float rpm, float engineLoad) {
float baseFuel = getBaseTableFuel(rpm, engineLoad); float baseFuel = getBaseTableFuel((int) rpm, engineLoad);
float iatCorrection = getIatCorrection(getIntakeAirTemperature()); float iatCorrection = getIatCorrection(getIntakeAirTemperature());
float cltCorrection = getCltCorrection(getCoolantTemperature()); float cltCorrection = getCltCorrection(getCoolantTemperature());
@ -283,49 +313,19 @@ static void showFuelMap2(float rpm, float engineLoad) {
scheduleMsg(&logger2, "iatCorrection=%f cltCorrection=%f injectorLag=%f", iatCorrection, cltCorrection, scheduleMsg(&logger2, "iatCorrection=%f cltCorrection=%f injectorLag=%f", iatCorrection, cltCorrection,
injectorLag); injectorLag);
float value = getRunningFuel(baseFuel, &engine, rpm); float value = getRunningFuel(baseFuel, &engine, (int) rpm);
scheduleMsg(&logger2, "injection pulse width: %f", value); scheduleMsg(&logger2, "injection pulse width: %f", value);
} }
static void showFuelMap(void) { static void showFuelMap(void) {
showFuelMap2(getRpm(), getEngineLoad()); showFuelMap2((float) getRpm(), getEngineLoad());
} }
static char buffer[10];
static char dateBuffer[30];
void updateHD44780lcd(void) {
lcd_HD44780_set_position(0, 9);
lcd_HD44780_print_char('R');
lcd_HD44780_set_position(0, 10);
char * ptr = itoa10(buffer, getRpm());
ptr[0] = 0;
int len = ptr - buffer;
for (int i = 0; i < 6 - len; i++)
lcd_HD44780_print_char(' ');
lcd_HD44780_print_string(buffer);
lcd_HD44780_set_position(2, 0);
lcd_HD44780_print_char('C');
ftoa(buffer, getCoolantTemperature(), 100);
lcd_HD44780_print_string(buffer);
#if EFI_PROD_CODE
dateToString(dateBuffer);
lcd_HD44780_set_position(1, 0);
lcd_HD44780_print_string(dateBuffer);
#endif /* EFI_PROD_CODE */
}
#endif /* EFI_PROD_CODE */ #endif /* EFI_PROD_CODE */
static THD_WORKING_AREA(lcdThreadStack, UTILITY_THREAD_STACK_SIZE); static THD_WORKING_AREA(lcdThreadStack, UTILITY_THREAD_STACK_SIZE);
static void lcdThread(void *arg) { static void lcdThread(void) {
chRegSetThreadName("lcd"); chRegSetThreadName("lcd");
while (true) { while (true) {
#if EFI_HD44780_LCD #if EFI_HD44780_LCD
@ -338,7 +338,6 @@ static void lcdThread(void *arg) {
static THD_WORKING_AREA(tsThreadStack, UTILITY_THREAD_STACK_SIZE); static THD_WORKING_AREA(tsThreadStack, UTILITY_THREAD_STACK_SIZE);
#if EFI_TUNER_STUDIO #if EFI_TUNER_STUDIO
extern TunerStudioOutputChannels tsOutputChannels;
void updateTunerStudioState(TunerStudioOutputChannels *tsOutputChannels) { void updateTunerStudioState(TunerStudioOutputChannels *tsOutputChannels) {
#if EFI_SHAFT_POSITION_INPUT #if EFI_SHAFT_POSITION_INPUT
@ -351,6 +350,9 @@ void updateTunerStudioState(TunerStudioOutputChannels *tsOutputChannels) {
float coolant = getCoolantTemperature(); float coolant = getCoolantTemperature();
float intake = getIntakeAirTemperature(); float intake = getIntakeAirTemperature();
float engineLoad = getEngineLoad();
float baseFuel = getBaseTableFuel((int) rpm, engineLoad);
tsOutputChannels->rpm = rpm; tsOutputChannels->rpm = rpm;
tsOutputChannels->coolant_temperature = coolant; tsOutputChannels->coolant_temperature = coolant;
tsOutputChannels->intake_air_temperature = intake; tsOutputChannels->intake_air_temperature = intake;
@ -362,8 +364,24 @@ void updateTunerStudioState(TunerStudioOutputChannels *tsOutputChannels) {
tsOutputChannels->atmospherePressure = getBaroPressure(); tsOutputChannels->atmospherePressure = getBaroPressure();
tsOutputChannels->manifold_air_pressure = getMap(); tsOutputChannels->manifold_air_pressure = getMap();
tsOutputChannels->checkEngine = hasErrorCodes(); tsOutputChannels->checkEngine = hasErrorCodes();
#if EFI_PROD_CODE
tsOutputChannels->needBurn = getNeedToWriteConfiguration();
tsOutputChannels->hasSdCard = isSdCardAlive();
tsOutputChannels->isFuelPumpOn = getOutputPinValue(FUEL_PUMP_RELAY);
tsOutputChannels->isFanOn = getOutputPinValue(FAN_RELAY);
tsOutputChannels->isO2HeaterOn = getOutputPinValue(O2_HEATER);
tsOutputChannels->ignition_enabled = engineConfiguration->isIgnitionEnabled;
tsOutputChannels->injection_enabled = engineConfiguration->isInjectionEnabled;
tsOutputChannels->cylinder_cleanup_enabled = engineConfiguration->isCylinderCleanupEnabled;
tsOutputChannels->secondTriggerChannelEnabled = engineConfiguration->secondTriggerChannelEnabled;
#endif
tsOutputChannels->tCharge = getTCharge(rpm, tps, coolant, intake); tsOutputChannels->tCharge = getTCharge(rpm, tps, coolant, intake);
tsOutputChannels->sparkDwell = getSparkDwellMs(rpm);
tsOutputChannels->pulseWidth = getRunningFuel(baseFuel, &engine, rpm);
tsOutputChannels->crankingFuel = getCrankingFuel();
} }
extern TunerStudioOutputChannels tsOutputChannels;
#endif /* EFI_TUNER_STUDIO */ #endif /* EFI_TUNER_STUDIO */
static void tsStatusThread(void *arg) { static void tsStatusThread(void *arg) {
@ -403,8 +421,8 @@ void initStatusLoop(void) {
void startStatusThreads(void) { void startStatusThreads(void) {
// todo: refactoring needed, this file should probably be split into pieces // todo: refactoring needed, this file should probably be split into pieces
chThdCreateStatic(lcdThreadStack, sizeof(lcdThreadStack), NORMALPRIO, (tfunc_t) lcdThread, NULL); chThdCreateStatic(lcdThreadStack, sizeof(lcdThreadStack), NORMALPRIO, (tfunc_t) lcdThread, (void*) NULL);
chThdCreateStatic(tsThreadStack, sizeof(tsThreadStack), NORMALPRIO, (tfunc_t) tsStatusThread, NULL); chThdCreateStatic(tsThreadStack, sizeof(tsThreadStack), NORMALPRIO, (tfunc_t) tsStatusThread, (void*) NULL);
} }
void setFullLog(int value) { void setFullLog(int value) {
@ -413,6 +431,6 @@ void setFullLog(int value) {
fullLog = value; fullLog = value;
} }
int getFullLog(void) { bool getFullLog(void) {
return fullLog; return fullLog;
} }

View File

@ -17,7 +17,7 @@ void printState(int currentCkpEventCounter);
void initStatusLoop(void); void initStatusLoop(void);
void updateDevConsoleState(void); void updateDevConsoleState(void);
int getFullLog(void); bool getFullLog(void);
void printSensors(void); void printSensors(void);
void setFullLog(int value); void setFullLog(int value);
void startStatusThreads(void); void startStatusThreads(void);

View File

@ -33,7 +33,6 @@
#include "tunerstudio_algo.h" #include "tunerstudio_algo.h"
#include "tunerstudio_configuration.h" #include "tunerstudio_configuration.h"
#include "malfunction_central.h" #include "malfunction_central.h"
#include "wave_math.h"
#include "console_io.h" #include "console_io.h"
#include "crc.h" #include "crc.h"
@ -54,14 +53,14 @@ static SerialConfig tsSerialConfig = { TS_SERIAL_SPEED, 0, USART_CR2_STOP1_BITS
#endif /* EFI_PROD_CODE */ #endif /* EFI_PROD_CODE */
#define MAX_PAGE_ID 0 #define MAX_PAGE_ID 0
#define PAGE_0_SIZE 5824 #define PAGE_0_SIZE 5928
#define TS_OUTPUT_SIZE 116
// in MS, that's 10 seconds // in MS, that's 10 seconds
#define TS_READ_TIMEOUT 10000 #define TS_READ_TIMEOUT 10000
#define PROTOCOL "001" #define PROTOCOL "001"
BaseChannel * getTsSerialDevice(void) { BaseChannel * getTsSerialDevice(void) {
#if EFI_PROD_CODE #if EFI_PROD_CODE
if (isSerialOverUart()) { if (isSerialOverUart()) {
@ -77,7 +76,6 @@ BaseChannel * getTsSerialDevice(void) {
static Logging logger; static Logging logger;
extern engine_configuration_s *engineConfiguration;
extern persistent_config_s configWorkingCopy; extern persistent_config_s configWorkingCopy;
extern persistent_config_container_s persistentState; extern persistent_config_container_s persistentState;
@ -108,6 +106,10 @@ extern TunerStudioOutputChannels tsOutputChannels;
extern TunerStudioState tsState; extern TunerStudioState tsState;
extern engine_configuration_s *engineConfiguration;
extern board_configuration_s *boardConfiguration;
static void printStats(void) { static void printStats(void) {
#if EFI_PROD_CODE #if EFI_PROD_CODE
if (!isSerialOverUart()) { if (!isSerialOverUart()) {
@ -127,14 +129,11 @@ static void printStats(void) {
// int fuelMapOffset = (int) (&engineConfiguration->fuelTable) - (int) engineConfiguration; // int fuelMapOffset = (int) (&engineConfiguration->fuelTable) - (int) engineConfiguration;
// scheduleMsg(&logger, "fuelTable %d", fuelMapOffset); // scheduleMsg(&logger, "fuelTable %d", fuelMapOffset);
// //
// int offset = (int) (&engineConfiguration->bc.injectionPinMode) - (int) engineConfiguration; // int offset = (int) (&boardConfiguration->o2heaterPin) - (int) engineConfiguration;
// scheduleMsg(&logger, "injectionPinMode %d", offset); // scheduleMsg(&logger, "o2heaterPin %d", offset);
// //
// offset = (int) (&engineConfiguration->bc.idleThreadPeriod) - (int) engineConfiguration; // offset = (int) (&boardConfiguration->idleSolenoidFrequency) - (int) engineConfiguration;
// scheduleMsg(&logger, "idleThreadPeriod %d", offset); // scheduleMsg(&logger, "idleSolenoidFrequency %d", offset);
if (sizeof(engine_configuration_s) != getTunerStudioPageSize(0))
firmwareError("TS page size mismatch");
} }
void tunerStudioWriteData(const uint8_t * buffer, int size) { void tunerStudioWriteData(const uint8_t * buffer, int size) {
@ -227,7 +226,7 @@ void handleWriteValueCommand(ts_response_format_e mode, uint16_t page, uint16_t
// scheduleMsg(&logger, "Page number %d\r\n", pageId); // we can get a lot of these // scheduleMsg(&logger, "Page number %d\r\n", pageId); // we can get a lot of these
#endif #endif
int size = sizeof(TunerStudioWriteValueRequest); // int size = sizeof(TunerStudioWriteValueRequest);
// scheduleMsg(&logger, "Reading %d\r\n", size); // scheduleMsg(&logger, "Reading %d\r\n", size);
if (offset > getTunerStudioPageSize(tsState.currentPageId)) { if (offset > getTunerStudioPageSize(tsState.currentPageId)) {
@ -258,7 +257,7 @@ void handlePageReadCommand(ts_response_format_e mode, uint16_t pageId, uint16_t
tsState.currentPageId = pageId; tsState.currentPageId = pageId;
#if EFI_TUNER_STUDIO_VERBOSE #if EFI_TUNER_STUDIO_VERBOSE
scheduleMsg(&logger, "Page requested: page %d offset=%d count=%d", tsState.currentPageId, offset, count); scheduleMsg(&logger, "Page requested: page %d offset=%d count=%d", (int)tsState.currentPageId, offset, count);
#endif #endif
if (tsState.currentPageId > MAX_PAGE_ID) { if (tsState.currentPageId > MAX_PAGE_ID) {
@ -305,7 +304,7 @@ void handleBurnCommand(ts_response_format_e mode, uint16_t page) {
memcpy(&persistentState.persistentConfiguration, &configWorkingCopy, sizeof(persistent_config_s)); memcpy(&persistentState.persistentConfiguration, &configWorkingCopy, sizeof(persistent_config_s));
#if EFI_INTERNAL_FLASH #if EFI_INTERNAL_FLASH
writeToFlash(); setNeedToWriteConfiguration();
#endif #endif
incrementGlobalConfigurationVersion(); incrementGlobalConfigurationVersion();
tunerStudioWriteCrcPacket(TS_RESPONSE_BURN_OK, NULL, 0); tunerStudioWriteCrcPacket(TS_RESPONSE_BURN_OK, NULL, 0);
@ -397,7 +396,7 @@ static msg_t tsThreadEntryPoint(void *arg) {
} }
// scheduleMsg(&logger, "Got secondByte=%x=[%c]", secondByte, secondByte); // scheduleMsg(&logger, "Got secondByte=%x=[%c]", secondByte, secondByte);
int incomingPacketSize = firstByte * 256 + secondByte; uint32_t incomingPacketSize = firstByte * 256 + secondByte;
if (incomingPacketSize == 0 || incomingPacketSize > sizeof(crcIoBuffer)) { if (incomingPacketSize == 0 || incomingPacketSize > sizeof(crcIoBuffer)) {
scheduleMsg(&logger, "TunerStudio: invalid size: %d", incomingPacketSize); scheduleMsg(&logger, "TunerStudio: invalid size: %d", incomingPacketSize);
@ -435,7 +434,7 @@ static msg_t tsThreadEntryPoint(void *arg) {
expectedCrc = SWAP_UINT32(expectedCrc); expectedCrc = SWAP_UINT32(expectedCrc);
int actualCrc = crc32(crcIoBuffer, incomingPacketSize); uint32_t actualCrc = crc32(crcIoBuffer, incomingPacketSize);
if (actualCrc != expectedCrc) { if (actualCrc != expectedCrc) {
scheduleMsg(&logger, "TunerStudio: CRC %x %x %x %x", crcIoBuffer[incomingPacketSize + 0], scheduleMsg(&logger, "TunerStudio: CRC %x %x %x %x", crcIoBuffer[incomingPacketSize + 0],
crcIoBuffer[incomingPacketSize + 1], crcIoBuffer[incomingPacketSize + 2], crcIoBuffer[incomingPacketSize + 1], crcIoBuffer[incomingPacketSize + 2],
@ -466,6 +465,13 @@ void syncTunerStudioCopy(void) {
void startTunerStudioConnectivity(void) { void startTunerStudioConnectivity(void) {
initLogging(&logger, "tuner studio"); initLogging(&logger, "tuner studio");
if (sizeof(engine_configuration_s) != getTunerStudioPageSize(0))
firmwareError("TS page size mismatch: %d/%d", sizeof(engine_configuration_s), getTunerStudioPageSize(0));
if (sizeof(TunerStudioOutputChannels) != TS_OUTPUT_SIZE)
firmwareError("TS outputs size mismatch: %d/%d", sizeof(TunerStudioOutputChannels), TS_OUTPUT_SIZE);
memset(&tsState, 0, sizeof(tsState)); memset(&tsState, 0, sizeof(tsState));
#if EFI_PROD_CODE #if EFI_PROD_CODE
if (isSerialOverUart()) { if (isSerialOverUart()) {

View File

@ -48,6 +48,7 @@
#include "tunerstudio_configuration.h" #include "tunerstudio_configuration.h"
#include "engine_configuration.h" #include "engine_configuration.h"
#include "tunerstudio.h" #include "tunerstudio.h"
#include "svnversion.h"
#ifndef FALSE #ifndef FALSE
#define FALSE 0 #define FALSE 0
@ -150,5 +151,6 @@ void handleTestCommand(void) {
* extension of the protocol to simplify troubleshooting * extension of the protocol to simplify troubleshooting
*/ */
tunerStudioDebug("got T (Test)"); tunerStudioDebug("got T (Test)");
tunerStudioWriteData((const uint8_t *) "alive\r\n", 7); tunerStudioWriteData((const uint8_t *)VCS_VERSION, sizeof(VCS_VERSION));
tunerStudioWriteData((const uint8_t *) " alive\r\n", 8);
} }

View File

@ -16,6 +16,7 @@
* support 'float' (F32) type. You would need a beta version to handle floats * support 'float' (F32) type. You would need a beta version to handle floats
*/ */
typedef struct { typedef struct {
// primary instrument cluster gauges
int rpm; // size 4, offset 0 int rpm; // size 4, offset 0
float coolant_temperature; // size 4, offset 4 float coolant_temperature; // size 4, offset 4
float intake_air_temperature; // size 4, offset 8 float intake_air_temperature; // size 4, offset 8
@ -28,8 +29,29 @@ typedef struct {
short int alignment; // size 2, offset 34 short int alignment; // size 2, offset 34
float atmospherePressure; // size 4, offset 36 float atmospherePressure; // size 4, offset 36
float manifold_air_pressure; // size 4, offset 40 float manifold_air_pressure; // size 4, offset 40
int checkEngine; // size 4, offset 44 float crankingFuel;
float tCharge; int tpsVolrage;
float tCharge; // 52
float inj_adv; // 56
float sparkDwell; // 60
float pulseWidth; // 64
float warmUpEnrich; // 68
/**
* Yes, I do not really enjoy packing bits into integers but we simply have too many boolean flags and I cannot
* water 4 bytes per trafic - I want gauges to work as fast as possible
*/
unsigned int hasSdCard : 1; // bit 0
unsigned int ignition_enabled : 1; // bit 1
unsigned int injection_enabled : 1; // bit 2
unsigned int cylinder_cleanup_enabled : 1; // bit 3
unsigned int cylinder_cleanup : 1; // bit 4
unsigned int isFuelPumpOn : 1; // bit 5
unsigned int isFanOn : 1; // bit 6
unsigned int isO2HeaterOn : 1; // bit 7
unsigned int checkEngine : 1; // bit 8
unsigned int needBurn : 1; // bit 9
unsigned int secondTriggerChannelEnabled : 1; // bit 10
int unused[10];
} TunerStudioOutputChannels; } TunerStudioOutputChannels;
#endif /* TUNERSTUDIO_CONFIGURATION_H_ */ #endif /* TUNERSTUDIO_CONFIGURATION_H_ */

View File

@ -49,7 +49,8 @@
/** /**
* This is the buffer into which all the data providers write * This is the buffer into which all the data providers write
*/ */
static char pendingBuffer[DL_OUTPUT_BUFFER] CCM_OPTIONAL; static char pendingBuffer[DL_OUTPUT_BUFFER] CCM_OPTIONAL
;
/** /**
* We copy all the pending data into this buffer once we are ready to push it out * We copy all the pending data into this buffer once we are ready to push it out
@ -57,10 +58,12 @@ static char pendingBuffer[DL_OUTPUT_BUFFER] CCM_OPTIONAL;
static char outputBuffer[DL_OUTPUT_BUFFER]; static char outputBuffer[DL_OUTPUT_BUFFER];
static MemoryStream intermediateLoggingBuffer; static MemoryStream intermediateLoggingBuffer;
static uint8_t intermediateLoggingBufferData[INTERMEDIATE_LOGGING_BUFFER_SIZE] CCM_OPTIONAL; //todo define max-printf-buffer static uint8_t intermediateLoggingBufferData[INTERMEDIATE_LOGGING_BUFFER_SIZE] CCM_OPTIONAL
;
//todo define max-printf-buffer
static bool intermediateLoggingBufferInited = FALSE; static bool intermediateLoggingBufferInited = FALSE;
static int validateBuffer(Logging *logging, int extraLen, const char *text) { static int validateBuffer(Logging *logging, uint32_t extraLen, const char *text) {
if (logging->buffer == NULL) { if (logging->buffer == NULL) {
firmwareError("Logging not initialized: %s", logging->name); firmwareError("Logging not initialized: %s", logging->name);
return TRUE; return TRUE;
@ -81,10 +84,11 @@ static int validateBuffer(Logging *logging, int extraLen, const char *text) {
void append(Logging *logging, const char *text) { void append(Logging *logging, const char *text) {
efiAssertVoid(text != NULL, "append NULL"); efiAssertVoid(text != NULL, "append NULL");
int extraLen = strlen(text); uint32_t extraLen = strlen(text);
int errcode = validateBuffer(logging, extraLen, text); int errcode = validateBuffer(logging, extraLen, text);
if (errcode) if (errcode) {
return; return;
}
strcpy(logging->linePointer, text); strcpy(logging->linePointer, text);
logging->linePointer += extraLen; logging->linePointer += extraLen;
} }
@ -171,6 +175,7 @@ char* getCaption(LoggingPoints loggingPoint) {
return NULL; return NULL;
} }
/*
// todo: this method does not really belong to this file // todo: this method does not really belong to this file
static char* get2ndCaption(int loggingPoint) { static char* get2ndCaption(int loggingPoint) {
switch (loggingPoint) { switch (loggingPoint) {
@ -192,6 +197,7 @@ static char* get2ndCaption(int loggingPoint) {
firmwareError("No such loggingPoint"); firmwareError("No such loggingPoint");
return NULL; return NULL;
} }
*/
void initLoggingExt(Logging *logging, const char *name, char *buffer, int bufferSize) { void initLoggingExt(Logging *logging, const char *name, char *buffer, int bufferSize) {
print("Init logging %s\r\n", name); print("Init logging %s\r\n", name);
@ -284,8 +290,8 @@ static void printWithLength(char *line) {
if (!isConsoleReady()) if (!isConsoleReady())
return; return;
consoleOutputBuffer((const int8_t *) header, strlen(header)); consoleOutputBuffer((const uint8_t *) header, strlen(header));
consoleOutputBuffer((const int8_t *) line, p - line); consoleOutputBuffer((const uint8_t *) line, p - line);
} }
void printLine(Logging *logging) { void printLine(Logging *logging) {
@ -368,15 +374,17 @@ void scheduleLogging(Logging *logging) {
// strcpy(fatalMessage, "datalogging.c: output buffer overflow: "); // strcpy(fatalMessage, "datalogging.c: output buffer overflow: ");
// strcat(fatalMessage, logging->name); // strcat(fatalMessage, logging->name);
// fatal(fatalMessage); // fatal(fatalMessage);
if (!alreadyLocked) if (!alreadyLocked) {
unlockOutputBuffer(); unlockOutputBuffer();
}
resetLogging(logging); resetLogging(logging);
return; return;
} }
strcat(pendingBuffer, logging->buffer); strcat(pendingBuffer, logging->buffer);
if (!alreadyLocked) if (!alreadyLocked) {
unlockOutputBuffer(); unlockOutputBuffer();
}
resetLogging(logging); resetLogging(logging);
} }
@ -387,16 +395,17 @@ uint32_t remainingSize(Logging *logging) {
/** /**
* This method actually sends all the pending data to the communication layer * This method actually sends all the pending data to the communication layer
*/ */
void printPending() { void printPending(void) {
lockOutputBuffer(); lockOutputBuffer();
// we cannot output under syslock, so another buffer // we cannot output under syslock, so another buffer
strcpy(outputBuffer, pendingBuffer); strcpy(outputBuffer, pendingBuffer);
pendingBuffer[0] = 0; // reset pending buffer pendingBuffer[0] = 0; // reset pending buffer
unlockOutputBuffer(); unlockOutputBuffer();
if (strlen(outputBuffer) > 0) if (strlen(outputBuffer) > 0) {
printWithLength(outputBuffer); printWithLength(outputBuffer);
} }
}
void initIntermediateLoggingBuffer(void) { void initIntermediateLoggingBuffer(void) {
msObjectInit(&intermediateLoggingBuffer, intermediateLoggingBufferData, INTERMEDIATE_LOGGING_BUFFER_SIZE, 0); msObjectInit(&intermediateLoggingBuffer, intermediateLoggingBufferData, INTERMEDIATE_LOGGING_BUFFER_SIZE, 0);

View File

@ -20,6 +20,7 @@
*/ */
#include <string.h> #include <string.h>
#include "main.h"
#include "rfiutil.h" #include "rfiutil.h"
/* /*
@ -49,7 +50,7 @@ int mylog10(int param) {
char hexChar(int v) { char hexChar(int v) {
v = v & 0xF; v = v & 0xF;
if (v < 10) if (v < 10)
return '0' + v; return (char)('0' + v);
return 'A' - 10 + v; return 'A' - 10 + v;
} }
@ -68,8 +69,7 @@ int isLocked(void) {
void chVTSetAny(virtual_timer_t *vtp, systime_t time, vtfunc_t vtfunc, void *par) { void chVTSetAny(virtual_timer_t *vtp, systime_t time, vtfunc_t vtfunc, void *par) {
if (isIsrContext()) { if (isIsrContext()) {
chSysLockFromIsr() bool wasLocked = lockAnyContext();
;
/** /**
* todo: this could be simplified once we migrate to ChibiOS 3.0 * todo: this could be simplified once we migrate to ChibiOS 3.0
@ -79,6 +79,7 @@ void chVTSetAny(virtual_timer_t *vtp, systime_t time, vtfunc_t vtfunc, void *par
chVTResetI(vtp); chVTResetI(vtp);
chVTSetI(vtp, time, vtfunc, par); chVTSetI(vtp, time, vtfunc, par);
if (!wasLocked)
chSysUnlockFromIsr() chSysUnlockFromIsr()
; ;
} else { } else {

View File

@ -23,11 +23,11 @@ static void startPwmTest(int freq) {
scheduleMsg(&logger, "running pwm test @%d", freq); scheduleMsg(&logger, "running pwm test @%d", freq);
// PD13, GPIO_NONE because pin is initialized elsewhere already // PD13, GPIO_NONE because pin is initialized elsewhere already
startSimplePwm(&pwmTest[0], "tester", LED_WARNING, 10, 0.5); startSimplePwm(&pwmTest[0], "tester", LED_WARNING, 10, 0.5f);
// currently this is PB9 by default - see boardConfiguration->injectionPins // currently this is PB9 by default - see boardConfiguration->injectionPins
startSimplePwm(&pwmTest[1], "tester", INJECTOR_1_OUTPUT, freq / 1.3333333333, 0.5); startSimplePwm(&pwmTest[1], "tester", INJECTOR_1_OUTPUT, freq / 1.3333333333, 0.5f);
// currently this is PB8 by default // currently this is PB8 by default
startSimplePwm(&pwmTest[2], "tester", INJECTOR_2_OUTPUT, freq / 1000, 0.5); startSimplePwm(&pwmTest[2], "tester", INJECTOR_2_OUTPUT, freq / 1000, 0.5f);
// currently this is PE3 by default // currently this is PE3 by default
startSimplePwm(&pwmTest[3], "tester", INJECTOR_3_OUTPUT, freq, 0.5); startSimplePwm(&pwmTest[3], "tester", INJECTOR_3_OUTPUT, freq, 0.5);
// currently this is PE5 by default // currently this is PE5 by default

View File

@ -35,7 +35,7 @@ static Map3D1616 advanceMap;
float getBaseAdvance(int rpm, float engineLoad) { float getBaseAdvance(int rpm, float engineLoad) {
efiAssert(!cisnan(engineLoad), "invalid el", NAN); efiAssert(!cisnan(engineLoad), "invalid el", NAN);
efiAssert(!cisnan(engineLoad), "invalid rpm", NAN); efiAssert(!cisnan(engineLoad), "invalid rpm", NAN);
return advanceMap.getValue(engineLoad, engineConfiguration->ignitionLoadBins, rpm, return advanceMap.getValue(engineLoad, engineConfiguration->ignitionLoadBins, (float)rpm,
engineConfiguration->ignitionRpmBins); engineConfiguration->ignitionRpmBins);
} }

View File

@ -40,25 +40,20 @@ extern "C"
* these fields are not integrated with Tuner Studio. Step by step :) * these fields are not integrated with Tuner Studio. Step by step :)
*/ */
class engine_configuration2_s { class engine_configuration2_s {
// todo: move these fields into Engine class, eliminate this class
public: public:
engine_configuration2_s(); engine_configuration2_s();
Thermistor iat; Thermistor iat;
Thermistor clt; Thermistor clt;
// int crankAngleRange;
trigger_shape_s triggerShape; trigger_shape_s triggerShape;
cranking_ignition_mode_e crankingIgnitionMode;
EventHandlerConfiguration engineEventConfiguration; EventHandlerConfiguration engineEventConfiguration;
int isInjectionEnabledFlag;
/** /**
* This coefficient translates ADC value directly into voltage adjusted according to * This coefficient translates ADC value directly into voltage adjusted according to
* voltage divider configuration. * voltage divider configuration. This is a future (?) performance optimization.
*/ */
float adcToVoltageInputDividerCoefficient; float adcToVoltageInputDividerCoefficient;
}; };
@ -72,6 +67,7 @@ void prepareOutputSignals(engine_configuration_s *engineConfiguration,
engine_configuration2_s *engineConfiguration2); engine_configuration2_s *engineConfiguration2);
void initializeIgnitionActions(float advance, float dwellAngle, engine_configuration_s *engineConfiguration, engine_configuration2_s *engineConfiguration2, IgnitionEventList *list); void initializeIgnitionActions(float advance, float dwellAngle, engine_configuration_s *engineConfiguration, engine_configuration2_s *engineConfiguration2, IgnitionEventList *list);
float getFuelMultiplier(engine_configuration_s const *e, injection_mode_e mode);
void addFuelEvents(engine_configuration_s const *e, engine_configuration2_s *engineConfiguration2, ActuatorEventList *list, injection_mode_e mode); void addFuelEvents(engine_configuration_s const *e, engine_configuration2_s *engineConfiguration2, ActuatorEventList *list, injection_mode_e mode);
void registerActuatorEventExt(engine_configuration_s const *engineConfiguration, trigger_shape_s * s, ActuatorEvent *e, OutputSignal *actuator, float angleOffset); void registerActuatorEventExt(engine_configuration_s const *engineConfiguration, trigger_shape_s * s, ActuatorEvent *e, OutputSignal *actuator, float angleOffset);
@ -83,7 +79,6 @@ void resetConfigurationExt(Logging * logger, engine_type_e engineType,
void applyNonPersistentConfiguration(Logging * logger, engine_configuration_s *engineConfiguration, void applyNonPersistentConfiguration(Logging * logger, engine_configuration_s *engineConfiguration,
engine_configuration2_s *engineConfiguration2); engine_configuration2_s *engineConfiguration2);
void setDefaultNonPersistentConfiguration(engine_configuration2_s *engineConfiguration2); void setDefaultNonPersistentConfiguration(engine_configuration2_s *engineConfiguration2);
void printConfiguration(engine_configuration_s *engineConfiguration, engine_configuration2_s *engineConfiguration2); void printConfiguration(engine_configuration_s *engineConfiguration, engine_configuration2_s *engineConfiguration2);

View File

@ -48,6 +48,7 @@
#include "ford_escort_gt.h" #include "ford_escort_gt.h"
#include "citroenBerlingoTU3JP.h" #include "citroenBerlingoTU3JP.h"
#include "rover_v8.h" #include "rover_v8.h"
#include "mitsubishi.h"
static volatile int globalConfigurationVersion = 0; static volatile int globalConfigurationVersion = 0;
@ -179,8 +180,8 @@ void setDefaultConfiguration(engine_configuration_s *engineConfiguration, board_
engineConfiguration->analogChartMode = AC_TRIGGER; engineConfiguration->analogChartMode = AC_TRIGGER;
engineConfiguration->map.sensor.hwChannel = 4; engineConfiguration->map.sensor.hwChannel = EFI_ADC_4;
engineConfiguration->baroSensor.hwChannel = 4; engineConfiguration->baroSensor.hwChannel = EFI_ADC_4;
engineConfiguration->firingOrder = FO_1_THEN_3_THEN_4_THEN2; engineConfiguration->firingOrder = FO_1_THEN_3_THEN_4_THEN2;
engineConfiguration->crankingInjectionMode = IM_SIMULTANEOUS; engineConfiguration->crankingInjectionMode = IM_SIMULTANEOUS;
@ -219,18 +220,19 @@ void setDefaultConfiguration(engine_configuration_s *engineConfiguration, board_
engineConfiguration->displayMode = DM_HD44780; engineConfiguration->displayMode = DM_HD44780;
engineConfiguration->logFormat = LF_NATIVE; engineConfiguration->logFormat = LF_NATIVE;
engineConfiguration->directSelfStimulation = false;
engineConfiguration->triggerConfig.triggerType = TT_TOOTHED_WHEEL_60_2; engineConfiguration->triggerConfig.triggerType = TT_TOOTHED_WHEEL_60_2;
engineConfiguration->HD44780width = 16; engineConfiguration->HD44780width = 20;
engineConfiguration->HD44780height = 2; engineConfiguration->HD44780height = 4;
engineConfiguration->tpsAdcChannel = 3; engineConfiguration->tpsAdcChannel = EFI_ADC_3;
engineConfiguration->vBattAdcChannel = 5; engineConfiguration->vBattAdcChannel = EFI_ADC_5;
engineConfiguration->cltAdcChannel = 6; engineConfiguration->cltAdcChannel = EFI_ADC_6;
engineConfiguration->iatAdcChannel = 7; engineConfiguration->iatAdcChannel = EFI_ADC_7;
engineConfiguration->mafAdcChannel = 0; engineConfiguration->mafAdcChannel = EFI_ADC_0;
engineConfiguration->afrSensor.afrAdcChannel = 14; engineConfiguration->afrSensor.afrAdcChannel = EFI_ADC_14;
initBpsxD1Sensor(&engineConfiguration->afrSensor); initBpsxD1Sensor(&engineConfiguration->afrSensor);
@ -247,12 +249,21 @@ void setDefaultConfiguration(engine_configuration_s *engineConfiguration, board_
engineConfiguration->hasMapSensor = TRUE; engineConfiguration->hasMapSensor = TRUE;
engineConfiguration->hasCltSensor = TRUE; engineConfiguration->hasCltSensor = TRUE;
boardConfiguration->idleSolenoidFrequency = 200;
// engineConfiguration->idleMode = IM_AUTO;
engineConfiguration->idleMode = IM_MANUAL;
engineConfiguration->isInjectionEnabled = true;
engineConfiguration->isIgnitionEnabled = true;
engineConfiguration->isCylinderCleanupEnabled = true;
engineConfiguration->secondTriggerChannelEnabled = true;
boardConfiguration->idleValvePin = GPIOE_2; boardConfiguration->idleValvePin = GPIOE_2;
boardConfiguration->idleValvePinMode = OM_DEFAULT; boardConfiguration->idleValvePinMode = OM_DEFAULT;
boardConfiguration->fuelPumpPin = GPIOC_13; boardConfiguration->fuelPumpPin = GPIOC_13;
boardConfiguration->fuelPumpPinMode = OM_DEFAULT; boardConfiguration->fuelPumpPinMode = OM_DEFAULT;
boardConfiguration->electronicThrottlePin1 = GPIOC_9; boardConfiguration->electronicThrottlePin1 = GPIOC_9;
boardConfiguration->o2heaterPin = GPIO_NONE;
boardConfiguration->injectionPins[0] = GPIOB_9; boardConfiguration->injectionPins[0] = GPIOB_9;
boardConfiguration->injectionPins[1] = GPIOB_8; boardConfiguration->injectionPins[1] = GPIOB_8;
@ -292,9 +303,11 @@ void setDefaultConfiguration(engine_configuration_s *engineConfiguration, board_
boardConfiguration->triggerSimulatorPins[0] = GPIOD_1; boardConfiguration->triggerSimulatorPins[0] = GPIOD_1;
boardConfiguration->triggerSimulatorPins[1] = GPIOD_2; boardConfiguration->triggerSimulatorPins[1] = GPIOD_2;
boardConfiguration->triggerSimulatorPins[2] = GPIOD_3;
boardConfiguration->triggerSimulatorPinModes[0] = OM_DEFAULT; boardConfiguration->triggerSimulatorPinModes[0] = OM_DEFAULT;
boardConfiguration->triggerSimulatorPinModes[1] = OM_DEFAULT; boardConfiguration->triggerSimulatorPinModes[1] = OM_DEFAULT;
boardConfiguration->triggerSimulatorPinModes[2] = OM_DEFAULT;
boardConfiguration->HD44780_rs = GPIOE_9; boardConfiguration->HD44780_rs = GPIOE_9;
boardConfiguration->HD44780_e = GPIOE_11; boardConfiguration->HD44780_e = GPIOE_11;
@ -303,6 +316,9 @@ void setDefaultConfiguration(engine_configuration_s *engineConfiguration, board_
boardConfiguration->HD44780_db6 = GPIOB_11; boardConfiguration->HD44780_db6 = GPIOB_11;
boardConfiguration->HD44780_db7 = GPIOB_13; boardConfiguration->HD44780_db7 = GPIOB_13;
boardConfiguration->gps_rx_pin = GPIOB_7;
boardConfiguration->gps_tx_pin = GPIOB_6;
memset(boardConfiguration->adcHwChannelEnabled, 0, sizeof(boardConfiguration->adcHwChannelEnabled)); memset(boardConfiguration->adcHwChannelEnabled, 0, sizeof(boardConfiguration->adcHwChannelEnabled));
boardConfiguration->adcHwChannelEnabled[0] = ADC_SLOW; boardConfiguration->adcHwChannelEnabled[0] = ADC_SLOW;
boardConfiguration->adcHwChannelEnabled[1] = ADC_SLOW; boardConfiguration->adcHwChannelEnabled[1] = ADC_SLOW;
@ -316,10 +332,10 @@ void setDefaultConfiguration(engine_configuration_s *engineConfiguration, board_
boardConfiguration->adcHwChannelEnabled[12] = ADC_SLOW; boardConfiguration->adcHwChannelEnabled[12] = ADC_SLOW;
boardConfiguration->adcHwChannelEnabled[13] = ADC_SLOW; boardConfiguration->adcHwChannelEnabled[13] = ADC_SLOW;
boardConfiguration->primaryTriggerInputPin = GPIOC_6; boardConfiguration->triggerInputPins[0] = GPIOC_6;
boardConfiguration->secondaryTriggerInputPin = GPIOA_5; boardConfiguration->triggerInputPins[1] = GPIOA_5;
boardConfiguration->primaryLogicAnalyzerPin = GPIOA_8; boardConfiguration->logicAnalyzerPins[0] = GPIOA_8;
boardConfiguration->secondaryLogicAnalyzerPin = GPIOE_7; boardConfiguration->logicAnalyzerPins[1] = GPIOE_7;
boardConfiguration->idleThreadPeriod = 100; boardConfiguration->idleThreadPeriod = 100;
boardConfiguration->consoleLoopPeriod = 200; boardConfiguration->consoleLoopPeriod = 200;
@ -342,13 +358,8 @@ void setDefaultConfiguration(engine_configuration_s *engineConfiguration, board_
boardConfiguration->digitalPotentiometerChipSelect[3] = GPIO_NONE; boardConfiguration->digitalPotentiometerChipSelect[3] = GPIO_NONE;
} }
void setDefaultNonPersistentConfiguration(engine_configuration2_s *engineConfiguration2) { //void setDefaultNonPersistentConfiguration(engine_configuration2_s *engineConfiguration2) {
/** //}
* 720 is the range for four stroke
*/
// engineConfiguration2->crankAngleRange = 720;
}
void resetConfigurationExt(Logging * logger, engine_type_e engineType, engine_configuration_s *engineConfiguration, void resetConfigurationExt(Logging * logger, engine_type_e engineType, engine_configuration_s *engineConfiguration,
engine_configuration2_s *engineConfiguration2, board_configuration_s *boardConfiguration) { engine_configuration2_s *engineConfiguration2, board_configuration_s *boardConfiguration) {
@ -381,8 +392,17 @@ void resetConfigurationExt(Logging * logger, engine_type_e engineType, engine_co
setNissanPrimeraEngineConfiguration(engineConfiguration); setNissanPrimeraEngineConfiguration(engineConfiguration);
break; break;
#endif #endif
case HONDA_ACCORD: case HONDA_ACCORD_CD:
setHondaAccordConfiguration(engineConfiguration, boardConfiguration); setHondaAccordConfigurationThreeWires(engineConfiguration, boardConfiguration);
break;
case HONDA_ACCORD_CD_TWO_WIRES:
setHondaAccordConfigurationTwoWires(engineConfiguration, boardConfiguration);
break;
case HONDA_ACCORD_CD_DIP:
setHondaAccordConfigurationDip(engineConfiguration, boardConfiguration);
break;
case MITSU_4G93:
setMitsubishiConfiguration(engineConfiguration, boardConfiguration);
break; break;
#if EFI_SUPPORT_1995_FORD_INLINE_6 || defined(__DOXYGEN__) #if EFI_SUPPORT_1995_FORD_INLINE_6 || defined(__DOXYGEN__)
case FORD_INLINE_6_1995: case FORD_INLINE_6_1995:
@ -435,8 +455,6 @@ void applyNonPersistentConfiguration(Logging * logger, engine_configuration_s *e
#if EFI_PROD_CODE #if EFI_PROD_CODE
scheduleMsg(logger, "applyNonPersistentConfiguration()"); scheduleMsg(logger, "applyNonPersistentConfiguration()");
#endif #endif
engineConfiguration2->isInjectionEnabledFlag = TRUE;
initializeTriggerShape(logger, engineConfiguration, engineConfiguration2); initializeTriggerShape(logger, engineConfiguration, engineConfiguration2);
if (engineConfiguration2->triggerShape.getSize() == 0) { if (engineConfiguration2->triggerShape.getSize() == 0) {
firmwareError("triggerShape size is zero"); firmwareError("triggerShape size is zero");

View File

@ -77,7 +77,7 @@ typedef enum {
} can_device_mode_e; } can_device_mode_e;
typedef struct { typedef struct {
int afrAdcChannel; adc_channel_e afrAdcChannel;
float v1; float v1;
float value1; float value1;
float v2; float v2;
@ -143,8 +143,13 @@ typedef struct {
brain_pin_e HD44780_db6; brain_pin_e HD44780_db6;
brain_pin_e HD44780_db7; brain_pin_e HD44780_db7;
brain_pin_e triggerSimulatorPins[2]; brain_pin_e gps_rx_pin;
pin_output_mode_e triggerSimulatorPinModes[2]; brain_pin_e gps_tx_pin;
int idleSolenoidFrequency;
int unused;
/** /**
* Digital Potentiometer is used by stock ECU stimulation code * Digital Potentiometer is used by stock ECU stimulation code
@ -154,10 +159,8 @@ typedef struct {
adc_channel_mode_e adcHwChannelEnabled[HW_MAX_ADC_INDEX]; adc_channel_mode_e adcHwChannelEnabled[HW_MAX_ADC_INDEX];
brain_pin_e primaryTriggerInputPin; brain_pin_e triggerInputPins[2];
brain_pin_e secondaryTriggerInputPin; brain_pin_e logicAnalyzerPins[2];
brain_pin_e primaryLogicAnalyzerPin;
brain_pin_e secondaryLogicAnalyzerPin;
int idleThreadPeriod; int idleThreadPeriod;
int consoleLoopPeriod; int consoleLoopPeriod;
@ -173,6 +176,15 @@ typedef struct {
brain_pin_e canTxPin; brain_pin_e canTxPin;
brain_pin_e canRxPin; brain_pin_e canRxPin;
brain_pin_e triggerSimulatorPins[3];
pin_output_mode_e triggerSimulatorPinModes[3];
brain_pin_e o2heaterPin;
pin_output_mode_e o2heaterPinModeTodO;
int unused2[8];
} board_configuration_s; } board_configuration_s;
@ -207,7 +219,11 @@ typedef struct {
float iatFuelCorrBins[IAT_CURVE_SIZE]; // size 64, offset 200 float iatFuelCorrBins[IAT_CURVE_SIZE]; // size 64, offset 200
float iatFuelCorr[IAT_CURVE_SIZE]; // size 64, offset 264 float iatFuelCorr[IAT_CURVE_SIZE]; // size 64, offset 264
short int unused2; // size 2, offset 328 /**
* Should the trigger emulator push data right into trigger input, eliminating the need for physical jumper wires?
* PS: Funny name, right? :)
*/
short int directSelfStimulation; // size 2, offset 328
// todo: extract these two fields into a structure // todo: extract these two fields into a structure
// todo: we need two sets of TPS parameters - modern ETBs have to sensors // todo: we need two sets of TPS parameters - modern ETBs have to sensors
@ -265,7 +281,6 @@ typedef struct {
injection_mode_e crankingInjectionMode; injection_mode_e crankingInjectionMode;
injection_mode_e injectionMode; injection_mode_e injectionMode;
/** /**
* Inside rusEfi all the angles are handled in relation to the trigger synchronization event * Inside rusEfi all the angles are handled in relation to the trigger synchronization event
* which depends on the trigger shape and has nothing to do wit Top Dead Center (TDC) * which depends on the trigger shape and has nothing to do wit Top Dead Center (TDC)
@ -329,21 +344,21 @@ typedef struct {
int HD44780width; int HD44780width;
int HD44780height; int HD44780height;
int tpsAdcChannel; adc_channel_e tpsAdcChannel;
int overrideCrankingIgnition; int overrideCrankingIgnition;
int analogChartFrequency; int analogChartFrequency;
trigger_config_s triggerConfig; trigger_config_s triggerConfig;
int space; int space;
int vBattAdcChannel; adc_channel_e vBattAdcChannel;
float globalFuelCorrection; float globalFuelCorrection;
// todo: merge with channel settings, use full-scale Thermistor! // todo: merge with channel settings, use full-scale Thermistor!
int cltAdcChannel; adc_channel_e cltAdcChannel;
int iatAdcChannel; adc_channel_e iatAdcChannel;
int mafAdcChannel; adc_channel_e mafAdcChannel;
afr_sensor_s afrSensor; afr_sensor_s afrSensor;
@ -367,12 +382,20 @@ typedef struct {
float veTable[VE_LOAD_COUNT][VE_RPM_COUNT]; // size 1024 float veTable[VE_LOAD_COUNT][VE_RPM_COUNT]; // size 1024
float afrTable[AFR_LOAD_COUNT][AFR_RPM_COUNT]; // size 1024 float afrTable[AFR_LOAD_COUNT][AFR_RPM_COUNT]; // size 1024
board_configuration_s bc; board_configuration_s bc;
int hasMapSensor; int hasMapSensor;
int hasCltSensor; int hasCltSensor;
idle_mode_e idleMode;
bool isInjectionEnabled : 1; // bit 0
bool isIgnitionEnabled : 1; // bit 1
bool isCylinderCleanupEnabled : 1; // bit 2
bool secondTriggerChannelEnabled : 1; // bit 3
int unused3[8];
} engine_configuration_s; } engine_configuration_s;

View File

@ -29,6 +29,7 @@ int warning(obd_code_e code, const char *fmt, ...);
*/ */
void firmwareError(const char *fmt, ...); void firmwareError(const char *fmt, ...);
bool hasFirmwareError(void); bool hasFirmwareError(void);
char *getFirmwareError(void);
/** /**
* declared as a macro so that this code does not use stack * declared as a macro so that this code does not use stack
@ -43,6 +44,7 @@ bool hasFirmwareError(void);
void chDbgPanic3(const char *msg, const char * file, int line); void chDbgPanic3(const char *msg, const char * file, int line);
void initErrorHandling(void); void initErrorHandling(void);
char *getWarninig(void);
// todo: better place for this shared declaration? // todo: better place for this shared declaration?
int getRusEfiVersion(void); int getRusEfiVersion(void);

View File

@ -62,7 +62,7 @@ void ArrayList< Type, Dimention>::resetEventList(void) {
template <class Type, int Dimention> template <class Type, int Dimention>
Type * ArrayList< Type, Dimention>::getNextActuatorEvent(void) { Type * ArrayList< Type, Dimention>::getNextActuatorEvent(void) {
efiAssert(size < Dimention, "registerActuatorEvent() too many events", NULL); efiAssert(size < Dimention, "registerActuatorEvent() too many events", (Type *)NULL);
return &events[size++]; return &events[size++];
} }

View File

@ -23,6 +23,7 @@ float getIatCorrection(float iat);
float getInjectorLag(float vBatt); float getInjectorLag(float vBatt);
float getCltCorrection(float clt); float getCltCorrection(float clt);
float getRunningFuel(float baseFuel, Engine *engine, int rpm); float getRunningFuel(float baseFuel, Engine *engine, int rpm);
float getCrankingFuel(void);
float getStartingFuel(float coolantTemperature); float getStartingFuel(float coolantTemperature);
float getFuelMs(int rpm, Engine *engine); float getFuelMs(int rpm, Engine *engine);

View File

@ -78,10 +78,10 @@ int getIdle(IdleValveState *idle, int currentRpm, int now) {
} }
if (currentRpm >= idle->targetRpmRangeRight + 100) if (currentRpm >= idle->targetRpmRangeRight + 100)
return changeValue(idle, currentRpm, now, "rpm is too high: ", -IDLE_DECREASE_STEP); return changeValue(idle, currentRpm, now, "idle control: rpm is too high: ", -IDLE_DECREASE_STEP);
if (currentRpm >= idle->targetRpmRangeRight) if (currentRpm >= idle->targetRpmRangeRight)
return changeValue(idle, currentRpm, now, "rpm is a bit too high: ", -1); return changeValue(idle, currentRpm, now, "idle control: rpm is a bit too high: ", -1);
// we are here if RPM is low, let's see how low // we are here if RPM is low, let's see how low
// if (currentRpm < 0.7 * idle->targetRpmRangeLeft) { // if (currentRpm < 0.7 * idle->targetRpmRangeLeft) {
@ -89,7 +89,7 @@ int getIdle(IdleValveState *idle, int currentRpm, int now) {
// return setNewValue(idle, currentRpm, now, "RPMs are seriously low: ", 15 * IDLE_INCREASE_STEP); // return setNewValue(idle, currentRpm, now, "RPMs are seriously low: ", 15 * IDLE_INCREASE_STEP);
// } else // } else
if (currentRpm < idle->targetRpmRangeLeft - 100) { if (currentRpm < idle->targetRpmRangeLeft - 100) {
return changeValue(idle, currentRpm, now, "RPMs are low: ", IDLE_INCREASE_STEP); return changeValue(idle, currentRpm, now, "idle control: RPMs are low: ", IDLE_INCREASE_STEP);
} }
return changeValue(idle, currentRpm, now, "RPMs are a bit low: ", 1); return changeValue(idle, currentRpm, now, "idle control: RPMs are a bit low: ", 1);
} }

View File

@ -12,6 +12,9 @@
#define GPIO_NULL NULL #define GPIO_NULL NULL
/**
* Logical pins. See brain_pin_e for physical pins.
*/
typedef enum { typedef enum {
LED_WARNING, // Orange on-board LED LED_WARNING, // Orange on-board LED
LED_RUNNING, // Green on-board LED LED_RUNNING, // Green on-board LED
@ -23,9 +26,13 @@ typedef enum {
LED_DEBUG, LED_DEBUG,
LED_EMULATOR, LED_EMULATOR,
/**
* see board_configuration_s->idleValvePin
*/
IDLE_VALVE, IDLE_VALVE,
TRIGGER_EMULATOR_PRIMARY, TRIGGER_EMULATOR_PRIMARY,
TRIGGER_EMULATOR_SECONDARY, TRIGGER_EMULATOR_SECONDARY,
TRIGGER_EMULATOR_3RD,
SPARKOUT_1_OUTPUT, SPARKOUT_1_OUTPUT,
SPARKOUT_2_OUTPUT, SPARKOUT_2_OUTPUT,
@ -57,6 +64,12 @@ typedef enum {
ELECTRONIC_THROTTLE_CONTROL_2, ELECTRONIC_THROTTLE_CONTROL_2,
ELECTRONIC_THROTTLE_CONTROL_3, ELECTRONIC_THROTTLE_CONTROL_3,
/**
* these seven segment display pins are related to unused external tachometer code
* I still have the hardware so maybe one day I will fix it, but for now it's just dead code
* See https://www.youtube.com/watch?v=YYiHoN6MBqE
* todo: this should be re-implemented in a smarter way with some sort of multiplexing anyway
*/
/* digit 1 */ /* digit 1 */
LED_HUGE_0, // B2 LED_HUGE_0, // B2
LED_HUGE_1, LED_HUGE_1,
@ -87,6 +100,7 @@ typedef enum {
FUEL_PUMP_RELAY, FUEL_PUMP_RELAY,
FAN_RELAY, FAN_RELAY,
O2_HEATER,
SPI_CS_1, SPI_CS_1,
SPI_CS_2, SPI_CS_2,

View File

@ -46,7 +46,7 @@ typedef enum {
NISSAN_PRIMERA = 5, NISSAN_PRIMERA = 5,
#endif /* EFI_SUPPORT_NISSAN_PRIMERA */ #endif /* EFI_SUPPORT_NISSAN_PRIMERA */
HONDA_ACCORD = 6, HONDA_ACCORD_CD = 6,
FORD_INLINE_6_1995 = 7, FORD_INLINE_6_1995 = 7,
/** /**
@ -70,6 +70,15 @@ typedef enum {
CITROEN_TU3JP = 15, CITROEN_TU3JP = 15,
MITSU_4G93 = 16,
/**
* a version of HONDA_ACCORD_CD which only uses two of three trigger input sensors
*/
HONDA_ACCORD_CD_TWO_WIRES = 17,
HONDA_ACCORD_CD_DIP = 18,
Internal_ForceMyEnumIntSize_engine_type = ENUM_SIZE_HACK, Internal_ForceMyEnumIntSize_engine_type = ENUM_SIZE_HACK,
} engine_type_e; } engine_type_e;
@ -85,6 +94,14 @@ typedef enum {
TT_TOOTHED_WHEEL_60_2 = 8, TT_TOOTHED_WHEEL_60_2 = 8,
TT_TOOTHED_WHEEL_36_1 = 9, TT_TOOTHED_WHEEL_36_1 = 9,
TT_HONDA_ACCORD_CD = 10,
TT_MITSU = 11,
TT_HONDA_ACCORD_CD_TWO_WIRES = 12,
TT_HONDA_ACCORD_CD_DIP = 13,
Internal_ForceMyEnumIntSize_trigger_type = ENUM_SIZE_HACK, Internal_ForceMyEnumIntSize_trigger_type = ENUM_SIZE_HACK,
} trigger_type_e; } trigger_type_e;
@ -96,11 +113,28 @@ typedef enum {
Internal_ForceMyEnumIntSize_adc_channel_mode = ENUM_SIZE_HACK, Internal_ForceMyEnumIntSize_adc_channel_mode = ENUM_SIZE_HACK,
} adc_channel_mode_e; } adc_channel_mode_e;
// todo: better names?
typedef enum { typedef enum {
SHAFT_PRIMARY_UP = 0, TV_LOW = 0,
SHAFT_PRIMARY_DOWN = 1, TV_HIGH = 1
SHAFT_SECONDARY_UP = 2, } trigger_value_e;
SHAFT_SECONDARY_DOWN = 3,
// todo: better names?
typedef enum {
T_PRIMARY = 0,
T_SECONDARY = 1,
// todo: I really do not want to call this 'tertiary'. maybe we should rename all of these?
T_CHANNEL_3 = 2
} trigger_wheel_e;
// todo: better names?
typedef enum {
SHAFT_PRIMARY_DOWN = 0,
SHAFT_PRIMARY_UP = 1,
SHAFT_SECONDARY_DOWN = 2,
SHAFT_SECONDARY_UP = 3,
SHAFT_3RD_DOWN = 4,
SHAFT_3RD_UP = 5,
} trigger_event_e; } trigger_event_e;
/** /**
@ -149,6 +183,13 @@ typedef enum {
Internal_ForceMyEnumIntSize_log_format = ENUM_SIZE_HACK, Internal_ForceMyEnumIntSize_log_format = ENUM_SIZE_HACK,
} log_format_e; } log_format_e;
typedef enum {
IM_AUTO = 0,
IM_MANUAL = 1,
Internal_ForceMyEnumIntSize_idle_mode = ENUM_SIZE_HACK,
} idle_mode_e;
typedef enum { typedef enum {
/** /**
* GND for logical OFF, VCC for logical ON * GND for logical OFF, VCC for logical ON
@ -244,6 +285,29 @@ typedef enum {
Internal_ForceMyEnumIntSize_cranking_internal_error = ENUM_SIZE_HACK, Internal_ForceMyEnumIntSize_cranking_internal_error = ENUM_SIZE_HACK,
} internal_error_e; } internal_error_e;
typedef enum {
EFI_ADC_0 = 0,
EFI_ADC_1 = 1,
EFI_ADC_2 = 2,
EFI_ADC_3 = 3,
EFI_ADC_4 = 4,
EFI_ADC_5 = 5,
EFI_ADC_6 = 6,
EFI_ADC_7 = 7,
EFI_ADC_8 = 8,
EFI_ADC_9 = 9,
EFI_ADC_10 = 10,
EFI_ADC_11 = 11,
EFI_ADC_12 = 12,
EFI_ADC_13 = 13,
EFI_ADC_14 = 14,
EFI_ADC_15 = 15,
EFI_ADC_ERROR = 999,
Internal_ForceMyEnumIntSize_cranking_adc_channel = ENUM_SIZE_HACK,
} adc_channel_e;
/** /**
* Hardware pin. This enum is platform-specific. * Hardware pin. This enum is platform-specific.
*/ */
@ -334,6 +398,7 @@ typedef enum {
GPIOE_15 = 79, GPIOE_15 = 79,
GPIO_NONE = 80, GPIO_NONE = 80,
GPIO_INVALID = 81,
Internal_ForceMyEnumIntSize_cranking_brain_pin = ENUM_SIZE_HACK, Internal_ForceMyEnumIntSize_cranking_brain_pin = ENUM_SIZE_HACK,
} brain_pin_e; } brain_pin_e;

View File

@ -79,7 +79,7 @@ void resetWaveChart(WaveChart *chart) {
static char WAVE_LOGGING_BUFFER[WAVE_LOGGING_SIZE] CCM_OPTIONAL static char WAVE_LOGGING_BUFFER[WAVE_LOGGING_SIZE] CCM_OPTIONAL
; ;
int isWaveChartFull(WaveChart *chart) { static int isWaveChartFull(WaveChart *chart) {
return chart->counter >= chartSize; return chart->counter >= chartSize;
} }
@ -94,8 +94,9 @@ static void setChartActive(int value) {
} }
void setChartSize(int newSize) { void setChartSize(int newSize) {
if (newSize < 5) if (newSize < 5) {
return; return;
}
chartSize = newSize; chartSize = newSize;
printStatus(); printStatus();
} }
@ -114,9 +115,11 @@ void publishChart(WaveChart *chart) {
Logging *l = &chart->logging; Logging *l = &chart->logging;
scheduleSimpleMsg(&debugLogging, "IT'S TIME", strlen(l->buffer)); scheduleSimpleMsg(&debugLogging, "IT'S TIME", strlen(l->buffer));
#endif #endif
if (isChartActive && getFullLog()) bool isFullLog = getFullLog();
if (isChartActive && isFullLog) {
scheduleLogging(&chart->logging); scheduleLogging(&chart->logging);
} }
}
static char timeBuffer[10]; static char timeBuffer[10];
@ -128,8 +131,9 @@ void addWaveChartEvent3(WaveChart *chart, const char *name, const char * msg, co
#if DEBUG_WAVE #if DEBUG_WAVE
scheduleSimpleMsg(&debugLogging, "current", chart->counter); scheduleSimpleMsg(&debugLogging, "current", chart->counter);
#endif #endif
if (isWaveChartFull(chart)) if (isWaveChartFull(chart)) {
return; return;
}
#if EFI_HISTOGRAMS && EFI_PROD_CODE #if EFI_HISTOGRAMS && EFI_PROD_CODE
int beforeCallback = hal_lld_get_counter_value(); int beforeCallback = hal_lld_get_counter_value();
@ -140,8 +144,9 @@ void addWaveChartEvent3(WaveChart *chart, const char *name, const char * msg, co
bool alreadyLocked = lockOutputBuffer(); // we have multiple threads writing to the same output buffer bool alreadyLocked = lockOutputBuffer(); // we have multiple threads writing to the same output buffer
if (chart->counter == 0) if (chart->counter == 0) {
chart->startTime = time100; chart->startTime = time100;
}
chart->counter++; chart->counter++;
if (remainingSize(&chart->logging) > 30) { if (remainingSize(&chart->logging) > 30) {
/** /**
@ -161,13 +166,15 @@ void addWaveChartEvent3(WaveChart *chart, const char *name, const char * msg, co
appendFast(&chart->logging, msg2); appendFast(&chart->logging, msg2);
appendFast(&chart->logging, CHART_DELIMETER); appendFast(&chart->logging, CHART_DELIMETER);
} }
if (!alreadyLocked) if (!alreadyLocked) {
unlockOutputBuffer(); unlockOutputBuffer();
}
#if EFI_HISTOGRAMS && EFI_PROD_CODE #if EFI_HISTOGRAMS && EFI_PROD_CODE
int diff = hal_lld_get_counter_value() - beforeCallback; int64_t diff = hal_lld_get_counter_value() - beforeCallback;
if (diff > 0) if (diff > 0) {
hsAdd(&waveChartHisto, diff); hsAdd(&waveChartHisto, diff);
}
#endif /* EFI_HISTOGRAMS */ #endif /* EFI_HISTOGRAMS */
} }
@ -181,8 +188,9 @@ void showWaveChartHistogram(void) {
void initWaveChart(WaveChart *chart) { void initWaveChart(WaveChart *chart) {
initLogging(&logger, "wave info"); initLogging(&logger, "wave info");
if (!isChartActive) if (!isChartActive) {
printMsg(&logger, "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! chart disabled"); printMsg(&logger, "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! chart disabled");
}
printStatus(); printStatus();

View File

@ -10,7 +10,6 @@
#include "main.h" #include "main.h"
#include "rpm_calculator.h" #include "rpm_calculator.h"
#include "pwm_generator.h" #include "pwm_generator.h"
#include "wave_math.h"
#include "alternatorController.h" #include "alternatorController.h"
#include "pin_repository.h" #include "pin_repository.h"
#include "engine_configuration.h" #include "engine_configuration.h"

View File

@ -13,4 +13,5 @@ CONTROLLERS_SRC_CPP = $(PROJECT_DIR)/controllers/settings.cpp \
controllers/idle_thread.cpp \ controllers/idle_thread.cpp \
controllers/PwmTester.cpp \ controllers/PwmTester.cpp \
$(PROJECT_DIR)/controllers/alternatorController.cpp \ $(PROJECT_DIR)/controllers/alternatorController.cpp \
$(PROJECT_DIR)/controllers/lcd_controller.cpp \
$(PROJECT_DIR)/controllers/engine_controller.cpp $(PROJECT_DIR)/controllers/engine_controller.cpp

View File

@ -44,7 +44,7 @@ float multi_wave_s::getSwitchTime(int index) const {
void checkSwitchTimes2(int size, float *switchTimes) { void checkSwitchTimes2(int size, float *switchTimes) {
for (int i = 0; i < size - 1; i++) { for (int i = 0; i < size - 1; i++) {
if (switchTimes[i] >= switchTimes[i + 1]) { if (switchTimes[i] >= switchTimes[i + 1]) {
firmwareError("invalid switchTimes"); firmwareError("invalid switchTimes @%d: %f/%f", i, switchTimes[i], switchTimes[i + 1]);
} }
} }
} }

View File

@ -10,7 +10,7 @@
#include "engine_configuration.h" #include "engine_configuration.h"
#define PWM_PHASE_MAX_COUNT 250 #define PWM_PHASE_MAX_COUNT 250
#define PWM_PHASE_MAX_WAVE_PER_PWM 2 #define PWM_PHASE_MAX_WAVE_PER_PWM 3
/** /**
* @brief PWM configuration for the specific output pin * @brief PWM configuration for the specific output pin
@ -35,6 +35,10 @@ public:
void setSwitchTime(int phaseIndex, float value); void setSwitchTime(int phaseIndex, float value);
void checkSwitchTimes(int size); void checkSwitchTimes(int size);
int getChannelState(int channelIndex, int phaseIndex) const; int getChannelState(int channelIndex, int phaseIndex) const;
int findAngleMatch(float angle, int size) const;
int waveIndertionAngle(float angle, int size) const;
/** /**
* Number of signal wires * Number of signal wires
*/ */
@ -49,5 +53,7 @@ public:
}; };
void checkSwitchTimes2(int size, float *switchTimes); void checkSwitchTimes2(int size, float *switchTimes);
void configureHondaAccordCD(trigger_shape_s *s, bool with3rdSignal);
void configureHondaAccordCDDip(trigger_shape_s *s);
#endif /* EFI_WAVE_H_ */ #endif /* EFI_WAVE_H_ */

View File

@ -22,10 +22,9 @@ void avgReset(AvgTable *table) {
} }
void avgAddValue(AvgTable *table, int rpm, float key, float value) { void avgAddValue(AvgTable *table, int rpm, float key, float value) {
if (rpm >= MAX_RPM) if (rpm >= MAX_RPM || key >= MAX_KEY) {
return;
if (key >= MAX_KEY)
return; return;
}
int i = (int)(AVG_TAB_SIZE * rpm / MAX_RPM); int i = (int)(AVG_TAB_SIZE * rpm / MAX_RPM);
int j = (int)(AVG_TAB_SIZE * key / MAX_KEY); int j = (int)(AVG_TAB_SIZE * key / MAX_KEY);
@ -35,26 +34,25 @@ void avgAddValue(AvgTable *table, int rpm, float key, float value) {
float avgGetValueByIndexes(AvgTable *table, int i, int j) { float avgGetValueByIndexes(AvgTable *table, int i, int j) {
int count = table->counts[i][j]; int count = table->counts[i][j];
if (count == 0) if (count == 0) {
return NAN; return NAN;
}
return table->values[i][j] / count; return table->values[i][j] / count;
} }
float avgGetValue(AvgTable *table, int rpm, float key) { float avgGetValue(AvgTable *table, int rpm, float key) {
if (rpm >= MAX_RPM) if (rpm >= MAX_RPM || key >= MAX_KEY) {
return NAN;
if (key >= MAX_KEY)
return NAN; return NAN;
}
int i = (int)(AVG_TAB_SIZE * rpm / MAX_RPM); int i = (int)(AVG_TAB_SIZE * rpm / MAX_RPM);
int j = (int)(AVG_TAB_SIZE * key / MAX_KEY); int j = (int)(AVG_TAB_SIZE * key / MAX_KEY);
return avgGetValueByIndexes(table, i, j); return avgGetValueByIndexes(table, i, j);
} }
int avgGetValuesCount(AvgTable *table, int rpm, float key) { int avgGetValuesCount(AvgTable *table, int rpm, float key) {
if (rpm >= MAX_RPM) if (rpm >= MAX_RPM || key >= MAX_KEY) {
return 0;
if (key >= MAX_KEY)
return 0; return 0;
}
int i = (int)(AVG_TAB_SIZE * rpm / MAX_RPM); int i = (int)(AVG_TAB_SIZE * rpm / MAX_RPM);
int j = (int)(AVG_TAB_SIZE * key / MAX_KEY); int j = (int)(AVG_TAB_SIZE * key / MAX_KEY);

View File

@ -26,8 +26,9 @@ private:
template<int RPM_BIN_SIZE, int LOAD_BIN_SIZE> template<int RPM_BIN_SIZE, int LOAD_BIN_SIZE>
void Map3D<RPM_BIN_SIZE, LOAD_BIN_SIZE>::init(float table[RPM_BIN_SIZE][LOAD_BIN_SIZE]) { void Map3D<RPM_BIN_SIZE, LOAD_BIN_SIZE>::init(float table[RPM_BIN_SIZE][LOAD_BIN_SIZE]) {
for (int k = 0; k < LOAD_BIN_SIZE; k++) for (int k = 0; k < LOAD_BIN_SIZE; k++) {
pointers[k] = table[k]; pointers[k] = table[k];
}
initialized = MAGIC_TRUE_VALUE; initialized = MAGIC_TRUE_VALUE;
} }

View File

@ -30,6 +30,7 @@
#include "main_trigger_callback.h" #include "main_trigger_callback.h"
#include "map_multiplier_thread.h" #include "map_multiplier_thread.h"
#include "io_pins.h" #include "io_pins.h"
#include "flash_main.h"
#include "tunerstudio.h" #include "tunerstudio.h"
#include "injector_central.h" #include "injector_central.h"
#include "ignition_central.h" #include "ignition_central.h"
@ -54,7 +55,8 @@
extern board_configuration_s *boardConfiguration; extern board_configuration_s *boardConfiguration;
persistent_config_container_s persistentState CCM_OPTIONAL; persistent_config_container_s persistentState CCM_OPTIONAL
;
engine_configuration_s *engineConfiguration = &persistentState.persistentConfiguration.engineConfiguration; engine_configuration_s *engineConfiguration = &persistentState.persistentConfiguration.engineConfiguration;
board_configuration_s *boardConfiguration = &persistentState.persistentConfiguration.engineConfiguration.bc; board_configuration_s *boardConfiguration = &persistentState.persistentConfiguration.engineConfiguration.bc;
@ -69,7 +71,8 @@ static VirtualTimer fuelPumpTimer;
static Logging logger; static Logging logger;
static engine_configuration2_s ec2 CCM_OPTIONAL; static engine_configuration2_s ec2 CCM_OPTIONAL
;
engine_configuration2_s * engineConfiguration2 = &ec2; engine_configuration2_s * engineConfiguration2 = &ec2;
static configuration_s cfg = { &persistentState.persistentConfiguration.engineConfiguration, &ec2 }; static configuration_s cfg = { &persistentState.persistentConfiguration.engineConfiguration, &ec2 };
@ -104,10 +107,8 @@ static void updateErrorCodes(void) {
/** /**
* technically we can set error codes right inside the getMethods, but I a bit on a fence about it * technically we can set error codes right inside the getMethods, but I a bit on a fence about it
*/ */
setError(isValidIntakeAirTemperature(getIntakeAirTemperature()), setError(isValidIntakeAirTemperature(getIntakeAirTemperature()), OBD_Intake_Air_Temperature_Circuit_Malfunction);
OBD_Intake_Air_Temperature_Circuit_Malfunction); setError(isValidCoolantTemperature(getCoolantTemperature()), OBD_Engine_Coolant_Temperature_Circuit_Malfunction);
setError(isValidCoolantTemperature(getCoolantTemperature()),
OBD_Engine_Coolant_Temperature_Circuit_Malfunction);
} }
static void fanRelayControl(void) { static void fanRelayControl(void) {
@ -118,11 +119,9 @@ static void fanRelayControl(void) {
int newValue; int newValue;
if (isCurrentlyOn) { if (isCurrentlyOn) {
// if the fan is already on, we keep it on till the 'fanOff' temperature // if the fan is already on, we keep it on till the 'fanOff' temperature
newValue = getCoolantTemperature() newValue = getCoolantTemperature() > engineConfiguration->fanOffTemperature;
> engineConfiguration->fanOffTemperature;
} else { } else {
newValue = getCoolantTemperature() newValue = getCoolantTemperature() > engineConfiguration->fanOnTemperature;
> engineConfiguration->fanOnTemperature;
} }
if (isCurrentlyOn != newValue) { if (isCurrentlyOn != newValue) {
@ -156,25 +155,31 @@ static void onEvenyGeneralMilliseconds(void *arg) {
*/ */
halTime.get(hal_lld_get_counter_value(), true); halTime.get(hal_lld_get_counter_value(), true);
if (!engine.rpmCalculator->isRunning())
writeToFlashIfPending();
engine.updateSlowSensors(); engine.updateSlowSensors();
updateErrorCodes(); updateErrorCodes();
fanRelayControl(); fanRelayControl();
setOutputPinValue(O2_HEATER, engine.rpmCalculator->isRunning());
// schedule next invocation // schedule next invocation
chVTSetAny(&everyMsTimer, boardConfiguration->generalPeriodicThreadPeriod * TICKS_IN_MS, &onEvenyGeneralMilliseconds, 0); chVTSetAny(&everyMsTimer, boardConfiguration->generalPeriodicThreadPeriod * TICKS_IN_MS,
&onEvenyGeneralMilliseconds, 0);
} }
static void initPeriodicEvents(void) { static void initPeriodicEvents(void) {
// schedule first invocation // schedule first invocation
chVTSetAny(&everyMsTimer, boardConfiguration->generalPeriodicThreadPeriod * TICKS_IN_MS, &onEvenyGeneralMilliseconds, 0); chVTSetAny(&everyMsTimer, boardConfiguration->generalPeriodicThreadPeriod * TICKS_IN_MS,
&onEvenyGeneralMilliseconds, 0);
} }
static void fuelPumpOff(void *arg) { static void fuelPumpOff(void *arg) {
if (getOutputPinValue(FUEL_PUMP_RELAY)) if (getOutputPinValue(FUEL_PUMP_RELAY))
scheduleMsg(&logger, "fuelPump OFF at %s%d", scheduleMsg(&logger, "fuelPump OFF at %s%d", hwPortname(boardConfiguration->fuelPumpPin));
hwPortname(boardConfiguration->fuelPumpPin));
turnOutputPinOff(FUEL_PUMP_RELAY); turnOutputPinOff(FUEL_PUMP_RELAY);
} }
@ -182,10 +187,8 @@ static void fuelPumpOn(trigger_event_e signal, int index, void *arg) {
if (index != 0) if (index != 0)
return; // let's not abuse the timer - one time per revolution would be enough return; // let's not abuse the timer - one time per revolution would be enough
// todo: the check about GPIO_NONE should be somewhere else! // todo: the check about GPIO_NONE should be somewhere else!
if (!getOutputPinValue(FUEL_PUMP_RELAY) if (!getOutputPinValue(FUEL_PUMP_RELAY) && boardConfiguration->fuelPumpPin != GPIO_NONE)
&& boardConfiguration->fuelPumpPin != GPIO_NONE) scheduleMsg(&logger, "fuelPump ON at %s", hwPortname(boardConfiguration->fuelPumpPin));
scheduleMsg(&logger, "fuelPump ON at %s",
hwPortname(boardConfiguration->fuelPumpPin));
turnOutputPinOn(FUEL_PUMP_RELAY); turnOutputPinOn(FUEL_PUMP_RELAY);
/** /**
* the idea of this implementation is that we turn the pump when the ECU turns on or * the idea of this implementation is that we turn the pump when the ECU turns on or
@ -200,7 +203,7 @@ static void initFuelPump(void) {
fuelPumpOn(SHAFT_PRIMARY_UP, 0, NULL); fuelPumpOn(SHAFT_PRIMARY_UP, 0, NULL);
} }
char * getPinNameByAdcChannel(int hwChannel, char *buffer) { char * getPinNameByAdcChannel(adc_channel_e hwChannel, char *buffer) {
strcpy((char*) buffer, portname(getAdcChannelPort(hwChannel))); strcpy((char*) buffer, portname(getAdcChannelPort(hwChannel)));
itoa10(&buffer[2], getAdcChannelPin(hwChannel)); itoa10(&buffer[2], getAdcChannelPin(hwChannel));
return (char*) buffer; return (char*) buffer;
@ -208,14 +211,14 @@ char * getPinNameByAdcChannel(int hwChannel, char *buffer) {
static char pinNameBuffer[16]; static char pinNameBuffer[16];
static void printAnalogChannelInfoExt(const char *name, int hwChannel, static void printAnalogChannelInfoExt(const char *name, adc_channel_e hwChannel, float adcVoltage) {
float adcVoltage) {
float voltage = adcVoltage * engineConfiguration->analogInputDividerCoefficient; float voltage = adcVoltage * engineConfiguration->analogInputDividerCoefficient;
scheduleMsg(&logger, "%s ADC%d %s rawValue=%f/divided=%fv", name, hwChannel, scheduleMsg(&logger, "%s ADC%d %s %s rawValue=%f/divided=%fv", name, hwChannel,
getAdcMode(hwChannel),
getPinNameByAdcChannel(hwChannel, pinNameBuffer), adcVoltage, voltage); getPinNameByAdcChannel(hwChannel, pinNameBuffer), adcVoltage, voltage);
} }
static void printAnalogChannelInfo(const char *name, int hwChannel) { static void printAnalogChannelInfo(const char *name, adc_channel_e hwChannel) {
printAnalogChannelInfoExt(name, hwChannel, getVoltage(hwChannel)); printAnalogChannelInfoExt(name, hwChannel, getVoltage(hwChannel));
} }
@ -227,8 +230,7 @@ static void printAnalogInfo(void) {
printAnalogChannelInfo("AFR", engineConfiguration->afrSensor.afrAdcChannel); printAnalogChannelInfo("AFR", engineConfiguration->afrSensor.afrAdcChannel);
printAnalogChannelInfo("MAP", engineConfiguration->map.sensor.hwChannel); printAnalogChannelInfo("MAP", engineConfiguration->map.sensor.hwChannel);
printAnalogChannelInfo("BARO", engineConfiguration->baroSensor.hwChannel); printAnalogChannelInfo("BARO", engineConfiguration->baroSensor.hwChannel);
printAnalogChannelInfoExt("Vbatt", engineConfiguration->vBattAdcChannel, printAnalogChannelInfoExt("Vbatt", engineConfiguration->vBattAdcChannel, getVBatt());
getVBatt());
} }
static THD_WORKING_AREA(csThreadStack, UTILITY_THREAD_STACK_SIZE); // declare thread stack static THD_WORKING_AREA(csThreadStack, UTILITY_THREAD_STACK_SIZE); // declare thread stack
@ -269,8 +271,7 @@ void initEngineContoller(void) {
// multiple issues with this initMapAdjusterThread(); // multiple issues with this initMapAdjusterThread();
initPeriodicEvents(); initPeriodicEvents();
chThdCreateStatic(csThreadStack, sizeof(csThreadStack), LOWPRIO, chThdCreateStatic(csThreadStack, sizeof(csThreadStack), LOWPRIO, (tfunc_t) csThread, NULL);
(tfunc_t) csThread, NULL);
initInjectorCentral(); initInjectorCentral();
initPwmTester(); initPwmTester();
@ -306,6 +307,5 @@ void initEngineContoller(void) {
initFuelPump(); initFuelPump();
#endif #endif
addConsoleAction("analoginfo", printAnalogInfo); addConsoleAction("analoginfo", printAnalogInfo);
} }

View File

@ -13,7 +13,7 @@
#include "signal_executor.h" #include "signal_executor.h"
#include "engine_configuration.h" #include "engine_configuration.h"
char * getPinNameByAdcChannel(int hwChannel, char *buffer); char * getPinNameByAdcChannel(adc_channel_e hwChannel, char *buffer);
void initEngineContoller(void); void initEngineContoller(void);
#endif /* ENGINE_STATUS_H_ */ #endif /* ENGINE_STATUS_H_ */

View File

@ -8,6 +8,7 @@
#include "main.h" #include "main.h"
#include "error_handling.h" #include "error_handling.h"
#include "io_pins.h" #include "io_pins.h"
#include "memstreams.h"
#if EFI_HD44780_LCD #if EFI_HD44780_LCD
#include "lcd_HD44780.h" #include "lcd_HD44780.h"
@ -16,6 +17,8 @@
static time_t timeOfPreviousWarning = -10; static time_t timeOfPreviousWarning = -10;
static Logging logger; static Logging logger;
#define WARNING_PREFIX "WARNING: "
extern int warningEnabled; extern int warningEnabled;
extern int main_loop_started; extern int main_loop_started;
@ -45,6 +48,10 @@ void chDbgPanic3(const char *msg, const char * file, int line) {
} }
} }
#define WARNING_BUFFER_SIZE 80
static char warningBuffer[WARNING_BUFFER_SIZE];
static MemoryStream warningStream;
/** /**
* @returns TRUE in case there are too many warnings * @returns TRUE in case there are too many warnings
*/ */
@ -56,16 +63,27 @@ int warning(obd_code_e code, const char *fmt, ...) {
resetLogging(&logger); // todo: is 'reset' really needed here? resetLogging(&logger); // todo: is 'reset' really needed here?
appendMsgPrefix(&logger); appendMsgPrefix(&logger);
va_list ap; va_list ap;
va_start(ap, fmt); va_start(ap, fmt);
vappendPrintf(&logger, fmt, ap); append(&logger, WARNING_PREFIX);
warningStream.eos = 0; // reset
chvprintf((BaseSequentialStream *) &warningStream, fmt, ap);
warningStream.buffer[warningStream.eos] = 0;
va_end(ap); va_end(ap);
append(&logger, warningBuffer);
append(&logger, DELIMETER); append(&logger, DELIMETER);
scheduleLogging(&logger); scheduleLogging(&logger);
return FALSE; return FALSE;
} }
char *getWarninig(void) {
return warningBuffer;
}
void initErrorHandling(void) { void initErrorHandling(void) {
initLogging(&logger, "error handling"); initLogging(&logger, "error handling");
msObjectInit(&warningStream, (uint8_t *)warningBuffer, WARNING_BUFFER_SIZE, 0);
} }

View File

@ -19,20 +19,12 @@
#include "datalogging.h" #include "datalogging.h"
#include "audi_aan.h"
#include "dodge_neon.h"
#include "ford_aspire.h"
#include "ford_fiesta.h"
#include "ford_1995_inline_6.h"
#include "snow_blower.h"
#include "nissan_primera.h"
#include "honda_accord.h"
#include "GY6_139QMB.h"
#include "ec2.h" #include "ec2.h"
static engine_type_e defaultEngineType = FORD_ASPIRE_1996; static engine_type_e defaultEngineType = FORD_ASPIRE_1996;
static bool needToWriteConfiguration = false;
static Logging logger; static Logging logger;
extern persistent_config_container_s persistentState; extern persistent_config_container_s persistentState;
@ -50,6 +42,24 @@ crc_t flashStateCrc(persistent_config_container_s *state) {
return calc_crc((const crc_t*) &state->persistentConfiguration, sizeof(persistent_config_s)); return calc_crc((const crc_t*) &state->persistentConfiguration, sizeof(persistent_config_s));
} }
void setNeedToWriteConfiguration(void) {
scheduleMsg(&logger, "Scheduling configuration write");
needToWriteConfiguration = true;
}
bool getNeedToWriteConfiguration(void) {
return needToWriteConfiguration;
}
void writeToFlashIfPending() {
if(!getNeedToWriteConfiguration())
return;
// todo: technically we need a lock here, realistically we should be fine.
needToWriteConfiguration = false;
scheduleMsg(&logger, "Writing pending configuration");
writeToFlash();
}
void writeToFlash(void) { void writeToFlash(void) {
#if EFI_INTERNAL_FLASH #if EFI_INTERNAL_FLASH
persistentState.size = PERSISTENT_SIZE; persistentState.size = PERSISTENT_SIZE;
@ -89,7 +99,7 @@ void readFromFlash(void) {
flashRead(FLASH_ADDR, (char *) &persistentState, PERSISTENT_SIZE); flashRead(FLASH_ADDR, (char *) &persistentState, PERSISTENT_SIZE);
setDefaultNonPersistentConfiguration(engineConfiguration2); //setDefaultNonPersistentConfiguration(engineConfiguration2);
if (!isValidCrc(&persistentState) || persistentState.size != PERSISTENT_SIZE) { if (!isValidCrc(&persistentState) || persistentState.size != PERSISTENT_SIZE) {
printMsg(&logger, "Need to reset flash to default"); printMsg(&logger, "Need to reset flash to default");
@ -104,7 +114,6 @@ void readFromFlash(void) {
} }
void initFlash(void) { void initFlash(void) {
print("initFlash()\r\n");
initLogging(&logger, "Flash memory"); initLogging(&logger, "Flash memory");
addConsoleAction("readconfig", readFromFlash); addConsoleAction("readconfig", readFromFlash);

View File

@ -11,7 +11,7 @@
#include "engine_configuration.h" #include "engine_configuration.h"
#define FLASH_DATA_VERSION 3975 #define FLASH_DATA_VERSION 4320
#ifdef __cplusplus #ifdef __cplusplus
extern "C" extern "C"
@ -20,7 +20,19 @@ extern "C"
void readFromFlash(void); void readFromFlash(void);
void initFlash(void); void initFlash(void);
/**
* Because of hardware-related issues, stm32f4 chip is totally
* frozen while we are writing to internal flash. Writing the configuration takes
* about 1-2 seconds, we cannot afford to do that while the engine is
* running so we postpone the write until the engine is stopped.
*/
void writeToFlash(void); void writeToFlash(void);
void setNeedToWriteConfiguration(void);
/**
* @return true if an flash write is pending
*/
bool getNeedToWriteConfiguration(void);
void writeToFlashIfPending(void);
#ifdef __cplusplus #ifdef __cplusplus
} }

View File

@ -27,18 +27,15 @@
#include "idle_controller.h" #include "idle_controller.h"
#include "rpm_calculator.h" #include "rpm_calculator.h"
#include "pwm_generator.h" #include "pwm_generator.h"
#include "wave_math.h"
#include "idle_thread.h" #include "idle_thread.h"
#include "pin_repository.h" #include "pin_repository.h"
#include "engine_configuration.h" #include "engine_configuration.h"
#include "engine.h" #include "engine.h"
#define IDLE_AIR_CONTROL_VALVE_PWM_FREQUENCY 200
static THD_WORKING_AREA(ivThreadStack, UTILITY_THREAD_STACK_SIZE); static THD_WORKING_AREA(ivThreadStack, UTILITY_THREAD_STACK_SIZE);
static volatile int isIdleControlActive = EFI_IDLE_CONTROL;
extern board_configuration_s *boardConfiguration; extern board_configuration_s *boardConfiguration;
extern engine_configuration_s *engineConfiguration;
/** /**
* here we keep the value we got from IDLE SWITCH input * here we keep the value we got from IDLE SWITCH input
@ -48,7 +45,7 @@ static volatile int idleSwitchState;
static Logging logger; static Logging logger;
extern Engine engine; extern Engine engine;
static SimplePwm idleValve; static SimplePwm idleValvePwm;
/** /**
* Idle level calculation algorithm lives in idle_controller.c * Idle level calculation algorithm lives in idle_controller.c
@ -65,20 +62,21 @@ void idleDebug(char *msg, int value) {
} }
static void setIdleControlEnabled(int value) { static void setIdleControlEnabled(int value) {
isIdleControlActive = value; engineConfiguration->idleMode = value ? IM_MANUAL : IM_AUTO;
scheduleMsg(&logger, "isIdleControlActive=%d", isIdleControlActive); scheduleMsg(&logger, "isIdleControlActive=%d", engineConfiguration->idleMode);
} }
static void setIdleValvePwm(int value) { static void setIdleValvePwm(int value) {
// todo: change parameter type, maybe change parameter validation? // todo: change parameter type, maybe change parameter validation?
if (value < 1 || value > 999) if (value < 1 || value > 999)
return; return;
scheduleMsg(&logger, "setting idle valve PWM %d", value); scheduleMsg(&logger, "setting idle valve PWM %d @%d on %s", value, boardConfiguration->idleSolenoidFrequency,
hwPortname(boardConfiguration->idleValvePin));
/** /**
* currently idle level is an integer per mil (0-1000 range), and PWM takes a float in the 0..1 range * currently idle level is an integer per mil (0-1000 range), and PWM takes a float in the 0..1 range
* todo: unify? * todo: unify?
*/ */
idleValve.setSimplePwmDutyCycle(0.001 * value); idleValvePwm.setSimplePwmDutyCycle(0.001 * value);
} }
static msg_t ivThread(int param) { static msg_t ivThread(int param) {
@ -91,7 +89,7 @@ static msg_t ivThread(int param) {
// this value is not used yet // this value is not used yet
idleSwitchState = palReadPad(getHwPort(boardConfiguration->idleSwitchPin), getHwPin(boardConfiguration->idleSwitchPin)); idleSwitchState = palReadPad(getHwPort(boardConfiguration->idleSwitchPin), getHwPin(boardConfiguration->idleSwitchPin));
if (!isIdleControlActive) if (engineConfiguration->idleMode != IM_AUTO)
continue; continue;
int nowSec = getTimeNowSeconds(); int nowSec = getTimeNowSeconds();
@ -117,10 +115,13 @@ static void setIdleRpmAction(int value) {
void startIdleThread() { void startIdleThread() {
initLogging(&logger, "Idle Valve Control"); initLogging(&logger, "Idle Valve Control");
startSimplePwmExt(&idleValve, "Idle Valve", /**
* Start PWM for IDLE_VALVE logical / idleValvePin physical
*/
startSimplePwmExt(&idleValvePwm, "Idle Valve",
boardConfiguration->idleValvePin, boardConfiguration->idleValvePin,
IDLE_VALVE, IDLE_VALVE,
IDLE_AIR_CONTROL_VALVE_PWM_FREQUENCY, boardConfiguration->idleSolenoidFrequency,
0.5); 0.5);
idleInit(&idle); idleInit(&idle);

View File

@ -32,7 +32,6 @@
static Logging logger; static Logging logger;
extern engine_configuration_s *engineConfiguration; extern engine_configuration_s *engineConfiguration;
extern engine_configuration2_s *engineConfiguration2;
extern board_configuration_s *boardConfiguration; extern board_configuration_s *boardConfiguration;
static int is_injector_enabled[MAX_INJECTOR_COUNT]; static int is_injector_enabled[MAX_INJECTOR_COUNT];
@ -76,46 +75,83 @@ static void setInjectorEnabled(int id, int value) {
printStatus(); printStatus();
} }
void runBench(brain_pin_e brainPin, io_pin_e pin, float onTime, float offTime, int count) { static void runBench(brain_pin_e brainPin, io_pin_e pin, float delayMs, float onTimeMs, float offTimeMs, int count) {
scheduleMsg(&logger, "Running bench: ON_TIME=%f ms OFF_TIME=%fms Counter=%d", onTime, offTime, count); scheduleMsg(&logger, "Running bench: ON_TIME=%f ms OFF_TIME=%fms Counter=%d", onTimeMs, offTimeMs, count);
scheduleMsg(&logger, "output on %s", hwPortname(brainPin)); scheduleMsg(&logger, "output on %s", hwPortname(brainPin));
int delaySt = (int) (delayMs * CH_FREQUENCY / 1000);
if (delaySt != 0) {
chThdSleep(delaySt);
}
for (int i = 0; i < count; i++) { for (int i = 0; i < count; i++) {
setOutputPinValue(pin, TRUE); setOutputPinValue(pin, TRUE);
chThdSleep((int) (onTime * CH_FREQUENCY / 1000)); chThdSleep((int) (onTimeMs * CH_FREQUENCY / 1000));
setOutputPinValue(pin, FALSE); setOutputPinValue(pin, FALSE);
chThdSleep((int) (offTime * CH_FREQUENCY / 1000)); int offTimeSt = (int) (offTimeMs * CH_FREQUENCY / 1000);
if (offTimeSt > 0) {
chThdSleep(offTimeSt);
}
} }
scheduleMsg(&logger, "Done!"); scheduleMsg(&logger, "Done!");
} }
static volatile int needToRunBench = FALSE; static volatile int needToRunBench = FALSE;
float onTime; static float onTime;
float offTime; static float offTime;
int count; static float delayMs;
brain_pin_e brainPin; static int count;
io_pin_e pin; static brain_pin_e brainPin;
static io_pin_e pin;
static void fuelbench(char * onStr, char *offStr, char *countStr) { static void pinbench(const char *delayStr, const char *onTimeStr, const char *offTimeStr, const char *countStr,
onTime = atoff(onStr); io_pin_e pinParam, brain_pin_e brainPinParam) {
offTime = atoff(offStr); delayMs = atoff(delayStr);
onTime = atoff(onTimeStr);
offTime = atoff(offTimeStr);
count = atoi(countStr); count = atoi(countStr);
brainPin = boardConfiguration->injectionPins[0]; brainPin = brainPinParam;
pin = INJECTOR_1_OUTPUT; pin = pinParam;
needToRunBench = TRUE; needToRunBench = TRUE;
} }
static void sparkbench(char * onStr, char *offStr, char *countStr) { static void fuelbench2(const char *delayStr, const char *indexStr, const char * onTimeStr, const char *offTimeStr,
onTime = atoff(onStr); const char *countStr) {
offTime = atoff(offStr); int index = atoi(indexStr);
count = atoi(countStr); brain_pin_e b = boardConfiguration->injectionPins[index - 1];
io_pin_e p = (io_pin_e) ((int) INJECTOR_1_OUTPUT - 1 + index);
pinbench(delayStr, onTimeStr, offTimeStr, countStr, p, b);
}
static void fuelpumpbench(int delayParam, int onTimeParam) {
brainPin = boardConfiguration->fuelPumpPin;
pin = FUEL_PUMP_RELAY;
delayMs = delayParam;
onTime = onTimeParam;
offTime = 0;
count = 1;
brainPin = boardConfiguration->ignitionPins[0];
pin = SPARKOUT_1_OUTPUT;
needToRunBench = TRUE; needToRunBench = TRUE;
} }
static void fuelbench(const char * onTimeStr, const char *offTimeStr, const char *countStr) {
fuelbench2("0", "1", onTimeStr, offTimeStr, countStr);
}
static void sparkbench2(const char *delayStr, const char *indexStr, const char * onTimeStr, const char *offTimeStr,
const char *countStr) {
int index = atoi(indexStr);
brain_pin_e b = boardConfiguration->ignitionPins[index - 1];
io_pin_e p = (io_pin_e) ((int) SPARKOUT_1_OUTPUT - 1 + index);
pinbench(delayStr, onTimeStr, offTimeStr, countStr, p, b);
}
static void sparkbench(const char * onTimeStr, const char *offTimeStr, const char *countStr) {
sparkbench2("0", "1", onTimeStr, offTimeStr, countStr);
}
static THD_WORKING_AREA(benchThreadStack, UTILITY_THREAD_STACK_SIZE); static THD_WORKING_AREA(benchThreadStack, UTILITY_THREAD_STACK_SIZE);
static msg_t benchThread(int param) { static msg_t benchThread(int param) {
@ -126,7 +162,7 @@ static msg_t benchThread(int param) {
chThdSleepMilliseconds(200); chThdSleepMilliseconds(200);
} }
needToRunBench = FALSE; needToRunBench = FALSE;
runBench(brainPin, pin, onTime, offTime, count); runBench(brainPin, pin, delayMs, onTime, offTime, count);
} }
#if defined __GNUC__ #if defined __GNUC__
return 0; return 0;
@ -144,13 +180,18 @@ void initInjectorCentral(void) {
for (int i = 0; i < engineConfiguration->cylindersCount; i++) { for (int i = 0; i < engineConfiguration->cylindersCount; i++) {
io_pin_e pin = (io_pin_e) ((int) INJECTOR_1_OUTPUT + i); io_pin_e pin = (io_pin_e) ((int) INJECTOR_1_OUTPUT + i);
outputPinRegisterExt2(getPinName(pin), pin, outputPinRegisterExt2(getPinName(pin), pin, boardConfiguration->injectionPins[i],
boardConfiguration->injectionPins[i], &boardConfiguration->injectionPinMode); &boardConfiguration->injectionPinMode);
} }
printStatus(); printStatus();
addConsoleActionII("injector", setInjectorEnabled); addConsoleActionII("injector", setInjectorEnabled);
addConsoleActionII("fuelpumpbench", &fuelpumpbench);
addConsoleActionSSS("fuelbench", &fuelbench); addConsoleActionSSS("fuelbench", &fuelbench);
addConsoleActionSSS("sparkbench", &sparkbench); addConsoleActionSSS("sparkbench", &sparkbench);
addConsoleActionSSSSS("fuelbench2", &fuelbench2);
addConsoleActionSSSSS("sparkbench2", &sparkbench2);
} }

View File

@ -0,0 +1,193 @@
/**
* @file lcd_controller.cpp
*
* @date Aug 14, 2014
* @author Andrey Belomutskiy, (c) 2012-2014
*/
#include "main.h"
#include "lcd_controller.h"
#include "lcd_HD44780.h"
#include "efilib.h"
#include "rpm_calculator.h"
#include "allsensors.h"
#include "engine.h"
#include "rtc_helper.h"
#include "io_pins.h"
extern Engine engine;
extern engine_configuration_s *engineConfiguration;
#define LCD_WIDTH 20
// this value should be even
#define NUMBER_OF_DIFFERENT_LINES 4
char * appendStr(char *ptr, const char *suffix) {
for (uint32_t i = 0; i < strlen(suffix); i++) {
*ptr++ = suffix[i];
}
return ptr;
}
static char * prepareVBattMapLine(char *buffer) {
char *ptr = buffer;
*ptr++ = 'V';
ptr = ftoa(ptr, getVBatt(), 10.0f);
ptr = appendStr(ptr, " M");
ptr = ftoa(ptr, getRawMap(), 10.0f);
return ptr;
}
static char * prepareCltIatTpsLine(char *buffer) {
char *ptr = buffer;
*ptr++ = 'C';
ptr = ftoa(ptr, getCoolantTemperature(), 10.0f);
ptr = appendStr(ptr, " C");
ptr = ftoa(ptr, getIntakeAirTemperature(), 10.0f);
ptr = appendStr(ptr, " TP");
ptr = itoa10(ptr, (int) getTPS());
return ptr;
}
static const char* algorithmStr[] = { "MAF", "TPS", "MAP", "SD" };
static const char* ignitionModeStr[] = { "1C", "IND", "WS" };
static const char* injectionModeStr[] = { "Sim", "Seq", "Bch" };
static const char* idleModeStr[] = { "I:A", "I:M" };
static const char *getPinShortName(io_pin_e pin) {
switch (pin) {
case ALTERNATOR_SWITCH:
return "AL";
case FUEL_PUMP_RELAY:
return "FP";
case FAN_RELAY:
return "FN";
case O2_HEATER:
return "O2H";
default:
firmwareError("No short name for %d", (int) pin);
return "";
}
}
char * appendPinStatus(char *buffer, io_pin_e pin) {
char *ptr = appendStr(buffer, getPinShortName(pin));
int state = getOutputPinValue(pin);
// todo: should we handle INITIAL_PIN_STATE?
if (state) {
return appendStr(ptr, ":Y ");
} else {
return appendStr(ptr, ":n ");
}
}
static char * prepareInfoLine(char *buffer) {
char *ptr = buffer;
ptr = appendStr(ptr, algorithmStr[engineConfiguration->algorithm]);
ptr = appendStr(ptr, " ");
ptr = appendStr(ptr, ignitionModeStr[engineConfiguration->ignitionMode]);
ptr = appendStr(ptr, " ");
ptr = appendStr(ptr, injectionModeStr[engineConfiguration->injectionMode]);
ptr = appendStr(ptr, " ");
ptr = appendStr(ptr, idleModeStr[engineConfiguration->idleMode]);
ptr = appendStr(ptr, " ");
return ptr;
}
static char * prepareStatusLine(char *buffer) {
char *ptr = buffer;
ptr = appendPinStatus(ptr, FUEL_PUMP_RELAY);
ptr = appendPinStatus(ptr, FAN_RELAY);
ptr = appendPinStatus(ptr, O2_HEATER);
return ptr;
}
static char buffer[LCD_WIDTH + 4];
static char dateBuffer[30];
static void prepareCurrentSecondLine(int index) {
memset(buffer, ' ', LCD_WIDTH);
char *ptr;
switch (index) {
case 0:
ptr = prepareCltIatTpsLine(buffer);
break;
case 1:
ptr = prepareInfoLine(buffer);
break;
case 2:
ptr = prepareVBattMapLine(buffer);
break;
case 3:
ptr = prepareStatusLine(buffer);
break;
}
*ptr = ' ';
}
void updateHD44780lcd(void) {
lcd_HD44780_set_position(0, 9);
/**
* this would blink so that we know the LCD is alive
*/
if (getTimeNowSeconds() % 2 == 0) {
lcd_HD44780_print_char('R');
} else {
lcd_HD44780_print_char(' ');
}
lcd_HD44780_set_position(0, 10);
char * ptr = itoa10(buffer, getRpm());
ptr[0] = 0;
int len = ptr - buffer;
for (int i = 0; i < 6 - len; i++) {
lcd_HD44780_print_char(' ');
}
lcd_HD44780_print_string(buffer);
if (hasFirmwareError()) {
memcpy(buffer, getFirmwareError(), LCD_WIDTH);
buffer[LCD_WIDTH] = 0;
lcd_HD44780_set_position(1, 0);
lcd_HD44780_print_string(buffer);
return;
}
lcd_HD44780_set_position(1, 0);
memset(buffer, ' ', LCD_WIDTH);
memcpy(buffer, getWarninig(), LCD_WIDTH);
buffer[LCD_WIDTH] = 0;
lcd_HD44780_print_string(buffer);
if (engineConfiguration->HD44780height < 3) {
return;
}
int index = (getTimeNowSeconds() / 2) % (NUMBER_OF_DIFFERENT_LINES / 2);
prepareCurrentSecondLine(index);
buffer[LCD_WIDTH] = 0;
lcd_HD44780_set_position(2, 0);
lcd_HD44780_print_string(buffer);
prepareCurrentSecondLine(index + NUMBER_OF_DIFFERENT_LINES / 2);
buffer[LCD_WIDTH] = 0;
lcd_HD44780_set_position(3, 0);
lcd_HD44780_print_string(buffer);
#if EFI_PROD_CODE
dateToString(dateBuffer);
lcd_HD44780_set_position(1, 0);
lcd_HD44780_print_string(dateBuffer);
#endif /* EFI_PROD_CODE */
}

View File

@ -0,0 +1,13 @@
/**
* @file lcd_controller.h
*
* @date Aug 14, 2014
* @author Andrey Belomutskiy, (c) 2012-2014
*/
#ifndef LCD_CONTROLLER_H_
#define LCD_CONTROLLER_H_
void updateHD44780lcd(void);
#endif /* LCD_CONTROLLER_H_ */

View File

@ -22,7 +22,7 @@
#include "map.h" #include "map.h"
#if EFI_MAP_AVERAGING #if EFI_MAP_AVERAGING || defined(__DOXYGEN__)
#include "map_averaging.h" #include "map_averaging.h"
#include "trigger_central.h" #include "trigger_central.h"

View File

@ -13,7 +13,6 @@
#include "map_adjuster.h" #include "map_adjuster.h"
#include "rpm_calculator.h" #include "rpm_calculator.h"
#include "main_trigger_callback.h" #include "main_trigger_callback.h"
#include "wave_math.h"
#include "allsensors.h" #include "allsensors.h"
#include "engine_math.h" #include "engine_math.h"
#include "engine.h" #include "engine.h"

View File

@ -188,6 +188,21 @@ static void registerInjectionEvent(engine_configuration_s const *e,
} }
float getFuelMultiplier(engine_configuration_s const *e, injection_mode_e mode) {
switch(mode) {
case IM_SEQUENTIAL:
return 1;
case IM_SIMULTANEOUS:
// todo: pre-calculate and save into ec2?
return 1.0 / e->cylindersCount;
case IM_BATCH:
return 2.0 / e->cylindersCount;
default:
firmwareError("Unexpected getFuelMultiplier %d", mode);
return NAN;
}
}
void addFuelEvents(engine_configuration_s const *e, engine_configuration2_s *engineConfiguration2, void addFuelEvents(engine_configuration_s const *e, engine_configuration2_s *engineConfiguration2,
ActuatorEventList *list, injection_mode_e mode) { ActuatorEventList *list, injection_mode_e mode) {
list->resetEventList(); list->resetEventList();
@ -216,9 +231,16 @@ void addFuelEvents(engine_configuration_s const *e, engine_configuration2_s *eng
break; break;
case IM_BATCH: case IM_BATCH:
for (int i = 0; i < e->cylindersCount; i++) { for (int i = 0; i < e->cylindersCount; i++) {
io_pin_e pin = (io_pin_e) ((int) INJECTOR_1_OUTPUT + (i % 2)); int index = i % (e->cylindersCount / 2);
io_pin_e pin = (io_pin_e) ((int) INJECTOR_1_OUTPUT + index);
float angle = baseAngle + i * 720.0 / e->cylindersCount; float angle = baseAngle + i * 720.0 / e->cylindersCount;
registerInjectionEvent(e, s, list, pin, angle); registerInjectionEvent(e, s, list, pin, angle);
/**
* also fire the 2nd half of the injectors so that we can implement a batch mode on individual wires
*/
pin = (io_pin_e) ((int) INJECTOR_1_OUTPUT + index + (e->cylindersCount / 2));
registerInjectionEvent(e, s, list, pin, angle);
} }
break; break;
default: default:
@ -270,8 +292,9 @@ void findTriggerPosition(engine_configuration_s const *engineConfiguration, trig
while (true) { while (true) {
middle = (left + right) / 2; middle = (left + right) / 2;
if (middle == left) if (middle == left) {
break; break;
}
if (angleOffset < s->eventAngles[middle]) { if (angleOffset < s->eventAngles[middle]) {
right = middle; right = middle;
@ -299,8 +322,10 @@ void registerActuatorEventExt(engine_configuration_s const *engineConfiguration,
OutputSignal *actuator, float angleOffset) { OutputSignal *actuator, float angleOffset) {
efiAssertVoid(s->getSize() > 0, "uninitialized trigger_shape_s"); efiAssertVoid(s->getSize() > 0, "uninitialized trigger_shape_s");
if (e == NULL) if (e == NULL) {
return; // error already reported // error already reported
return;
}
e->actuator = actuator; e->actuator = actuator;
findTriggerPosition(engineConfiguration, s, &e->position, angleOffset); findTriggerPosition(engineConfiguration, s, &e->position, angleOffset);
@ -337,8 +362,7 @@ int getCylinderId(firing_order_e firingOrder, int index) {
void prepareOutputSignals(engine_configuration_s *engineConfiguration, engine_configuration2_s *engineConfiguration2) { void prepareOutputSignals(engine_configuration_s *engineConfiguration, engine_configuration2_s *engineConfiguration2) {
// todo: move this reset into decoder // todo: move this reset into decoder
engineConfiguration2->triggerShape.setTriggerShapeSynchPointIndex(findTriggerZeroEventIndex( engineConfiguration2->triggerShape.calculateTriggerSynchPoint(&engineConfiguration->triggerConfig);
&engineConfiguration2->triggerShape, &engineConfiguration->triggerConfig));
injectonSignals.clear(); injectonSignals.clear();
EventHandlerConfiguration *config = &engineConfiguration2->engineEventConfiguration; EventHandlerConfiguration *config = &engineConfiguration2->engineEventConfiguration;
@ -364,6 +388,7 @@ void setTimingLoadBin(engine_configuration_s *engineConfiguration, float l, floa
setTableBin(engineConfiguration->ignitionLoadBins, IGN_LOAD_COUNT, l, r); setTableBin(engineConfiguration->ignitionLoadBins, IGN_LOAD_COUNT, l, r);
} }
int isInjectionEnabled(engine_configuration2_s const *engineConfiguration2) { int isInjectionEnabled(engine_configuration_s *engineConfiguration) {
return engineConfiguration2->isInjectionEnabledFlag; // todo: is this worth a method? should this be inlined?
return engineConfiguration->isInjectionEnabled;
} }

View File

@ -19,7 +19,7 @@
void findTriggerPosition(engine_configuration_s const *engineConfiguration, trigger_shape_s * s, void findTriggerPosition(engine_configuration_s const *engineConfiguration, trigger_shape_s * s,
event_trigger_position_s *position, float angleOffset); event_trigger_position_s *position, float angleOffset);
int isInjectionEnabled(engine_configuration2_s const *engineConfiguration2); int isInjectionEnabled(engine_configuration_s *engineConfiguration);
float fixAngle(float angle); float fixAngle(float angle);

View File

@ -26,7 +26,7 @@ static Map3D1616 afrMap;
#define tpMin 0 #define tpMin 0
#define tpMax 100 #define tpMax 100
// http://rusefi.com/math/t_charge.html // http://rusefi.com/math/t_charge.html
float getTCharge(int rpm, int tps, float coolantTemp, float airTemp) { float getTCharge(int rpm, float tps, float coolantTemp, float airTemp) {
float minRpmKcurrentTPS = interpolate(tpMin, K_AT_MIN_RPM_MIN_TPS, tpMax, float minRpmKcurrentTPS = interpolate(tpMin, K_AT_MIN_RPM_MIN_TPS, tpMax,
K_AT_MIN_RPM_MAX_TPS, tps); K_AT_MIN_RPM_MAX_TPS, tps);
float maxRpmKcurrentTPS = interpolate(tpMin, K_AT_MAX_RPM_MIN_TPS, tpMax, float maxRpmKcurrentTPS = interpolate(tpMin, K_AT_MAX_RPM_MIN_TPS, tpMax,

View File

@ -11,7 +11,7 @@
#include "ec2.h" #include "ec2.h"
#include "engine.h" #include "engine.h"
float getTCharge(int rpm, int tps, float coolantTemp, float airTemp); float getTCharge(int rpm, float tps, float coolantTemp, float airTemp);
void setDetaultVETable(engine_configuration_s *engineConfiguration); void setDetaultVETable(engine_configuration_s *engineConfiguration);
float sdMath(engine_configuration_s *engineConfiguration, float VE, float MAP, float AFR, float temp); float sdMath(engine_configuration_s *engineConfiguration, float VE, float MAP, float AFR, float temp);

View File

@ -18,8 +18,11 @@ extern engine_configuration_s * engineConfiguration;
*/ */
static FastInterpolation denso183(0, -6.64, 5, 182.78); static FastInterpolation denso183(0, -6.64, 5, 182.78);
// todo: figure out real values /**
static FastInterpolation honda3bar(0.32, -95.8371264, 4.84, 300); * MAP sensor output voltage of 3.0v = a gauge reading of 0 in. Hg
* MAP sensor output voltage of 0.5v = a gauge reading of 27 in. Hg
*/
static FastInterpolation honda3bar(0.5, 91.422, 3.0, 0);
static FastInterpolation mpx4250(0, 8, 5, 260); static FastInterpolation mpx4250(0, 8, 5, 260);

View File

@ -20,7 +20,7 @@ typedef struct {
float Min; float Min;
float Max; float Max;
air_pressure_sensor_type_e sensorType; air_pressure_sensor_type_e sensorType;
int hwChannel; adc_channel_e hwChannel;
} air_pressure_sensor_config_s; } air_pressure_sensor_config_s;
/** /**
@ -65,7 +65,7 @@ typedef struct {
typedef struct { typedef struct {
ThermistorConf *config; ThermistorConf *config;
int channel; adc_channel_e channel;
} Thermistor; } Thermistor;
#endif /* THERMISTOR_TYPES_H_ */ #endif /* THERMISTOR_TYPES_H_ */

View File

@ -1,5 +1,5 @@
/** /**
* @file thermistors.c * @file thermistors.cpp
* *
* @date Feb 17, 2013 * @date Feb 17, 2013
* @author Andrey Belomutskiy, (c) 2012-2014 * @author Andrey Belomutskiy, (c) 2012-2014
@ -18,8 +18,8 @@
#include "ec2.h" #include "ec2.h"
// Celsius // Celsius
#define LIMPING_MODE_IAT_TEMPERATURE 30 #define LIMPING_MODE_IAT_TEMPERATURE 30.0f
#define LIMPING_MODE_CLT_TEMPERATURE 70 #define LIMPING_MODE_CLT_TEMPERATURE 70.0f
extern engine_configuration_s *engineConfiguration; extern engine_configuration_s *engineConfiguration;
extern engine_configuration2_s *engineConfiguration2; extern engine_configuration2_s *engineConfiguration2;
@ -34,8 +34,9 @@ float getR1InVoltageDividor(float Vout, float Vin, float r2) {
} }
float getR2InVoltageDividor(float Vout, float Vin, float r1) { float getR2InVoltageDividor(float Vout, float Vin, float r1) {
if (Vout == 0) if (Vout == 0) {
return NAN; return NAN;
}
return r1 / (Vin / Vout - 1); return r1 / (Vin / Vout - 1);
} }
@ -46,7 +47,7 @@ float getVoutInVoltageDividor(float Vin, float r1, float r2) {
float convertResistanceToKelvinTemperature(float resistance, ThermistorConf *thermistor) { float convertResistanceToKelvinTemperature(float resistance, ThermistorConf *thermistor) {
if (resistance <= 0) { if (resistance <= 0) {
//warning("Invalid resistance in convertResistanceToKelvinTemperature=", resistance); //warning("Invalid resistance in convertResistanceToKelvinTemperature=", resistance);
return 0; return 0.0f;
} }
float logR = logf(resistance); float logR = logf(resistance);
return 1 / (thermistor->s_h_a + thermistor->s_h_b * logR + thermistor->s_h_c * logR * logR * logR); return 1 / (thermistor->s_h_a + thermistor->s_h_b * logR + thermistor->s_h_c * logR * logR * logR);
@ -93,12 +94,12 @@ float getTemperatureC(Thermistor *thermistor) {
return convertKelvinToCelcius(kelvinTemperature); return convertKelvinToCelcius(kelvinTemperature);
} }
int isValidCoolantTemperature(float temperature) { bool isValidCoolantTemperature(float temperature) {
// I hope magic constants are appropriate here // I hope magic constants are appropriate here
return !cisnan(temperature) && temperature > -50 && temperature < 250; return !cisnan(temperature) && temperature > -50 && temperature < 250;
} }
int isValidIntakeAirTemperature(float temperature) { bool isValidIntakeAirTemperature(float temperature) {
// I hope magic constants are appropriate here // I hope magic constants are appropriate here
return !cisnan(temperature) && temperature > -50 && temperature < 100; return !cisnan(temperature) && temperature > -50 && temperature < 100;
} }
@ -109,7 +110,7 @@ int isValidIntakeAirTemperature(float temperature) {
float getCoolantTemperature(void) { float getCoolantTemperature(void) {
float temperature = getTemperatureC(&engineConfiguration2->clt); float temperature = getTemperatureC(&engineConfiguration2->clt);
if (!isValidCoolantTemperature(temperature)) { if (!isValidCoolantTemperature(temperature)) {
warning(OBD_PCM_Processor_Fault, "unrealistic coolant temperature %f", temperature); warning(OBD_PCM_Processor_Fault, "unrealistic CLT %f", temperature);
return LIMPING_MODE_CLT_TEMPERATURE; return LIMPING_MODE_CLT_TEMPERATURE;
} }
return temperature; return temperature;
@ -154,13 +155,13 @@ void prepareThermistorCurve(ThermistorConf * config) {
float getIntakeAirTemperature(void) { float getIntakeAirTemperature(void) {
float temperature = getTemperatureC(&engineConfiguration2->iat); float temperature = getTemperatureC(&engineConfiguration2->iat);
if (!isValidIntakeAirTemperature(temperature)) { if (!isValidIntakeAirTemperature(temperature)) {
warning(OBD_PCM_Processor_Fault, "unrealistic intake temperature %f", temperature); warning(OBD_PCM_Processor_Fault, "unrealistic IAT %f", temperature);
return LIMPING_MODE_IAT_TEMPERATURE; return LIMPING_MODE_IAT_TEMPERATURE;
} }
return temperature; return temperature;
} }
static void initThermistorCurve(Thermistor * t, ThermistorConf *config, int channel) { static void initThermistorCurve(Thermistor * t, ThermistorConf *config, adc_channel_e channel) {
prepareThermistorCurve(config); prepareThermistorCurve(config);
t->config = config; t->config = config;
t->channel = channel; t->channel = channel;

View File

@ -38,9 +38,9 @@ float getKelvinTemperature(float resistance, ThermistorConf *thermistor);
float getResistance(Thermistor *thermistor); float getResistance(Thermistor *thermistor);
float getTemperatureC(Thermistor *thermistor); float getTemperatureC(Thermistor *thermistor);
float getCoolantTemperature(void); float getCoolantTemperature(void);
int isValidCoolantTemperature(float temperature); bool isValidCoolantTemperature(float temperature);
float getIntakeAirTemperature(void); float getIntakeAirTemperature(void);
int isValidIntakeAirTemperature(float temperature); bool isValidIntakeAirTemperature(float temperature);
float convertResistanceToKelvinTemperature(float resistance, float convertResistanceToKelvinTemperature(float resistance,
ThermistorConf *thermistor); ThermistorConf *thermistor);

View File

@ -4,7 +4,6 @@
#include "engine_configuration.h" #include "engine_configuration.h"
#include "interpolation.h" #include "interpolation.h"
#include "adc_inputs.h" #include "adc_inputs.h"
#include "wave_math.h"
extern engine_configuration_s *engineConfiguration; extern engine_configuration_s *engineConfiguration;
@ -49,14 +48,16 @@ float getTpsRateOfChange(void) {
* *
* */ * */
float getTpsValue(int adc) { float getTpsValue(int adc) {
if (adc < engineConfiguration->tpsMin) if (adc < engineConfiguration->tpsMin) {
return 0; return 0.0f;
if (adc > engineConfiguration->tpsMax) }
return 100; if (adc > engineConfiguration->tpsMax) {
return 100.0f;
}
// todo: double comparison using EPS // todo: double comparison using EPS
if (engineConfiguration->tpsMin == engineConfiguration->tpsMax) { if (engineConfiguration->tpsMin == engineConfiguration->tpsMax) {
firmwareError("Invalid TPS configuration: same value"); firmwareError("Invalid TPS configuration: same value");
return 0; return 0.0f;
} }
return interpolate(engineConfiguration->tpsMin, 0, engineConfiguration->tpsMax, 100, adc); return interpolate(engineConfiguration->tpsMin, 0, engineConfiguration->tpsMax, 100, adc);
} }

View File

@ -11,6 +11,7 @@
#include "boards.h" #include "boards.h"
#include "engine_configuration.h" #include "engine_configuration.h"
#include "adc_inputs.h" #include "adc_inputs.h"
#include "voltage.h"
extern engine_configuration_s *engineConfiguration; extern engine_configuration_s *engineConfiguration;

View File

@ -43,17 +43,21 @@ static void doPrintConfiguration(void) {
printConfiguration(engineConfiguration, engineConfiguration2); printConfiguration(engineConfiguration, engineConfiguration2);
} }
/*
static void printIntArray(int array[], int size) { static void printIntArray(int array[], int size) {
for (int j = 0; j < size; j++) for (int j = 0; j < size; j++) {
print("%d ", array[j]); print("%d ", array[j]);
}
print("\r\n"); print("\r\n");
} }
*/
void printFloatArray(const char *prefix, float array[], int size) { void printFloatArray(const char *prefix, float array[], int size) {
appendMsgPrefix(&logger); appendMsgPrefix(&logger);
appendPrintf(&logger, prefix); appendPrintf(&logger, prefix);
for (int j = 0; j < size; j++) for (int j = 0; j < size; j++) {
appendPrintf(&logger, "%f ", array[j]); appendPrintf(&logger, "%f ", array[j]);
}
appendMsgPostfix(&logger); appendMsgPostfix(&logger);
scheduleLogging(&logger); scheduleLogging(&logger);
} }
@ -76,8 +80,12 @@ const char* getConfigurationName(engine_configuration_s *engineConfiguration) {
case NISSAN_PRIMERA: case NISSAN_PRIMERA:
return "Nissan Primera"; return "Nissan Primera";
#endif /* EFI_SUPPORT_NISSAN_PRIMERA */ #endif /* EFI_SUPPORT_NISSAN_PRIMERA */
case HONDA_ACCORD: case HONDA_ACCORD_CD:
return "Honda Accord"; return "Honda Accord 3w";
case HONDA_ACCORD_CD_TWO_WIRES:
return "Honda Accord 2w";
case HONDA_ACCORD_CD_DIP:
return "Honda Dip";
case FORD_INLINE_6_1995: case FORD_INLINE_6_1995:
return "Ford 1995 inline 6"; return "Ford 1995 inline 6";
case GY6_139QMB: case GY6_139QMB:
@ -96,7 +104,10 @@ const char* getConfigurationName(engine_configuration_s *engineConfiguration) {
return "Citroen TU3JP"; return "Citroen TU3JP";
case ROVER_V8: case ROVER_V8:
return "Rover v8"; return "Rover v8";
case MITSU_4G93:
return "Mitsu 4G93";
default: default:
firmwareError("Unexpected: engineType %d", engineConfiguration->engineType);
return NULL; return NULL;
} }
} }
@ -125,7 +136,8 @@ static const char * boolToString(bool value) {
*/ */
void printConfiguration(engine_configuration_s *engineConfiguration, engine_configuration2_s *engineConfiguration2) { void printConfiguration(engine_configuration_s *engineConfiguration, engine_configuration2_s *engineConfiguration2) {
scheduleMsg(&logger, "Template %s trigger %d", getConfigurationName(engineConfiguration), engineConfiguration->triggerConfig.triggerType); scheduleMsg(&logger, "Template %s/%d trigger %d", getConfigurationName(engineConfiguration),
engineConfiguration->engineType, engineConfiguration->triggerConfig.triggerType);
scheduleMsg(&logger, "configurationVersion=%d", getGlobalConfigurationVersion()); scheduleMsg(&logger, "configurationVersion=%d", getGlobalConfigurationVersion());
@ -152,16 +164,18 @@ void printConfiguration(engine_configuration_s *engineConfiguration, engine_conf
// appendMsgPrefix(&logger); // appendMsgPrefix(&logger);
scheduleMsg(&logger, "rpmHardLimit: %d", engineConfiguration->rpmHardLimit); scheduleMsg(&logger, "rpmHardLimit: %d/rpmMultiplier=%f", engineConfiguration->rpmHardLimit,
scheduleMsg(&logger, "rpmMultiplier=%f", engineConfiguration->rpmMultiplier); engineConfiguration->rpmMultiplier);
scheduleMsg(&logger, "tpsMin: %d", engineConfiguration->tpsMin); scheduleMsg(&logger, "tpsMin: %d/tpsMax: %d", engineConfiguration->tpsMin, engineConfiguration->tpsMax);
scheduleMsg(&logger, "tpsMax: %d", engineConfiguration->tpsMax);
scheduleMsg(&logger, "ignitionMode: %d/enabled=%s", engineConfiguration->ignitionMode,
boolToString(engineConfiguration->isIgnitionEnabled));
scheduleMsg(&logger, "timingMode: %d", engineConfiguration->timingMode); scheduleMsg(&logger, "timingMode: %d", engineConfiguration->timingMode);
scheduleMsg(&logger, "fixedModeTiming: %d", (int) engineConfiguration->fixedModeTiming); scheduleMsg(&logger, "fixedModeTiming: %d", (int) engineConfiguration->fixedModeTiming);
scheduleMsg(&logger, "ignitionOffset=%f", engineConfiguration->ignitionOffset); scheduleMsg(&logger, "ignitionOffset=%f", engineConfiguration->ignitionOffset);
scheduleMsg(&logger, "injectionOffset=%f", engineConfiguration->injectionOffset); scheduleMsg(&logger, "injectionOffset=%f/enabled=%s", (double) engineConfiguration->injectionOffset,
boolToString(engineConfiguration->isInjectionEnabled));
scheduleMsg(&logger, "crankingChargeAngle=%f", engineConfiguration->crankingChargeAngle); scheduleMsg(&logger, "crankingChargeAngle=%f", engineConfiguration->crankingChargeAngle);
scheduleMsg(&logger, "crankingTimingAngle=%f", engineConfiguration->crankingTimingAngle); scheduleMsg(&logger, "crankingTimingAngle=%f", engineConfiguration->crankingTimingAngle);
@ -176,7 +190,8 @@ void printConfiguration(engine_configuration_s *engineConfiguration, engine_conf
pinModeToString(boardConfiguration->malfunctionIndicatorPinMode)); pinModeToString(boardConfiguration->malfunctionIndicatorPinMode));
scheduleMsg(&logger, "analogInputDividerCoefficient: %f", engineConfiguration->analogInputDividerCoefficient); scheduleMsg(&logger, "analogInputDividerCoefficient: %f", engineConfiguration->analogInputDividerCoefficient);
scheduleMsg(&logger, "needSecondTriggerInput: %s", boolToString(engineConfiguration2->triggerShape.needSecondTriggerInput)); scheduleMsg(&logger, "needSecondTriggerInput: %s",
boolToString(engineConfiguration2->triggerShape.needSecondTriggerInput));
#if EFI_PROD_CODE #if EFI_PROD_CODE
scheduleMsg(&logger, "idleValvePin: %s", hwPortname(boardConfiguration->idleValvePin)); scheduleMsg(&logger, "idleValvePin: %s", hwPortname(boardConfiguration->idleValvePin));
@ -201,19 +216,23 @@ void printConfiguration(engine_configuration_s *engineConfiguration, engine_conf
pinModeToString(boardConfiguration->triggerSimulatorPinModes[0])); pinModeToString(boardConfiguration->triggerSimulatorPinModes[0]));
scheduleMsg(&logger, "secondary trigger simulator: %s %s", hwPortname(boardConfiguration->triggerSimulatorPins[1]), scheduleMsg(&logger, "secondary trigger simulator: %s %s", hwPortname(boardConfiguration->triggerSimulatorPins[1]),
pinModeToString(boardConfiguration->triggerSimulatorPinModes[1])); pinModeToString(boardConfiguration->triggerSimulatorPinModes[1]));
scheduleMsg(&logger, "3rd trigger simulator: %s %s", hwPortname(boardConfiguration->triggerSimulatorPins[2]),
pinModeToString(boardConfiguration->triggerSimulatorPinModes[2]));
scheduleMsg(&logger, "primary trigger input: %s", hwPortname(boardConfiguration->primaryTriggerInputPin)); scheduleMsg(&logger, "primary trigger input: %s", hwPortname(boardConfiguration->triggerInputPins[0]));
scheduleMsg(&logger, "secondary trigger input: %s", hwPortname(boardConfiguration->triggerInputPins[1]));
scheduleMsg(&logger, "primary logic input: %s", hwPortname(boardConfiguration->logicAnalyzerPins[0]));
scheduleMsg(&logger, "secondary logic input: %s", hwPortname(boardConfiguration->logicAnalyzerPins[1]));
scheduleMsg(&logger, "boardTestModeJumperPin: %s", hwPortname(boardConfiguration->boardTestModeJumperPin)); scheduleMsg(&logger, "boardTestModeJumperPin: %s", hwPortname(boardConfiguration->boardTestModeJumperPin));
scheduleMsg(&logger, "digitalPotentiometerSpiDevice %d", boardConfiguration->digitalPotentiometerSpiDevice); scheduleMsg(&logger, "digitalPotentiometerSpiDevice %d", boardConfiguration->digitalPotentiometerSpiDevice);
for (int i = 0; i < DIGIPOT_COUNT; i++) { for (int i = 0; i < DIGIPOT_COUNT; i++) {
scheduleMsg(&logger, "digitalPotentiometer CS%d %s", i, hwPortname(boardConfiguration->digitalPotentiometerChipSelect[i])); scheduleMsg(&logger, "digitalPotentiometer CS%d %s", i,
hwPortname(boardConfiguration->digitalPotentiometerChipSelect[i]));
} }
#endif /* EFI_PROD_CODE */ #endif /* EFI_PROD_CODE */
scheduleMsg(&logger, "isInjectionEnabledFlag %s", boolToString(engineConfiguration2->isInjectionEnabledFlag));
} }
static void setFixedModeTiming(int value) { static void setFixedModeTiming(int value) {
@ -228,6 +247,10 @@ static void setTimingMode(int value) {
incrementGlobalConfigurationVersion(); incrementGlobalConfigurationVersion();
} }
static void setIdleMode(int mode) {
engineConfiguration->idleMode = (idle_mode_e) mode;
}
void setEngineType(int value) { void setEngineType(int value) {
engineConfiguration->engineType = (engine_type_e) value; engineConfiguration->engineType = (engine_type_e) value;
resetConfigurationExt(&logger, (engine_type_e) value, engineConfiguration, engineConfiguration2, resetConfigurationExt(&logger, (engine_type_e) value, engineConfiguration, engineConfiguration2,
@ -295,7 +318,7 @@ static void setRpmMultiplier(int value) {
static char pinNameBuffer[16]; static char pinNameBuffer[16];
static void printThermistor(const char *msg, Thermistor *thermistor) { static void printThermistor(const char *msg, Thermistor *thermistor) {
int adcChannel = thermistor->channel; adc_channel_e adcChannel = thermistor->channel;
float voltage = getVoltageDivided(adcChannel); float voltage = getVoltageDivided(adcChannel);
float r = getResistance(thermistor); float r = getResistance(thermistor);
@ -309,8 +332,8 @@ static void printThermistor(const char *msg, Thermistor *thermistor) {
#endif #endif
} }
static void printMAPInfo(void) {
#if EFI_PROD_CODE #if EFI_PROD_CODE
static void printMAPInfo(void) {
scheduleMsg(&logger, "map type=%d raw=%f MAP=%f", engineConfiguration->map.sensor.sensorType, getRawMap(), scheduleMsg(&logger, "map type=%d raw=%f MAP=%f", engineConfiguration->map.sensor.sensorType, getRawMap(),
getMap()); getMap());
if (engineConfiguration->map.sensor.sensorType == MT_CUSTOM) { if (engineConfiguration->map.sensor.sensorType == MT_CUSTOM) {
@ -321,8 +344,8 @@ static void printMAPInfo(void) {
if (engineConfiguration->baroSensor.sensorType == MT_CUSTOM) { if (engineConfiguration->baroSensor.sensorType == MT_CUSTOM) {
scheduleMsg(&logger, "min=%f max=%f", engineConfiguration->baroSensor.Min, engineConfiguration->baroSensor.Max); scheduleMsg(&logger, "min=%f max=%f", engineConfiguration->baroSensor.Min, engineConfiguration->baroSensor.Max);
} }
#endif
} }
#endif
static void printTPSInfo(void) { static void printTPSInfo(void) {
#if EFI_PROD_CODE #if EFI_PROD_CODE
@ -337,16 +360,22 @@ static void printTPSInfo(void) {
static void printTemperatureInfo(void) { static void printTemperatureInfo(void) {
printThermistor("CLT", &engineConfiguration2->clt); printThermistor("CLT", &engineConfiguration2->clt);
if (!isValidCoolantTemperature(getCoolantTemperature())) {
scheduleMsg(&logger, "CLT sensing error");
}
printThermistor("IAT", &engineConfiguration2->iat); printThermistor("IAT", &engineConfiguration2->iat);
if (!isValidIntakeAirTemperature(getIntakeAirTemperature())) {
scheduleMsg(&logger, "IAT sensing error");
}
float rClt = getResistance(&engineConfiguration2->clt); float rClt = getResistance(&engineConfiguration2->clt);
float rIat = getResistance(&engineConfiguration2->iat); float rIat = getResistance(&engineConfiguration2->iat);
#if EFI_ANALOG_INPUTS #if EFI_ANALOG_INPUTS
int cltChannel = engineConfiguration2->clt.channel; adc_channel_e cltChannel = engineConfiguration2->clt.channel;
scheduleMsg(&logger, "CLT R=%f on channel %d@%s", rClt, cltChannel, scheduleMsg(&logger, "CLT R=%f on channel %d@%s", rClt, cltChannel,
getPinNameByAdcChannel(cltChannel, pinNameBuffer)); getPinNameByAdcChannel(cltChannel, pinNameBuffer));
int iatChannel = engineConfiguration2->iat.channel; adc_channel_e iatChannel = engineConfiguration2->iat.channel;
scheduleMsg(&logger, "IAT R=%f on channel %d@%s", rIat, iatChannel, scheduleMsg(&logger, "IAT R=%f on channel %d@%s", rIat, iatChannel,
getPinNameByAdcChannel(iatChannel, pinNameBuffer)); getPinNameByAdcChannel(iatChannel, pinNameBuffer));
@ -450,6 +479,10 @@ static void setGlobalFuelCorrection(float value) {
engineConfiguration->globalFuelCorrection = value; engineConfiguration->globalFuelCorrection = value;
} }
static void setVBattDivider(float value) {
engineConfiguration->vbattDividerCoeff = value;
}
static void setWholeTimingMap(float value) { static void setWholeTimingMap(float value) {
// todo: table helper? // todo: table helper?
scheduleMsg(&logger, "Setting whole timing map to %f", value); scheduleMsg(&logger, "Setting whole timing map to %f", value);
@ -465,7 +498,72 @@ static void setWholeFuelMapCmd(float value) {
setWholeFuelMap(engineConfiguration, value); setWholeFuelMap(engineConfiguration, value);
} }
static void setTimingMap(char * rpmStr, char *loadStr, char *valueStr) { static void setTriggerInputPin(const char *indexStr, const char *pinName) {
#if EFI_PROD_CODE
int index = atoi(indexStr);
if (index < 0 || index > 2)
return;
brain_pin_e pin = parseBrainPin(pinName);
scheduleMsg(&logger, "setting trigger pin[%d] to %s please save&restart", index, hwPortname(pin));
boardConfiguration->triggerInputPins[index] = pin;
#endif
}
#if EFI_PROD_CODE
static void setTriggerSimulatorMode(const char *indexStr, const char *modeCode) {
int index = atoi(indexStr);
if (index < 0 || index > 2 || absI(index) == ERROR_CODE) {
return;
}
int mode = atoi(modeCode);
if (absI(mode) == ERROR_CODE) {
return;
}
boardConfiguration->triggerSimulatorPinModes[index] = (pin_output_mode_e) mode;
}
static void setTriggerSimulatorPin(const char *indexStr, const char *pinName) {
int index = atoi(indexStr);
if (index < 0 || index > 2)
return;
brain_pin_e pin = parseBrainPin(pinName);
scheduleMsg(&logger, "setting trigger simulator pin[%d] to %s please save&restart", index, hwPortname(pin));
boardConfiguration->triggerSimulatorPins[index] = pin;
}
static void setAnalogInputPin(const char *sensorStr, const char *pinName) {
brain_pin_e pin = parseBrainPin(pinName);
adc_channel_e channel = getAdcChannel(pin);
if (channel == EFI_ADC_ERROR) {
scheduleMsg(&logger, "Error with [%s]", pinName);
return;
}
if (strEqual("map", sensorStr)) {
engineConfiguration->map.sensor.hwChannel = channel;
scheduleMsg(&logger, "setting MAP to %s/%d", pinName, channel);
} else if (strEqual("clt", sensorStr)) {
engineConfiguration->cltAdcChannel = channel;
scheduleMsg(&logger, "setting CLT to %s/%d", pinName, channel);
} else if (strEqual("iat", sensorStr)) {
engineConfiguration->iatAdcChannel = channel;
scheduleMsg(&logger, "setting IAT to %s/%d", pinName, channel);
} else if (strEqual("tps", sensorStr)) {
engineConfiguration->tpsAdcChannel = channel;
scheduleMsg(&logger, "setting TPS to %s/%d", pinName, channel);
}
}
static void setLogicInputPin(const char *indexStr, const char *pinName) {
int index = atoi(indexStr);
if (index < 0 || index > 2)
return;
brain_pin_e pin = parseBrainPin(pinName);
scheduleMsg(&logger, "setting logic input pin[%d] to %s please save&restart", index, hwPortname(pin));
boardConfiguration->logicAnalyzerPins[index] = pin;
}
#endif /* EFI_PROD_CODE */
static void setTimingMap(const char * rpmStr, const char *loadStr, const char *valueStr) {
float rpm = atoff(rpmStr); float rpm = atoff(rpmStr);
float engineLoad = atoff(loadStr); float engineLoad = atoff(loadStr);
float value = atoff(valueStr); float value = atoff(valueStr);
@ -479,7 +577,7 @@ static void setTimingMap(char * rpmStr, char *loadStr, char *valueStr) {
scheduleMsg(&logger, "Setting timing map entry %d:%d to %f", rpmIndex, loadIndex, value); scheduleMsg(&logger, "Setting timing map entry %d:%d to %f", rpmIndex, loadIndex, value);
} }
static void setFuelMap(char * rpmStr, char *loadStr, char *valueStr) { static void setFuelMap(const char * rpmStr, const char *loadStr, const char *valueStr) {
float rpm = atoff(rpmStr); float rpm = atoff(rpmStr);
float engineLoad = atoff(loadStr); float engineLoad = atoff(loadStr);
float value = atoff(valueStr); float value = atoff(valueStr);
@ -494,15 +592,35 @@ static void setFuelMap(char * rpmStr, char *loadStr, char *valueStr) {
} }
static void enableInjection(void) { static void enableInjection(void) {
engineConfiguration2->isInjectionEnabledFlag = TRUE; engineConfiguration->isInjectionEnabled = true;
scheduleMsg(&logger, "injection enabled"); scheduleMsg(&logger, "injection enabled");
} }
static void disableInjection(void) { static void disableInjection(void) {
engineConfiguration2->isInjectionEnabledFlag = FALSE; engineConfiguration->isInjectionEnabled = false;
scheduleMsg(&logger, "injection disabled"); scheduleMsg(&logger, "injection disabled");
} }
static void enableIgnition(void) {
engineConfiguration->isIgnitionEnabled = true;
scheduleMsg(&logger, "ignition enabled");
}
static void disableIgnition(void) {
engineConfiguration->isIgnitionEnabled = false;
scheduleMsg(&logger, "ignition disabled");
}
static void enableSelfStimulation(void) {
engineConfiguration->directSelfStimulation = true;
scheduleMsg(&logger, "self stimulation enabled");
}
static void disableSelfStimulation(void) {
engineConfiguration->directSelfStimulation = false;
scheduleMsg(&logger, "self stimulation disabled");
}
#if EFI_WAVE_CHART #if EFI_WAVE_CHART
extern int waveChartUsedSize; extern int waveChartUsedSize;
#endif #endif
@ -510,11 +628,11 @@ extern int waveChartUsedSize;
static void printAllInfo(void) { static void printAllInfo(void) {
printTemperatureInfo(); printTemperatureInfo();
printTPSInfo(); printTPSInfo();
printMAPInfo();
#if EFI_WAVE_CHART #if EFI_WAVE_CHART
scheduleMsg(&logger, "waveChartUsedSize=%d", waveChartUsedSize); scheduleMsg(&logger, "waveChartUsedSize=%d", waveChartUsedSize);
#endif #endif
#if EFI_PROD_CODE #if EFI_PROD_CODE
printMAPInfo();
scheduleMsg(&logger, "console mode jumper: %s", boolToString(!GET_CONSOLE_MODE_VALUE())); scheduleMsg(&logger, "console mode jumper: %s", boolToString(!GET_CONSOLE_MODE_VALUE()));
scheduleMsg(&logger, "board test mode jumper: %s", boolToString(GET_BOARD_TEST_MODE_VALUE())); scheduleMsg(&logger, "board test mode jumper: %s", boolToString(GET_BOARD_TEST_MODE_VALUE()));
#endif #endif
@ -526,7 +644,6 @@ void initSettings(void) {
addConsoleAction("showconfig", doPrintConfiguration); addConsoleAction("showconfig", doPrintConfiguration);
addConsoleAction("tempinfo", printTemperatureInfo); addConsoleAction("tempinfo", printTemperatureInfo);
addConsoleAction("tpsinfo", printTPSInfo); addConsoleAction("tpsinfo", printTPSInfo);
addConsoleAction("mapinfo", printMAPInfo);
addConsoleAction("info", printAllInfo); addConsoleAction("info", printAllInfo);
addConsoleActionI("set_ignition_offset", setIgnitionOffset); addConsoleActionI("set_ignition_offset", setIgnitionOffset);
@ -536,6 +653,7 @@ void initSettings(void) {
addConsoleActionI("set_fixed_mode_timing", setFixedModeTiming); addConsoleActionI("set_fixed_mode_timing", setFixedModeTiming);
addConsoleActionI("set_timing_mode", setTimingMode); addConsoleActionI("set_timing_mode", setTimingMode);
addConsoleActionI("set_engine_type", setEngineType); addConsoleActionI("set_engine_type", setEngineType);
addConsoleActionI("set_idle_mode", setIdleMode);
addConsoleActionI("set_injection_pin_mode", setInjectionPinMode); addConsoleActionI("set_injection_pin_mode", setInjectionPinMode);
addConsoleActionI("set_ignition_pin_mode", setIgnitionPinMode); addConsoleActionI("set_ignition_pin_mode", setIgnitionPinMode);
@ -567,9 +685,28 @@ void initSettings(void) {
addConsoleActionI("set_firing_order", setFiringOrder); addConsoleActionI("set_firing_order", setFiringOrder);
addConsoleActionI("set_algorithm", setAlgorithm); addConsoleActionI("set_algorithm", setAlgorithm);
// todo: refactor this - looks like all boolean flags should be controlled with less code duplication
addConsoleAction("enable_injection", enableInjection); addConsoleAction("enable_injection", enableInjection);
addConsoleAction("disable_injection", disableInjection); addConsoleAction("disable_injection", disableInjection);
addConsoleAction("enable_ignition", enableIgnition);
addConsoleAction("disable_ignition", disableIgnition);
addConsoleAction("enable_self_stimulation", enableSelfStimulation);
addConsoleAction("disable_self_stimulation", disableSelfStimulation);
addConsoleActionII("set_toothed_wheel", setToothedWheel); addConsoleActionII("set_toothed_wheel", setToothedWheel);
addConsoleActionI("set_trigger_type", setTriggerType); addConsoleActionI("set_trigger_type", setTriggerType);
addConsoleActionSS("set_trigger_input_pin", setTriggerInputPin);
addConsoleActionF("set_vbatt_divider", setVBattDivider);
#if EFI_PROD_CODE
addConsoleActionSS("set_trigger_simulator_pin", setTriggerSimulatorPin);
addConsoleActionSS("set_trigger_simulator_mode", setTriggerSimulatorMode);
addConsoleAction("mapinfo", printMAPInfo);
addConsoleActionSS("set_analog_input_pin", setAnalogInputPin);
addConsoleActionSS("set_logic_input_pin", setLogicInputPin);
#endif /* EFI_PROD_CODE */
} }

View File

@ -56,7 +56,7 @@ static uint64_t getNextSwitchTimeUs(PwmConfig *state) {
* Once 'iteration' gets relatively high, we might lose calculation precision here. * Once 'iteration' gets relatively high, we might lose calculation precision here.
* This is addressed by ITERATION_LIMIT * This is addressed by ITERATION_LIMIT
*/ */
uint64_t timeToSwitchUs = (iteration + switchTime) * periodUs; uint64_t timeToSwitchUs = (uint64_t)((iteration + switchTime) * periodUs);
#if DEBUG_PWM #if DEBUG_PWM
scheduleMsg(&logger, "start=%d timeToSwitch=%d", state->safe.start, timeToSwitch); scheduleMsg(&logger, "start=%d timeToSwitch=%d", state->safe.start, timeToSwitch);
@ -66,8 +66,9 @@ static uint64_t getNextSwitchTimeUs(PwmConfig *state) {
void PwmConfig::handleCycleStart() { void PwmConfig::handleCycleStart() {
if (safe.phaseIndex == 0) { if (safe.phaseIndex == 0) {
if (cycleCallback != NULL) if (cycleCallback != NULL) {
cycleCallback(this); cycleCallback(this);
}
efiAssertVoid(periodUs != 0, "period not initialized"); efiAssertVoid(periodUs != 0, "period not initialized");
if (safe.periodUs != periodUs || safe.iteration == ITERATION_LIMIT) { if (safe.periodUs != periodUs || safe.iteration == ITERATION_LIMIT) {
/** /**
@ -119,6 +120,9 @@ static uint64_t togglePwmState(PwmConfig *state) {
* We are here if we are late for a state transition. * 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 * At 12000RPM=200Hz with a 60 toothed wheel we need to change state every
* 1000000 / 200 / 120 = ~41 uS. We are kind of OK. * 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"); //todo: introduce error and test this error handling warning(OBD_PCM_Processor_Fault, "PWM: negative switch time");
timeToSwitch = 10; timeToSwitch = 10;

View File

@ -30,7 +30,7 @@
#endif #endif
#if EFI_ENGINE_CONTROL #if EFI_ENGINE_CONTROL || defined(__DOXYGEN__)
#include "main_trigger_callback.h" #include "main_trigger_callback.h"
#include "ec2.h" #include "ec2.h"
@ -97,7 +97,7 @@ static void handleFuelInjectionEvent(MainTriggerCallback *mainTriggerCallback, A
} }
static void handleFuel(MainTriggerCallback *mainTriggerCallback, int eventIndex, int rpm) { static void handleFuel(MainTriggerCallback *mainTriggerCallback, int eventIndex, int rpm) {
if (!isInjectionEnabled(mainTriggerCallback->engineConfiguration2)) if (!isInjectionEnabled(mainTriggerCallback->engineConfiguration))
return; return;
efiAssertVoid(getRemainingStack(chThdSelf()) > 16, "stack#3"); efiAssertVoid(getRemainingStack(chThdSelf()) > 16, "stack#3");
efiAssertVoid(eventIndex < 2 * mainTriggerCallback->engineConfiguration2->triggerShape.shaftPositionEventCount, efiAssertVoid(eventIndex < 2 * mainTriggerCallback->engineConfiguration2->triggerShape.shaftPositionEventCount,
@ -185,7 +185,7 @@ static void handleSparkEvent(MainTriggerCallback *mainTriggerCallback, int event
} }
static void handleSpark(MainTriggerCallback *mainTriggerCallback, int eventIndex, int rpm, IgnitionEventList *list) { static void handleSpark(MainTriggerCallback *mainTriggerCallback, int eventIndex, int rpm, IgnitionEventList *list) {
if (!isValidRpm(rpm)) if (!isValidRpm(rpm) || !mainTriggerCallback->engineConfiguration->isIgnitionEnabled)
return; // this might happen for instance in case of a single trigger event after a pause return; // this might happen for instance in case of a single trigger event after a pause
/** /**
@ -326,17 +326,16 @@ void MainTriggerCallback::init(Engine *engine, engine_configuration2_s *engineCo
void initMainEventListener(Engine *engine, engine_configuration2_s *engineConfiguration2) { void initMainEventListener(Engine *engine, engine_configuration2_s *engineConfiguration2) {
efiAssertVoid(engine!=NULL, "null engine"); efiAssertVoid(engine!=NULL, "null engine");
engine_configuration_s *engineConfiguration = engine->engineConfiguration;
mainTriggerCallbackInstance.init(engine, engineConfiguration2); mainTriggerCallbackInstance.init(engine, engineConfiguration2);
#if EFI_PROD_CODE #if EFI_PROD_CODE || defined(__DOXYGEN__)
addConsoleAction("performanceinfo", showTriggerHistogram); addConsoleAction("performanceinfo", showTriggerHistogram);
addConsoleAction("maininfo", showMainInfo); addConsoleAction("maininfo", showMainInfo);
initLogging(&logger, "main event handler"); initLogging(&logger, "main event handler");
printMsg(&logger, "initMainLoop: %d", currentTimeMillis()); printMsg(&logger, "initMainLoop: %d", currentTimeMillis());
if (!isInjectionEnabled(mainTriggerCallbackInstance.engineConfiguration2)) if (!isInjectionEnabled(mainTriggerCallbackInstance.engineConfiguration))
printMsg(&logger, "!!!!!!!!!!!!!!!!!!! injection disabled"); printMsg(&logger, "!!!!!!!!!!!!!!!!!!! injection disabled");
#endif #endif

View File

@ -19,7 +19,7 @@
extern WaveChart waveChart; extern WaveChart waveChart;
#endif /* EFI_WAVE_CHART */ #endif /* EFI_WAVE_CHART */
#if EFI_SHAFT_POSITION_INPUT #if EFI_SHAFT_POSITION_INPUT || defined(__DOXYGEN__)
#include "trigger_central.h" #include "trigger_central.h"
#include "engine_configuration.h" #include "engine_configuration.h"
@ -77,7 +77,7 @@ uint64_t getLastRpmEventTime(void) {
return rpmState.lastRpmEventTimeUs; return rpmState.lastRpmEventTimeUs;
} }
#if EFI_PROD_CODE || EFI_SIMULATOR #if (EFI_PROD_CODE || EFI_SIMULATOR) || defined(__DOXYGEN__)
bool isCranking(void) { bool isCranking(void) {
int rpm = getRpm(); int rpm = getRpm();
return isCrankingR(rpm); return isCrankingR(rpm);
@ -141,10 +141,14 @@ void rpmShaftPositionCallback(trigger_event_e ckpSignalType, int index, RpmCalcu
addWaveChartEvent(WC_CRANK2, WC_UP, (char*) shaft_signal_msg_index); addWaveChartEvent(WC_CRANK2, WC_UP, (char*) shaft_signal_msg_index);
} else if (ckpSignalType == SHAFT_SECONDARY_DOWN) { } else if (ckpSignalType == SHAFT_SECONDARY_DOWN) {
addWaveChartEvent(WC_CRANK2, WC_DOWN, (char*) shaft_signal_msg_index); addWaveChartEvent(WC_CRANK2, WC_DOWN, (char*) shaft_signal_msg_index);
} else if (ckpSignalType == SHAFT_3RD_UP) {
addWaveChartEvent(WC_CRANK3, WC_UP, (char*) shaft_signal_msg_index);
} else if (ckpSignalType == SHAFT_3RD_DOWN) {
addWaveChartEvent(WC_CRANK3, WC_DOWN, (char*) shaft_signal_msg_index);
} }
if (index != 0) { if (index != 0) {
#if EFI_ANALOG_CHART #if EFI_ANALOG_CHART || defined(__DOXYGEN__)
if (engineConfiguration->analogChartMode == AC_TRIGGER) if (engineConfiguration->analogChartMode == AC_TRIGGER)
acAddData(getCrankshaftAngle(getTimeNowUs()), 1000 * ckpSignalType + index); acAddData(getCrankshaftAngle(getTimeNowUs()), 1000 * ckpSignalType + index);
#endif #endif
@ -174,7 +178,7 @@ void rpmShaftPositionCallback(trigger_event_e ckpSignalType, int index, RpmCalcu
} }
} }
rpmState->lastRpmEventTimeUs = nowUs; rpmState->lastRpmEventTimeUs = nowUs;
#if EFI_ANALOG_CHART #if EFI_ANALOG_CHART || defined(__DOXYGEN__)
if (engineConfiguration->analogChartMode == AC_TRIGGER) if (engineConfiguration->analogChartMode == AC_TRIGGER)
acAddData(getCrankshaftAngle(nowUs), index); acAddData(getCrankshaftAngle(nowUs), index);
#endif #endif
@ -184,22 +188,31 @@ static scheduling_s tdcScheduler[2];
static char rpmBuffer[10]; static char rpmBuffer[10];
#if EFI_PROD_CODE || EFI_SIMULATOR #if (EFI_PROD_CODE || EFI_SIMULATOR) || defined(__DOXYGEN__)
/**
* This callback has nothing to do with actual engine control, it just sends a Top Dead Center mark to the dev console
* digital sniffer.
*/
static void onTdcCallback(void) { static void onTdcCallback(void) {
itoa10(rpmBuffer, getRpm()); itoa10(rpmBuffer, getRpm());
addWaveChartEvent(TOP_DEAD_CENTER_MESSAGE, (char*) rpmBuffer, ""); addWaveChartEvent(TOP_DEAD_CENTER_MESSAGE, (char*) rpmBuffer, "");
} }
/**
* This trigger callback schedules the actual physical TDC callback in relation to trigger synchronization point.
*/
static void tdcMarkCallback(trigger_event_e ckpSignalType, int index, void *arg) { static void tdcMarkCallback(trigger_event_e ckpSignalType, int index, void *arg) {
if (index == 0) { bool isTriggerSynchronizationPoint = index == 0;
if (isTriggerSynchronizationPoint) {
int index = getRevolutionCounter() % 2; int index = getRevolutionCounter() % 2;
// todo: use event-based scheduling, not just time-based scheduling
scheduleByAngle(&tdcScheduler[index], engineConfiguration->globalTriggerAngleOffset, (schfunc_t) onTdcCallback, NULL); scheduleByAngle(&tdcScheduler[index], engineConfiguration->globalTriggerAngleOffset, (schfunc_t) onTdcCallback, NULL);
} }
} }
#endif #endif
void initRpmCalculator(void) { void initRpmCalculator(void) {
#if EFI_PROD_CODE || EFI_SIMULATOR #if (EFI_PROD_CODE || EFI_SIMULATOR) || defined(__DOXYGEN__)
initLogging(&logger, "rpm calc"); initLogging(&logger, "rpm calc");
engine.rpmCalculator = &rpmState; engine.rpmCalculator = &rpmState;
@ -213,7 +226,7 @@ void initRpmCalculator(void) {
addTriggerEventListener((ShaftPositionListener)&rpmShaftPositionCallback, "rpm reporter", &rpmState); addTriggerEventListener((ShaftPositionListener)&rpmShaftPositionCallback, "rpm reporter", &rpmState);
} }
#if EFI_PROD_CODE || EFI_SIMULATOR #if (EFI_PROD_CODE || EFI_SIMULATOR) || defined(__DOXYGEN__)
/** /**
* Schedules a callback 'angle' degree of crankshaft from now. * Schedules a callback 'angle' degree of crankshaft from now.
* The callback would be executed once after the duration of time which * The callback would be executed once after the duration of time which

View File

@ -9,12 +9,13 @@
#ifndef RPM_REPORTER_H_ #ifndef RPM_REPORTER_H_
#define RPM_REPORTER_H_ #define RPM_REPORTER_H_
#include <time.h> #include <global.h>
#define WC_DOWN "d" #define WC_DOWN "d"
#define WC_UP "u" #define WC_UP "u"
#define WC_CRANK1 "c1" #define WC_CRANK1 "c1"
#define WC_CRANK2 "c2" #define WC_CRANK2 "c2"
#define WC_CRANK3 "c3"
#define NOISY_RPM -1 #define NOISY_RPM -1
@ -58,6 +59,9 @@ uint64_t getLastRpmEventTime(void);
int getRevolutionCounter(void); int getRevolutionCounter(void);
float getCrankshaftAngle(uint64_t timeUs); float getCrankshaftAngle(uint64_t timeUs);
/**
* @return true if engine is running
*/
bool isRunning(void); bool isRunning(void);
bool isValidRpm(int rpm); bool isValidRpm(int rpm);
void addWaveChartEvent(const char *name, const char *msg, const char *msg2); void addWaveChartEvent(const char *name, const char *msg, const char *msg2);

View File

@ -7,6 +7,7 @@ TRIGGER_DECODERS_SRC_CPP = \
$(PROJECT_DIR)/controllers/trigger/trigger_chrysler.cpp \ $(PROJECT_DIR)/controllers/trigger/trigger_chrysler.cpp \
$(PROJECT_DIR)/controllers/trigger/trigger_structure.cpp \ $(PROJECT_DIR)/controllers/trigger/trigger_structure.cpp \
$(PROJECT_DIR)/controllers/trigger/trigger_decoder.cpp \ $(PROJECT_DIR)/controllers/trigger/trigger_decoder.cpp \
$(PROJECT_DIR)/controllers/trigger/trigger_mitsubishi.cpp \
$(PROJECT_DIR)/controllers/trigger/trigger_gm.cpp $(PROJECT_DIR)/controllers/trigger/trigger_gm.cpp
TRIGGER_SRC_CPP = \ TRIGGER_SRC_CPP = \

View File

@ -26,8 +26,9 @@ void configureMiniCooperTriggerShape(trigger_shape_s *s) {
s->addEvent(a, T_SECONDARY, TV_LOW); s->addEvent(a, T_SECONDARY, TV_LOW);
a += w; a += w;
for (int i = 0; i <= 22; i++) for (int i = 0; i <= 22; i++) {
a = addPair(s, a, w); a = addPair(s, a, w);
}
a += 3 * w; a += 3 * w;
@ -38,13 +39,15 @@ void configureMiniCooperTriggerShape(trigger_shape_s *s) {
s->addEvent(a, T_SECONDARY, TV_LOW); s->addEvent(a, T_SECONDARY, TV_LOW);
a += w; a += w;
for (int i = 0; i < 36; i++) for (int i = 0; i < 36; i++) {
a = addPair(s, a, w); a = addPair(s, a, w);
}
s->addEvent(376, T_PRIMARY, TV_HIGH); s->addEvent(376, T_PRIMARY, TV_HIGH);
for (int i = 0; i < 21; i++) for (int i = 0; i < 21; i++) {
a = addPair(s, a, w); a = addPair(s, a, w);
}
a += 3 * w; a += 3 * w;
efiAssertVoid(absF(firstGapAngle + 360 - a) < 0.1, "shape constraint"); efiAssertVoid(absF(firstGapAngle + 360 - a) < 0.1, "shape constraint");
@ -54,13 +57,14 @@ void configureMiniCooperTriggerShape(trigger_shape_s *s) {
s->addEvent(a, T_SECONDARY, TV_LOW); s->addEvent(a, T_SECONDARY, TV_LOW);
a += w; a += w;
for (int i = 0; i < 33; i++) for (int i = 0; i < 33; i++) {
a = addPair(s, a, w); a = addPair(s, a, w);
}
efiAssertVoid(absF(720 - w / 2 - a) < 0.1, "shape constraint"); efiAssertVoid(absF(720 - w / 2 - a) < 0.1, "shape constraint");
s->addEvent(a, T_SECONDARY, TV_HIGH); s->addEvent(a, T_SECONDARY, TV_HIGH);
s->addEvent(720, T_PRIMARY, TV_LOW); s->addEvent(720.0, T_PRIMARY, TV_LOW);
s->shaftPositionEventCount = s->getSize(); s->shaftPositionEventCount = s->getSize();
/** /**

View File

@ -11,7 +11,6 @@
#include "main_trigger_callback.h" #include "main_trigger_callback.h"
#include "engine_configuration.h" #include "engine_configuration.h"
#include "listener_array.h" #include "listener_array.h"
#include "wave_math.h"
#include "data_buffer.h" #include "data_buffer.h"
#include "histogram.h" #include "histogram.h"
#if EFI_PROD_CODE #if EFI_PROD_CODE
@ -63,6 +62,10 @@ TriggerCentral::TriggerCentral() {
clearCallbacks(&triggerListeneres); clearCallbacks(&triggerListeneres);
} }
int TriggerCentral::getHwEventCounter(int index) {
return hwEventCounters[index];
}
void TriggerCentral::handleShaftSignal(configuration_s *configuration, trigger_event_e signal, uint64_t nowUs) { void TriggerCentral::handleShaftSignal(configuration_s *configuration, trigger_event_e signal, uint64_t nowUs) {
efiAssertVoid(configuration!=NULL, "configuration"); efiAssertVoid(configuration!=NULL, "configuration");
@ -81,7 +84,7 @@ void TriggerCentral::handleShaftSignal(configuration_s *configuration, trigger_e
* We are here if there is a time gap between now and previous shaft event - that means the engine is not runnig. * We are here if there is a time gap between now and previous shaft event - that means the engine is not runnig.
* That means we have lost synchronization since the engine is not running :) * That means we have lost synchronization since the engine is not running :)
*/ */
triggerState.shaft_is_synchronized = FALSE; triggerState.shaft_is_synchronized = false;
} }
previousShaftEventTime = nowUs; previousShaftEventTime = nowUs;
@ -92,16 +95,19 @@ void TriggerCentral::handleShaftSignal(configuration_s *configuration, trigger_e
*/ */
triggerState.decodeTriggerEvent(triggerShape, &configuration->engineConfiguration->triggerConfig, signal, nowUs); triggerState.decodeTriggerEvent(triggerShape, &configuration->engineConfiguration->triggerConfig, signal, nowUs);
if (!triggerState.shaft_is_synchronized) if (!triggerState.shaft_is_synchronized) {
return; // we should not propagate event if we do not know where we are // we should not propagate event if we do not know where we are
return;
}
if (triggerState.getCurrentIndex() >= configuration->engineConfiguration2->triggerShape.shaftPositionEventCount) { if (triggerState.getCurrentIndex() >= configuration->engineConfiguration2->triggerShape.shaftPositionEventCount) {
int f = warning(OBD_PCM_Processor_Fault, "unexpected eventIndex=%d", triggerState.getCurrentIndex()); int f = warning(OBD_PCM_Processor_Fault, "unexpected eventIndex=%d", triggerState.getCurrentIndex());
if (!f) { if (!f) {
#if EFI_PROD_CODE #if EFI_PROD_CODE
// this temporary code is about trigger noise debugging // this temporary code is about trigger noise debugging
for (int i = 0; i < HW_EVENT_TYPES; i++) for (int i = 0; i < HW_EVENT_TYPES; i++) {
scheduleMsg(&logging, "event type: %d count=%d", i, hwEventCounters[i]); scheduleMsg(&logging, "event type: %d count=%d", i, hwEventCounters[i]);
}
#endif #endif
} }
} else { } else {
@ -119,7 +125,6 @@ void TriggerCentral::handleShaftSignal(configuration_s *configuration, trigger_e
triggerIndexForListeners = triggerState.getCurrentIndex() + (isEven ? 0 : triggerShape->getSize()); triggerIndexForListeners = triggerState.getCurrentIndex() + (isEven ? 0 : triggerShape->getSize());
} }
/** /**
* Here we invoke all the listeners - the main engine control logic is inside these listeners * Here we invoke all the listeners - the main engine control logic is inside these listeners
*/ */
@ -140,9 +145,18 @@ void printAllCallbacksHistogram(void) {
#endif #endif
} }
static void triggerInfo() {
#if EFI_PROD_CODE
scheduleMsg(&logging, "trigger %d/%d/%d/%d", triggerCentral.getHwEventCounter(0),
triggerCentral.getHwEventCounter(1), triggerCentral.getHwEventCounter(2),
triggerCentral.getHwEventCounter(3));
#endif
}
void initTriggerCentral(void) { void initTriggerCentral(void) {
#if EFI_PROD_CODE #if EFI_PROD_CODE
initLogging(&logging, "ShaftPosition"); initLogging(&logging, "ShaftPosition");
addConsoleAction("triggerinfo", triggerInfo);
#endif #endif
#if EFI_HISTOGRAMS #if EFI_HISTOGRAMS

View File

@ -10,19 +10,21 @@
#include "rusefi_enums.h" #include "rusefi_enums.h"
#include "listener_array.h" #include "listener_array.h"
#include "trigger_decoder.h"
typedef void (*ShaftPositionListener)(trigger_event_e signal, int index, void *arg); typedef void (*ShaftPositionListener)(trigger_event_e signal, int index, void *arg);
#ifdef __cplusplus #ifdef __cplusplus
#include "ec2.h" #include "ec2.h"
#define HW_EVENT_TYPES 4 #define HW_EVENT_TYPES 6
class TriggerCentral { class TriggerCentral {
public: public:
TriggerCentral(); TriggerCentral();
void addEventListener(ShaftPositionListener handler, const char *name, void *arg); void addEventListener(ShaftPositionListener handler, const char *name, void *arg);
void handleShaftSignal(configuration_s *configuration, trigger_event_e signal, uint64_t nowUs); void handleShaftSignal(configuration_s *configuration, trigger_event_e signal, uint64_t nowUs);
int getHwEventCounter(int index);
TriggerState triggerState; TriggerState triggerState;
private: private:
IntListenerArray triggerListeneres; IntListenerArray triggerListeneres;

View File

@ -19,7 +19,7 @@ void configureNeonTriggerShape(trigger_shape_s *s) {
// voodoo magic - we always need 720 at the end // voodoo magic - we always need 720 at the end
int base = 720 - 560; int base = 720 - 560;
s->initialState[0] = 1; s->initialState[T_PRIMARY] = 1;
s->addEvent(base - 720 + 600, T_SECONDARY, TV_HIGH); s->addEvent(base - 720 + 600, T_SECONDARY, TV_HIGH);
s->addEvent(base - 720 + 604, T_SECONDARY, TV_LOW); s->addEvent(base - 720 + 604, T_SECONDARY, TV_LOW);

View File

@ -26,13 +26,11 @@
#include "trigger_chrysler.h" #include "trigger_chrysler.h"
#include "trigger_gm.h" #include "trigger_gm.h"
#include "trigger_bmw.h" #include "trigger_bmw.h"
#include "trigger_mitsubishi.h"
extern "C" {
#include "trigger_structure.h" #include "trigger_structure.h"
#include "wave_math.h"
}
#if EFI_PROD_CODE || EFI_SIMULATOR #if (EFI_PROD_CODE || EFI_SIMULATOR) || defined(__DOXYGEN__)
static Logging logger; static Logging logger;
#endif #endif
@ -45,33 +43,43 @@ int isTriggerDecoderError(void) {
return errorDetection.sum(6) > 4; return errorDetection.sum(6) > 4;
} }
static inline int isSynchronizationGap(TriggerState const *shaftPositionState, trigger_shape_s const *triggerShape, static inline bool isSynchronizationGap(TriggerState const *shaftPositionState, trigger_shape_s const *triggerShape,
trigger_config_s const *triggerConfig, const int currentDuration) { const int currentDuration) {
if (!triggerShape->isSynchronizationNeeded) if (!triggerShape->isSynchronizationNeeded) {
return false; return false;
}
return currentDuration > shaftPositionState->toothed_previous_duration * triggerShape->syncRatioFrom return currentDuration > shaftPositionState->toothed_previous_duration * triggerShape->syncRatioFrom
&& currentDuration < shaftPositionState->toothed_previous_duration * triggerShape->syncRatioTo; && currentDuration < shaftPositionState->toothed_previous_duration * triggerShape->syncRatioTo;
} }
static inline int noSynchronizationResetNeeded(TriggerState *shaftPositionState, trigger_shape_s const *triggerShape, static inline bool noSynchronizationResetNeeded(TriggerState *shaftPositionState, trigger_shape_s const *triggerShape) {
trigger_config_s const*triggerConfig) { if (triggerShape->isSynchronizationNeeded) {
if (triggerShape->isSynchronizationNeeded)
return false; return false;
if (!shaftPositionState->shaft_is_synchronized) }
return TRUE; if (!shaftPositionState->shaft_is_synchronized) {
return true;
}
/** /**
* in case of noise the counter could be above the expected number of events * in case of noise the counter could be above the expected number of events
*/ */
return shaftPositionState->getCurrentIndex() >= triggerShape->shaftPositionEventCount - 1; return shaftPositionState->getCurrentIndex() >= triggerShape->shaftPositionEventCount - 1;
} }
static trigger_wheel_e eventIndex[6] = { T_PRIMARY, T_PRIMARY, T_SECONDARY, T_SECONDARY, T_CHANNEL_3, T_CHANNEL_3 };
static trigger_value_e eventType[6] = { TV_LOW, TV_HIGH, TV_LOW, TV_HIGH, TV_LOW, TV_HIGH };
/** /**
* @brief Trigger decoding happends here * @brief Trigger decoding happens here
* This method changes the state of trigger_state_s data structure according to the trigger event * This method changes the state of trigger_state_s data structure according to the trigger event
*/ */
void TriggerState::decodeTriggerEvent(trigger_shape_s const*triggerShape, trigger_config_s const*triggerConfig, void TriggerState::decodeTriggerEvent(trigger_shape_s const*triggerShape, trigger_config_s const*triggerConfig,
trigger_event_e signal, uint64_t nowUs) { trigger_event_e const signal, uint64_t nowUs) {
efiAssertVoid(signal <= SHAFT_3RD_UP, "unexpected signal");
trigger_wheel_e triggerWheel = eventIndex[signal];
eventCount[triggerWheel]++;
int isLessImportant = (triggerShape->useRiseEdge && signal != SHAFT_PRIMARY_UP) int isLessImportant = (triggerShape->useRiseEdge && signal != SHAFT_PRIMARY_UP)
|| (!triggerShape->useRiseEdge && signal != SHAFT_PRIMARY_DOWN); || (!triggerShape->useRiseEdge && signal != SHAFT_PRIMARY_DOWN);
@ -80,7 +88,7 @@ void TriggerState::decodeTriggerEvent(trigger_shape_s const*triggerShape, trigge
/** /**
* For less important events we simply increment the index. * For less important events we simply increment the index.
*/ */
nextTriggerEvent(); nextTriggerEvent(triggerWheel, nowUs);
return; return;
} }
@ -100,28 +108,37 @@ void TriggerState::decodeTriggerEvent(trigger_shape_s const*triggerShape, trigge
} }
#endif #endif
if (noSynchronizationResetNeeded(this, triggerShape, triggerConfig) if (noSynchronizationResetNeeded(this, triggerShape) || isSynchronizationGap(this, triggerShape, currentDuration)) {
|| isSynchronizationGap(this, triggerShape, triggerConfig, currentDuration)) {
/** /**
* We can check if things are fine by comparing the number of events in a cycle with the expected number of event. * We can check if things are fine by comparing the number of events in a cycle with the expected number of event.
*/ */
int isDecodingError = getCurrentIndex() != triggerShape->shaftPositionEventCount - 1; bool isDecodingError = eventCount[0] != triggerShape->expectedEventCount[0]
|| eventCount[1] != triggerShape->expectedEventCount[1]
|| eventCount[2] != triggerShape->expectedEventCount[2];
errorDetection.add(isDecodingError); errorDetection.add(isDecodingError);
if (isTriggerDecoderError()) if (isTriggerDecoderError()) {
warning(OBD_PCM_Processor_Fault, "trigger decoding issue"); warning(OBD_PCM_Processor_Fault, "trigger decoding issue. expected %d/%d/%d got %d/%d/%d",
triggerShape->expectedEventCount[0], triggerShape->expectedEventCount[1],
triggerShape->expectedEventCount[2], eventCount[0], eventCount[1], eventCount[2]);
}
shaft_is_synchronized = TRUE; shaft_is_synchronized = true;
nextRevolution(triggerShape->shaftPositionEventCount); // this call would update duty cycle values
// nextTriggerEvent(triggerWheel, nowUs);
nextRevolution(triggerShape->shaftPositionEventCount, nowUs);
} else { } else {
nextTriggerEvent(); nextTriggerEvent(triggerWheel, nowUs);
} }
toothed_previous_duration = currentDuration; toothed_previous_duration = currentDuration;
toothed_previous_time = nowUs; toothed_previous_time = nowUs;
} }
static void initializeSkippedToothTriggerShape(trigger_shape_s *s, int totalTeethCount, int skippedCount, operation_mode_e operationMode) { static void initializeSkippedToothTriggerShape(trigger_shape_s *s, int totalTeethCount, int skippedCount,
operation_mode_e operationMode) {
efiAssertVoid(s != NULL, "trigger_shape_s is NULL"); efiAssertVoid(s != NULL, "trigger_shape_s is NULL");
s->reset(operationMode); s->reset(operationMode);
@ -139,8 +156,8 @@ static void initializeSkippedToothTriggerShape(trigger_shape_s *s, int totalTeet
s->addEvent(720, T_PRIMARY, TV_LOW); s->addEvent(720, T_PRIMARY, TV_LOW);
} }
void initializeSkippedToothTriggerShapeExt(trigger_shape_s *s, int totalTeethCount, void initializeSkippedToothTriggerShapeExt(trigger_shape_s *s, int totalTeethCount, int skippedCount,
int skippedCount, operation_mode_e operationMode) { operation_mode_e operationMode) {
efiAssertVoid(totalTeethCount > 0, "totalTeethCount is zero"); efiAssertVoid(totalTeethCount > 0, "totalTeethCount is zero");
s->totalToothCount = totalTeethCount; s->totalToothCount = totalTeethCount;
@ -185,17 +202,16 @@ void initializeTriggerShape(Logging *logger, engine_configuration_s *engineConfi
triggerShape->useRiseEdge = TRUE; triggerShape->useRiseEdge = TRUE;
triggerShape->needSecondTriggerInput = TRUE; triggerShape->needSecondTriggerInput = TRUE;
switch (triggerConfig->triggerType) { switch (triggerConfig->triggerType) {
case TT_TOOTHED_WHEEL: case TT_TOOTHED_WHEEL:
engineConfiguration2->triggerShape.needSecondTriggerInput = false; engineConfiguration2->triggerShape.needSecondTriggerInput = false;
engineConfiguration2->triggerShape.isSynchronizationNeeded = engineConfiguration->triggerConfig.customIsSynchronizationNeeded; engineConfiguration2->triggerShape.isSynchronizationNeeded =
engineConfiguration->triggerConfig.customIsSynchronizationNeeded;
initializeSkippedToothTriggerShapeExt(triggerShape, triggerConfig->customTotalToothCount, initializeSkippedToothTriggerShapeExt(triggerShape, triggerConfig->customTotalToothCount,
triggerConfig->customSkippedToothCount, triggerConfig->customSkippedToothCount, getOperationMode(engineConfiguration));
getOperationMode(engineConfiguration));
return; return;
case TT_MAZDA_MIATA_NB: case TT_MAZDA_MIATA_NB:
@ -231,6 +247,22 @@ void initializeTriggerShape(Logging *logger, engine_configuration_s *engineConfi
setToothedWheelConfiguration(triggerShape, 36, 1, engineConfiguration); setToothedWheelConfiguration(triggerShape, 36, 1, engineConfiguration);
return; return;
case TT_HONDA_ACCORD_CD_TWO_WIRES:
configureHondaAccordCD(triggerShape, false);
return;
case TT_HONDA_ACCORD_CD:
configureHondaAccordCD(triggerShape, true);
return;
case TT_HONDA_ACCORD_CD_DIP:
configureHondaAccordCDDip(triggerShape);
return;
case TT_MITSU:
initializeMitsubishi4g18(triggerShape);
return;
default: default:
firmwareError("initializeTriggerShape() not implemented: %d", triggerConfig->triggerType); firmwareError("initializeTriggerShape() not implemented: %d", triggerConfig->triggerType);
; ;
@ -244,7 +276,8 @@ TriggerStimulatorHelper::TriggerStimulatorHelper() {
secondaryWheelState = false; secondaryWheelState = false;
} }
void TriggerStimulatorHelper::nextStep(TriggerState *state, trigger_shape_s * shape, int i, trigger_config_s const*triggerConfig) { void TriggerStimulatorHelper::nextStep(TriggerState *state, trigger_shape_s * shape, int i,
trigger_config_s const*triggerConfig) {
int stateIndex = i % shape->getSize(); int stateIndex = i % shape->getSize();
int loopIndex = i / shape->getSize(); int loopIndex = i / shape->getSize();
@ -267,6 +300,22 @@ void TriggerStimulatorHelper::nextStep(TriggerState *state, trigger_shape_s * sh
} }
} }
static void onFindIndex(TriggerState *state) {
}
static int doFindTrigger(TriggerStimulatorHelper *helper, trigger_shape_s * shape, trigger_config_s const*triggerConfig,
TriggerState *state) {
for (int i = 0; i < 4 * PWM_PHASE_MAX_COUNT; i++) {
helper->nextStep(state, shape, i, triggerConfig);
if (state->shaft_is_synchronized)
return i % shape->getSize();;
}
firmwareError("findTriggerZeroEventIndex() failed");
return EFI_ERROR_CODE;
}
/** /**
* Trigger shape is defined in a way which is convenient for trigger shape definition * Trigger shape is defined in a way which is convenient for trigger shape definition
* On the other hand, trigger decoder indexing begins from synchronization event. * On the other hand, trigger decoder indexing begins from synchronization event.
@ -278,16 +327,23 @@ int findTriggerZeroEventIndex(trigger_shape_s * shape, trigger_config_s const*tr
TriggerState state; TriggerState state;
errorDetection.clear(); errorDetection.clear();
TriggerStimulatorHelper helper; TriggerStimulatorHelper helper;
for (int i = 0; i < 4 * PWM_PHASE_MAX_COUNT; i++) { int index = doFindTrigger(&helper, shape, triggerConfig, &state);
helper.nextStep(&state, shape, i, triggerConfig); if (index == EFI_ERROR_CODE) {
return index;
if (state.shaft_is_synchronized)
return i % shape->getSize();;
} }
firmwareError("findTriggerZeroEventIndex() failed"); /**
return -1; * Now that we have just located the synch point, we can simulate the whole cycle
* in order to calculate expected duty cycle
*/
state.cycleCallback = onFindIndex;
return index;
} }
void initTriggerDecoder(void) { void initTriggerDecoder(void) {

View File

@ -8,12 +8,52 @@
#ifndef TRIGGER_DECODER_H_ #ifndef TRIGGER_DECODER_H_
#define TRIGGER_DECODER_H_ #define TRIGGER_DECODER_H_
#include <time.h>
#include "trigger_structure.h" #include "trigger_structure.h"
#include "engine_configuration.h" #include "engine_configuration.h"
#include "ec2.h" #include "ec2.h"
class TriggerState;
typedef void (*TriggerStateCallback)(TriggerState *);
class TriggerState {
public:
TriggerState();
int getCurrentIndex();
int getTotalRevolutionCounter();
uint64_t getTotalEventCounter();
uint64_t getStartOfRevolutionIndex();
void nextRevolution(int triggerEventCount, uint64_t nowUs);
void nextTriggerEvent(trigger_wheel_e triggerWheel, uint64_t nowUs);
void decodeTriggerEvent(trigger_shape_s const*triggerShape, trigger_config_s const*triggerConfig, trigger_event_e const signal, uint64_t nowUs);
TriggerStateCallback cycleCallback;
/**
* TRUE if we know where we are
*/
unsigned char shaft_is_synchronized;
uint64_t toothed_previous_duration;
uint64_t toothed_previous_time;
private:
void clear();
/**
* index within trigger revolution, from 0 to trigger event count
*/
int current_index;
/**
* Number of actual events within current trigger cycle
* see trigger_shape_s
*/
int eventCount[PWM_PHASE_MAX_WAVE_PER_PWM];
uint64_t timeOfPreviousEvent[PWM_PHASE_MAX_WAVE_PER_PWM];
int totalTime[PWM_PHASE_MAX_WAVE_PER_PWM];
uint64_t totalEventCountBase;
int totalRevolutionCounter;
bool isFirstEvent;
};
class TriggerStimulatorHelper { class TriggerStimulatorHelper {
public: public:
TriggerStimulatorHelper(); TriggerStimulatorHelper();

View File

@ -7,7 +7,6 @@
#include "main.h" #include "main.h"
#include "trigger_emulator_algo.h" #include "trigger_emulator_algo.h"
#include "engine_configuration.h" #include "engine_configuration.h"
#include "wave_math.h"
#include "LocalVersionHolder.h" #include "LocalVersionHolder.h"
#include "ec2.h" #include "ec2.h"
@ -20,8 +19,9 @@ extern engine_configuration2_s *engineConfiguration2;
*/ */
static int pinStates1[PWM_PHASE_MAX_COUNT]; static int pinStates1[PWM_PHASE_MAX_COUNT];
static int pinStates2[PWM_PHASE_MAX_COUNT]; static int pinStates2[PWM_PHASE_MAX_COUNT];
static single_wave_s waves[2] = {single_wave_s(pinStates1), single_wave_s(pinStates2)}; static int pinStates3[PWM_PHASE_MAX_COUNT];
static single_wave_s sr[2] = {waves[0], waves[1]}; static single_wave_s waves[PWM_PHASE_MAX_WAVE_PER_PWM] = {single_wave_s(pinStates1), single_wave_s(pinStates2), single_wave_s(pinStates3)};
static single_wave_s sr[PWM_PHASE_MAX_WAVE_PER_PWM] = {waves[0], waves[1], waves[2]};
static float swtchTms[PWM_PHASE_MAX_COUNT]; static float swtchTms[PWM_PHASE_MAX_COUNT];
@ -52,8 +52,8 @@ static void updateTriggerShapeIfNeeded(PwmConfig *state) {
applyNonPersistentConfiguration(&logger, engineConfiguration, engineConfiguration2); applyNonPersistentConfiguration(&logger, engineConfiguration, engineConfiguration2);
trigger_shape_s *s = &engineConfiguration2->triggerShape; trigger_shape_s *s = &engineConfiguration2->triggerShape;
int *pinStates[2] = {s->wave.waves[0].pinStates, s->wave.waves[1].pinStates}; int *pinStates[PWM_PHASE_MAX_WAVE_PER_PWM] = {s->wave.waves[0].pinStates, s->wave.waves[1].pinStates, s->wave.waves[2].pinStates};
copyPwmParameters(state, s->getSize(), s->wave.switchTimes, 2, pinStates); copyPwmParameters(state, s->getSize(), s->wave.switchTimes, PWM_PHASE_MAX_WAVE_PER_PWM, pinStates);
state->safe.periodUs = -1; // this would cause loop re-initialization state->safe.periodUs = -1; // this would cause loop re-initialization
} }
} }
@ -64,10 +64,9 @@ void initTriggerEmulatorLogic(pwm_gen_callback *stateChangeCallback) {
trigger_shape_s *s = &engineConfiguration2->triggerShape; trigger_shape_s *s = &engineConfiguration2->triggerShape;
setTriggerEmulatorRPM(DEFAULT_EMULATION_RPM); setTriggerEmulatorRPM(DEFAULT_EMULATION_RPM);
int *pinStates[2] = { s->wave.waves[0].pinStates, s->wave.waves[1].pinStates}; int *pinStates[PWM_PHASE_MAX_WAVE_PER_PWM] = { s->wave.waves[0].pinStates, s->wave.waves[1].pinStates, s->wave.waves[2].pinStates};
weComplexInit("position sensor", &triggerSignal, s->getSize(), s->wave.switchTimes, 2, pinStates, weComplexInit("position sensor", &triggerSignal, s->getSize(), s->wave.switchTimes, PWM_PHASE_MAX_WAVE_PER_PWM, pinStates,
updateTriggerShapeIfNeeded, updateTriggerShapeIfNeeded, stateChangeCallback);
stateChangeCallback);
addConsoleActionI("rpm", &setTriggerEmulatorRPM); addConsoleActionI("rpm", &setTriggerEmulatorRPM);

View File

@ -16,25 +16,25 @@ void configureGmTriggerShape(trigger_config_s *triggerConfig, trigger_shape_s *s
s->shaftPositionEventCount = 14; s->shaftPositionEventCount = 14;
s->addEvent(120 - w, T_PRIMARY, TV_HIGH); s->addEvent(120 - w, T_PRIMARY, TV_HIGH);
s->addEvent(120, T_PRIMARY, TV_LOW); s->addEvent(120.0, T_PRIMARY, TV_LOW);
s->addEvent(240 - w, T_PRIMARY, TV_HIGH); s->addEvent(240 - w, T_PRIMARY, TV_HIGH);
s->addEvent(240, T_PRIMARY, TV_LOW); s->addEvent(240.0, T_PRIMARY, TV_LOW);
s->addEvent(360 - w, T_PRIMARY, TV_HIGH); s->addEvent(360 - w, T_PRIMARY, TV_HIGH);
s->addEvent(360, T_PRIMARY, TV_LOW); s->addEvent(360.0, T_PRIMARY, TV_LOW);
s->addEvent(480 - w, T_PRIMARY, TV_HIGH); s->addEvent(480 - w, T_PRIMARY, TV_HIGH);
s->addEvent(480, T_PRIMARY, TV_LOW); s->addEvent(480.0, T_PRIMARY, TV_LOW);
s->addEvent(600 - w, T_PRIMARY, TV_HIGH); s->addEvent(600 - w, T_PRIMARY, TV_HIGH);
s->addEvent(600, T_PRIMARY, TV_LOW); s->addEvent(600.0, T_PRIMARY, TV_LOW);
s->addEvent(700 - w, T_PRIMARY, TV_HIGH); s->addEvent(700 - w, T_PRIMARY, TV_HIGH);
s->addEvent(700, T_PRIMARY, TV_LOW); s->addEvent(700.0, T_PRIMARY, TV_LOW);
s->addEvent(720 - w, T_PRIMARY, TV_HIGH); s->addEvent(720 - w, T_PRIMARY, TV_HIGH);
s->addEvent(720, T_PRIMARY, TV_LOW); s->addEvent(720.0, T_PRIMARY, TV_LOW);
} }

View File

@ -21,7 +21,7 @@
#include "trigger_mazda.h" #include "trigger_mazda.h"
void initializeMazdaMiataNbShape(trigger_shape_s *s) { void initializeMazdaMiataNbShape(trigger_shape_s *s) {
setTriggerSynchronizationGap(s, 0.11); setTriggerSynchronizationGap(s, 0.11f);
s->useRiseEdge = false; s->useRiseEdge = false;
s->reset(FOUR_STROKE_CAM_SENSOR); s->reset(FOUR_STROKE_CAM_SENSOR);
@ -29,33 +29,33 @@ void initializeMazdaMiataNbShape(trigger_shape_s *s) {
/** /**
* cam sensor is primary, crank sensor is secondary * cam sensor is primary, crank sensor is secondary
*/ */
s->addEvent(20, T_PRIMARY, TV_HIGH); s->addEvent(20.0f, T_PRIMARY, TV_HIGH);
s->addEvent(66, T_SECONDARY, TV_LOW); s->addEvent(66.0f, T_SECONDARY, TV_LOW);
s->addEvent(70, T_SECONDARY, TV_HIGH); s->addEvent(70.0f, T_SECONDARY, TV_HIGH);
s->addEvent(136, T_SECONDARY, TV_LOW); s->addEvent(136.0f, T_SECONDARY, TV_LOW);
s->addEvent(140, T_SECONDARY, TV_HIGH); s->addEvent(140.0f, T_SECONDARY, TV_HIGH);
s->addEvent(246, T_SECONDARY, TV_LOW); s->addEvent(246.0f, T_SECONDARY, TV_LOW);
s->addEvent(250, T_SECONDARY, TV_HIGH); s->addEvent(250.0f, T_SECONDARY, TV_HIGH);
s->addEvent(316, T_SECONDARY, TV_LOW); s->addEvent(316.0f, T_SECONDARY, TV_LOW);
s->addEvent(320, T_SECONDARY, TV_HIGH); s->addEvent(320.0f, T_SECONDARY, TV_HIGH);
s->addEvent(340, T_PRIMARY, TV_LOW); s->addEvent(340.0f, T_PRIMARY, TV_LOW);
s->addEvent(360, T_PRIMARY, TV_HIGH); s->addEvent(360.0f, T_PRIMARY, TV_HIGH);
s->addEvent(380, T_PRIMARY, TV_LOW); s->addEvent(380.0f, T_PRIMARY, TV_LOW);
s->addEvent(400, T_PRIMARY, TV_HIGH); s->addEvent(400.0f, T_PRIMARY, TV_HIGH);
s->addEvent(426, T_SECONDARY, TV_LOW); s->addEvent(426.0f, T_SECONDARY, TV_LOW);
s->addEvent(430, T_SECONDARY, TV_HIGH); s->addEvent(430.0f, T_SECONDARY, TV_HIGH);
s->addEvent(496, T_SECONDARY, TV_LOW); s->addEvent(496.0f, T_SECONDARY, TV_LOW);
s->addEvent(500, T_SECONDARY, TV_HIGH); s->addEvent(500.0f, T_SECONDARY, TV_HIGH);
s->addEvent(606, T_SECONDARY, TV_LOW); s->addEvent(606.0f, T_SECONDARY, TV_LOW);
s->addEvent(610, T_SECONDARY, TV_HIGH); s->addEvent(610.0f, T_SECONDARY, TV_HIGH);
s->addEvent(676, T_SECONDARY, TV_LOW); s->addEvent(676.0f, T_SECONDARY, TV_LOW);
s->addEvent(680, T_SECONDARY, TV_HIGH); s->addEvent(680.0f, T_SECONDARY, TV_HIGH);
s->addEvent(720, T_PRIMARY, TV_LOW); s->addEvent(720.0f, T_PRIMARY, TV_LOW);
s->shaftPositionEventCount = 6 + 16; s->shaftPositionEventCount = 6 + 16;
} }
@ -68,7 +68,7 @@ void configureMazdaProtegeLx(trigger_shape_s *s) {
s->reset(FOUR_STROKE_CAM_SENSOR); s->reset(FOUR_STROKE_CAM_SENSOR);
// s->initialState[0] = 1; // s->initialState[0] = 1;
float w = 720 / 4 * 0.215; // float w = 720 / 4 * 0.215;
float a = 5; float a = 5;
// s->addEvent(a, T_SECONDARY, TV_LOW); // s->addEvent(a, T_SECONDARY, TV_LOW);
// s->addEvent(a + w, T_SECONDARY, TV_HIGH); // s->addEvent(a + w, T_SECONDARY, TV_HIGH);

View File

@ -0,0 +1,35 @@
/**
* @file trigger_mitsubishi.cpp
*
* @date Aug 5, 2014
* @author Andrey Belomutskiy, (c) 2012-2014
*/
#include "trigger_mitsubishi.h"
void initializeMitsubishi4g18(trigger_shape_s *s) {
s->reset(FOUR_STROKE_CAM_SENSOR);
s->useRiseEdge = false;
setTriggerSynchronizationGap(s, 1.6666);
int secondaryWidth = 70;
s->addEvent(180.0 - 131 + 5, T_PRIMARY, TV_HIGH);
s->addEvent(180.0 - secondaryWidth, T_SECONDARY, TV_HIGH);
s->addEvent(180.0, T_SECONDARY, TV_LOW);
s->addEvent(180.0 + 49 + 5, T_PRIMARY, TV_LOW);
s->addEvent(360.0 - secondaryWidth, T_SECONDARY, TV_HIGH);
s->addEvent(360.0, T_SECONDARY, TV_LOW);
s->addEvent(540.0 - 131 + 5, T_PRIMARY, TV_HIGH);
s->addEvent(540.0 - secondaryWidth, T_SECONDARY, TV_HIGH);
s->addEvent(540.0 - 131 + 5 + 90, T_PRIMARY, TV_LOW);
s->addEvent(540.0, T_SECONDARY, TV_LOW);
s->addEvent(720.0 - secondaryWidth, T_SECONDARY, TV_HIGH);
s->addEvent(720.0, T_SECONDARY, TV_LOW);
s->shaftPositionEventCount = s->getSize();
}

View File

@ -0,0 +1,14 @@
/**
* @file trigger_mitsubishi.h
*
* @date Aug 5, 2014
* @author Andrey Belomutskiy, (c) 2012-2014
*/
#ifndef TRIGGER_MITSUBISHI_H_
#define TRIGGER_MITSUBISHI_H_
#include "trigger_structure.h"
void initializeMitsubishi4g18(trigger_shape_s *s);
#endif /* TRIGGER_MITSUBISHI_H_ */

View File

@ -24,17 +24,18 @@
#include "trigger_decoder.h" #include "trigger_decoder.h"
trigger_shape_helper::trigger_shape_helper() { trigger_shape_helper::trigger_shape_helper() {
waves[0].init(pinStates0); for (int i = 0; i < TRIGGER_CHANNEL_COUNT; i++) {
waves[1].init(pinStates1); waves[i].init(pinStates[i]);
}
} }
trigger_shape_s::trigger_shape_s() : trigger_shape_s::trigger_shape_s() :
wave(switchTimes, NULL) { wave(switchTimesBuffer, NULL) {
reset(OM_NONE); reset(OM_NONE);
wave.waves = h.waves; wave.waves = h.waves;
} }
int trigger_shape_s::getSize() { int trigger_shape_s::getSize() const {
return size; return size;
} }
@ -46,6 +47,10 @@ int trigger_shape_s::getTriggerShapeSynchPointIndex() {
int getEngineCycleEventCount2(operation_mode_e mode, trigger_shape_s * s); int getEngineCycleEventCount2(operation_mode_e mode, trigger_shape_s * s);
float fixAngle(float angle); float fixAngle(float angle);
void trigger_shape_s::calculateTriggerSynchPoint(trigger_config_s const*triggerConfig) {
setTriggerShapeSynchPointIndex(findTriggerZeroEventIndex(this, triggerConfig));
}
void trigger_shape_s::setTriggerShapeSynchPointIndex(int triggerShapeSynchPointIndex) { void trigger_shape_s::setTriggerShapeSynchPointIndex(int triggerShapeSynchPointIndex) {
this->triggerShapeSynchPointIndex = triggerShapeSynchPointIndex; this->triggerShapeSynchPointIndex = triggerShapeSynchPointIndex;
@ -69,7 +74,8 @@ void trigger_shape_s::reset(operation_mode_e operationMode) {
shaftPositionEventCount = 0; shaftPositionEventCount = 0;
triggerShapeSynchPointIndex = 0; triggerShapeSynchPointIndex = 0;
memset(initialState, 0, sizeof(initialState)); memset(initialState, 0, sizeof(initialState));
memset(switchTimes, 0, sizeof(switchTimes)); memset(switchTimesBuffer, 0, sizeof(switchTimesBuffer));
memset(expectedEventCount, 0, sizeof(expectedEventCount));
wave.reset(); wave.reset();
previousAngle = 0; previousAngle = 0;
} }
@ -78,11 +84,32 @@ int multi_wave_s::getChannelState(int channelIndex, int phaseIndex) const {
return waves[channelIndex].pinStates[phaseIndex]; return waves[channelIndex].pinStates[phaseIndex];
} }
int multi_wave_s::waveIndertionAngle(float angle, int size) const {
for (int i = size - 1; i >= 0; i--) {
if (angle > switchTimes[i])
return i + 1;
}
return 0;
}
int multi_wave_s::findAngleMatch(float angle, int size) const {
for (int i = 0; i < size; i++) {
if (isSameF(switchTimes[i], angle))
return i;
}
return EFI_ERROR_CODE;
}
void multi_wave_s::setSwitchTime(int index, float value) { void multi_wave_s::setSwitchTime(int index, float value) {
switchTimes[index] = value; switchTimes[index] = value;
} }
TriggerState::TriggerState() { TriggerState::TriggerState() {
cycleCallback = NULL;
shaft_is_synchronized = FALSE;
toothed_previous_time = 0;
toothed_previous_duration = 0;
totalRevolutionCounter = 0;
clear(); clear();
totalEventCountBase = 0; totalEventCountBase = 0;
isFirstEvent = true; isFirstEvent = true;
@ -100,8 +127,11 @@ uint64_t TriggerState::getTotalEventCounter() {
return totalEventCountBase + current_index; return totalEventCountBase + current_index;
} }
void TriggerState::nextRevolution(int triggerEventCount) { void TriggerState::nextRevolution(int triggerEventCount, uint64_t nowUs) {
current_index = 0; if (cycleCallback != NULL) {
cycleCallback(this);
}
clear();
totalRevolutionCounter++; totalRevolutionCounter++;
totalEventCountBase += triggerEventCount; totalEventCountBase += triggerEventCount;
} }
@ -110,21 +140,29 @@ int TriggerState::getTotalRevolutionCounter() {
return totalRevolutionCounter; return totalRevolutionCounter;
} }
void TriggerState::nextTriggerEvent() { void TriggerState::nextTriggerEvent(trigger_wheel_e triggerWheel, uint64_t nowUs) {
uint64_t prevTime = timeOfPreviousEvent[triggerWheel];
if (prevTime != 0) {
totalTime[triggerWheel] += (nowUs - prevTime);
timeOfPreviousEvent[triggerWheel] = 0;
} else {
timeOfPreviousEvent[triggerWheel] = nowUs;
}
current_index++; current_index++;
} }
void TriggerState::clear() { void TriggerState::clear() {
shaft_is_synchronized = FALSE; memset(eventCount, 0, sizeof(eventCount));
toothed_previous_time = 0; memset(timeOfPreviousEvent, 0, sizeof(timeOfPreviousEvent));
toothed_previous_duration = 0; memset(totalTime, 0, sizeof(totalTime));
current_index = 0; current_index = 0;
totalRevolutionCounter = 0;
} }
float trigger_shape_s::getAngle(int index) const { float trigger_shape_s::getAngle(int index) const {
if (operationMode == FOUR_STROKE_CAM_SENSOR) if (operationMode == FOUR_STROKE_CAM_SENSOR) {
return switchAngles[index]; return getSwitchAngle(index);
}
/** /**
* FOUR_STROKE_CRANK_SENSOR magic: * FOUR_STROKE_CRANK_SENSOR magic:
* We have two crank shaft revolutions for each engine cycle * We have two crank shaft revolutions for each engine cycle
@ -134,20 +172,29 @@ float trigger_shape_s::getAngle(int index) const {
int triggerEventCounter = size; int triggerEventCounter = size;
if (index < triggerEventCounter) { if (index < triggerEventCounter) {
return switchAngles[index]; return getSwitchAngle(index);
} else { } else {
return 360 + switchAngles[index - triggerEventCounter]; return 360 + getSwitchAngle(index - triggerEventCounter);
} }
} }
void trigger_shape_s::addEvent(float angle, trigger_wheel_e waveIndex, trigger_value_e state) { void trigger_shape_s::addEvent(float angle, trigger_wheel_e const waveIndex, trigger_value_e const state) {
efiAssertVoid(operationMode != OM_NONE, "operationMode not set"); efiAssertVoid(operationMode != OM_NONE, "operationMode not set");
/** /**
* While '720' value works perfectly it has not much sense for crank sensor-only scenario. * While '720' value works perfectly it has not much sense for crank sensor-only scenario.
* todo: accept angle as a value in the 0..1 range? * todo: accept angle as a value in the 0..1 range?
*/ */
angle /= 720; angle /= 720;
efiAssertVoid(angle > previousAngle, "invalid angle order");
expectedEventCount[waveIndex]++;
efiAssertVoid(angle > 0, "angle should be positive");
if (size > 0) {
if (angle <= previousAngle) {
firmwareError("invalid angle order: %f and %f", angle, previousAngle);
return;
}
}
previousAngle = angle; previousAngle = angle;
if (size == 0) { if (size == 0) {
size = 1; size = 1;
@ -165,46 +212,203 @@ void trigger_shape_s::addEvent(float angle, trigger_wheel_e waveIndex, trigger_v
wave->pinStates[0] = initialState[i]; wave->pinStates[0] = initialState[i];
} }
setSwitchTime(0, angle); wave.setSwitchTime(0, angle);
wave.waves[waveIndex].pinStates[0] = state; wave.waves[waveIndex].pinStates[0] = state;
return; return;
} }
// if(angle!=trigger->wave.switchTimes[trigger->currentIndex]) int exactMatch = wave.findAngleMatch(angle, size);
if (exactMatch != EFI_ERROR_CODE) {
firmwareError("same angle: not supported");
return;
}
int index = size++; int index = wave.waveIndertionAngle(angle, size);
for (int i = 0; i < PWM_PHASE_MAX_WAVE_PER_PWM; i++) // shifting existing data
for (int i = size - 1; i >= index; i--) {
for (int j = 0; j < PWM_PHASE_MAX_WAVE_PER_PWM; j++) {
wave.waves[j].pinStates[i + 1] = wave.getChannelState(j, index);
}
wave.setSwitchTime(i + 1, wave.getSwitchTime(i));
}
// int index = size;
size++;
for (int i = 0; i < PWM_PHASE_MAX_WAVE_PER_PWM; i++) {
wave.waves[i].pinStates[index] = wave.getChannelState(i, index - 1); wave.waves[i].pinStates[index] = wave.getChannelState(i, index - 1);
setSwitchTime(index, angle); }
wave.setSwitchTime(index, angle);
wave.waves[waveIndex].pinStates[index] = state; wave.waves[waveIndex].pinStates[index] = state;
} }
void trigger_shape_s::setSwitchTime(int index, float angle) { int trigger_shape_s::getCycleDuration() const {
int cycleDuration = (operationMode == FOUR_STROKE_CAM_SENSOR) ? 720 : 360; return (operationMode == FOUR_STROKE_CAM_SENSOR) ? 720 : 360;
switchAngles[index] = cycleDuration * angle; }
wave.setSwitchTime(index, angle);
float trigger_shape_s::getSwitchAngle(int index) const {
return getCycleDuration() * wave.getSwitchTime(index);
} }
void multi_wave_s::checkSwitchTimes(int size) { void multi_wave_s::checkSwitchTimes(int size) {
checkSwitchTimes2(size, switchTimes); checkSwitchTimes2(size, switchTimes);
} }
void setToothedWheelConfiguration(trigger_shape_s *s, int total, int skipped, engine_configuration_s const *engineConfiguration) { void setToothedWheelConfiguration(trigger_shape_s *s, int total, int skipped,
engine_configuration_s const *engineConfiguration) {
s->isSynchronizationNeeded = (skipped != 0); s->isSynchronizationNeeded = (skipped != 0);
s->totalToothCount = total; s->totalToothCount = total;
s->skippedToothCount = skipped; s->skippedToothCount = skipped;
s->needSecondTriggerInput = false; s->needSecondTriggerInput = false;
s->useRiseEdge = TRUE; s->useRiseEdge = true;
initializeSkippedToothTriggerShapeExt(s, s->totalToothCount, initializeSkippedToothTriggerShapeExt(s, s->totalToothCount, s->skippedToothCount,
s->skippedToothCount,
getOperationMode(engineConfiguration)); getOperationMode(engineConfiguration));
} }
void setTriggerSynchronizationGap(trigger_shape_s *s, float synchGap) { void setTriggerSynchronizationGap(trigger_shape_s *s, float synchGap) {
s->isSynchronizationNeeded = TRUE; s->isSynchronizationNeeded = true;
s->syncRatioFrom = synchGap * 0.75; s->syncRatioFrom = synchGap * 0.75;
s->syncRatioTo = synchGap * 1.25; s->syncRatioTo = synchGap * 1.25;
} }
#define S24 (720.0f / 24 / 2)
static float addAccordPair(trigger_shape_s *s, float sb) {
s->addEvent(sb, T_SECONDARY, TV_HIGH);
sb += S24;
s->addEvent(sb, T_SECONDARY, TV_LOW);
sb += S24;
return sb;
}
#define DIP 7.5f
static float addAccordPair3(trigger_shape_s *s, float sb) {
sb += DIP;
s->addEvent(sb, T_CHANNEL_3, TV_HIGH);
sb += DIP;
s->addEvent(sb, T_CHANNEL_3, TV_LOW);
sb += 2 * DIP;
return sb;
}
/**
* Thank you Dip!
* http://forum.pgmfi.org/viewtopic.php?f=2&t=15570start=210#p139007
*/
void configureHondaAccordCDDip(trigger_shape_s *s) {
s->reset(FOUR_STROKE_CAM_SENSOR);
s->initialState[T_SECONDARY] = TV_HIGH;
float sb = 0;
sb = addAccordPair3(s, sb);
sb = addAccordPair3(s, sb);
sb = addAccordPair3(s, sb);
s->addEvent(90, T_SECONDARY, TV_LOW);
sb = 90;
sb = addAccordPair3(s, sb);
sb = addAccordPair3(s, sb);
sb = addAccordPair3(s, sb);
s->addEvent(180, T_SECONDARY, TV_HIGH);
sb = 180;
sb = addAccordPair3(s, sb);
sb = addAccordPair3(s, sb);
sb = addAccordPair3(s, sb);
s->addEvent(270, T_SECONDARY, TV_LOW);
sb = 270;
sb = addAccordPair3(s, sb);
sb = addAccordPair3(s, sb);
sb = addAccordPair3(s, sb);
s->addEvent(360.0f - DIP, T_PRIMARY, TV_HIGH);
s->addEvent(360, T_SECONDARY, TV_HIGH);
sb = 360;
sb = addAccordPair3(s, sb);
sb = addAccordPair3(s, sb);
sb = addAccordPair3(s, sb);
s->addEvent(450, T_SECONDARY, TV_LOW);
sb = 450;
sb = addAccordPair3(s, sb);
sb = addAccordPair3(s, sb);
sb = addAccordPair3(s, sb);
s->addEvent(540, T_SECONDARY, TV_HIGH);
sb = 540;
sb = addAccordPair3(s, sb);
sb = addAccordPair3(s, sb);
sb = addAccordPair3(s, sb);
s->addEvent(630, T_SECONDARY, TV_LOW);
sb = 630;
sb = addAccordPair3(s, sb);
sb = addAccordPair3(s, sb);
sb = addAccordPair3(s, sb);
s->addEvent(720.0f - DIP, T_PRIMARY, TV_LOW);
// s->addEvent(720.0f - 12 * sb, T_SECONDARY, TV_LOW);
// s->addEvent(720.0f, T_SECONDARY, TV_LOW);
s->addEvent(720.0f, T_SECONDARY, TV_HIGH);
s->isSynchronizationNeeded = false;
s->shaftPositionEventCount = s->getSize();
}
void configureHondaAccordCD(trigger_shape_s *s, bool with3rdSignal) {
s->reset(FOUR_STROKE_CAM_SENSOR);
float sb = 5.0f;
float tdcWidth = 0.1854 * 720 / 4;
s->isSynchronizationNeeded = false;
sb = addAccordPair(s, sb);
if (with3rdSignal)
s->addEvent(sb - S24 / 2, T_CHANNEL_3, TV_HIGH);
sb = addAccordPair(s, sb);
sb = addAccordPair(s, sb);
if (with3rdSignal)
s->addEvent(sb - S24 / 2, T_CHANNEL_3, TV_LOW);
sb = addAccordPair(s, sb);
sb = addAccordPair(s, sb);
s->addEvent(1 * 180.0f - tdcWidth, T_PRIMARY, TV_HIGH);
sb = addAccordPair(s, sb);
s->addEvent(1 * 180.0f, T_PRIMARY, TV_LOW);
sb = addAccordPair(s, sb);
sb = addAccordPair(s, sb);
sb = addAccordPair(s, sb);
sb = addAccordPair(s, sb);
sb = addAccordPair(s, sb);
s->addEvent(2 * 180.0f - tdcWidth, T_PRIMARY, TV_HIGH);
sb = addAccordPair(s, sb);
s->addEvent(2 * 180.0f, T_PRIMARY, TV_LOW);
for (int i = 3; i <= 4; i++) {
sb = addAccordPair(s, sb);
sb = addAccordPair(s, sb);
sb = addAccordPair(s, sb);
sb = addAccordPair(s, sb);
sb = addAccordPair(s, sb);
s->addEvent(i * 180.0f - tdcWidth, T_PRIMARY, TV_HIGH);
sb = addAccordPair(s, sb);
s->addEvent(i * 180.0f, T_PRIMARY, TV_LOW);
}
s->shaftPositionEventCount = s->getSize();
}

View File

@ -16,61 +16,19 @@
class trigger_shape_s; class trigger_shape_s;
class TriggerState { #define TRIGGER_CHANNEL_COUNT 3
public:
TriggerState();
int getCurrentIndex();
int getTotalRevolutionCounter();
uint64_t getTotalEventCounter();
uint64_t getStartOfRevolutionIndex();
void nextRevolution(int triggerEventCount);
void nextTriggerEvent();
void decodeTriggerEvent(trigger_shape_s const*triggerShape, trigger_config_s const*triggerConfig, trigger_event_e signal, uint64_t nowUs);
/**
* TRUE if we know where we are
*/
unsigned char shaft_is_synchronized;
uint64_t toothed_previous_duration;
uint64_t toothed_previous_time;
private:
void clear();
/**
* index within trigger revolution, from 0 to trigger event count
*/
int current_index;
uint64_t totalEventCountBase;
int totalRevolutionCounter;
bool isFirstEvent;
};
typedef enum {
TV_LOW = 0,
TV_HIGH = 1
} trigger_value_e;
typedef enum {
T_PRIMARY = 0,
T_SECONDARY = 1
} trigger_wheel_e;
class trigger_shape_helper { class trigger_shape_helper {
int pinStates0[PWM_PHASE_MAX_COUNT]; int pinStates[TRIGGER_CHANNEL_COUNT][PWM_PHASE_MAX_COUNT];
int pinStates1[PWM_PHASE_MAX_COUNT];
public: public:
trigger_shape_helper(); trigger_shape_helper();
single_wave_s waves[2]; single_wave_s waves[TRIGGER_CHANNEL_COUNT];
}; };
class trigger_shape_s { class trigger_shape_s {
private:
void setSwitchTime(int index, float angle);
trigger_shape_helper h;
int size;
public: public:
trigger_shape_s();
int isSynchronizationNeeded; int isSynchronizationNeeded;
int totalToothCount; int totalToothCount;
@ -81,14 +39,15 @@ public:
int useRiseEdge; int useRiseEdge;
/**
* This is used for signal validation
*/
int expectedEventCount[PWM_PHASE_MAX_WAVE_PER_PWM];
bool needSecondTriggerInput; bool needSecondTriggerInput;
void addEvent(float angle, trigger_wheel_e const waveIndex, trigger_value_e const state);
trigger_shape_s();
void addEvent(float angle, trigger_wheel_e waveIndex, trigger_value_e state);
float getAngle(int phaseIndex) const;
void reset(operation_mode_e operationMode); void reset(operation_mode_e operationMode);
int getSize(); int getSize() const;
multi_wave_s wave; multi_wave_s wave;
/** /**
@ -102,16 +61,20 @@ public:
// tood: maybe even automate this flag calculation? // tood: maybe even automate this flag calculation?
int initialState[PWM_PHASE_MAX_WAVE_PER_PWM]; int initialState[PWM_PHASE_MAX_WAVE_PER_PWM];
int getTriggerShapeSynchPointIndex(); int getTriggerShapeSynchPointIndex();
void calculateTriggerSynchPoint(trigger_config_s const*triggerConfig);
void setTriggerShapeSynchPointIndex(int triggerShapeSynchPointIndex); void setTriggerShapeSynchPointIndex(int triggerShapeSynchPointIndex);
/** /**
* These angles are in event coordinates - with synchronization point located at angle zero. * These angles are in event coordinates - with synchronization point located at angle zero.
* These values are pre-calculated for performance reasons.
*/ */
float eventAngles[PWM_PHASE_MAX_COUNT]; float eventAngles[PWM_PHASE_MAX_COUNT];
private: private:
trigger_shape_helper h;
int size;
/** /**
* index of synchronization event within trigger_shape_s * index of synchronization event within trigger_shape_s
* See findTriggerZeroEventIndex() * See findTriggerZeroEventIndex()
@ -120,20 +83,25 @@ private:
/** /**
* Values are in the 0..1 range * Values are in the 0..1 range
*/ */
float switchTimes[PWM_PHASE_MAX_COUNT]; float switchTimesBuffer[PWM_PHASE_MAX_COUNT];
/** /**
* These are the same values as in switchTimes, but these are angles in the 0..360 or 0..720 range.
* That's a performance optimization - this should save as one multiplication in a critical spot.
*
* These angles are in trigger DESCRIPTION coordinates - i.e. the way you add events while declaring trigger shape * These angles are in trigger DESCRIPTION coordinates - i.e. the way you add events while declaring trigger shape
*/ */
float switchAngles[PWM_PHASE_MAX_COUNT]; float getSwitchAngle(int index) const;
float previousAngle; float previousAngle;
/** /**
* this is part of performance optimization * this is part of performance optimization
*/ */
operation_mode_e operationMode; operation_mode_e operationMode;
/**
* This private method should only be used to prepare the array of pre-calculated values
* See eventAngles array
*/
float getAngle(int phaseIndex) const;
int getCycleDuration() const;
}; };
void setTriggerSynchronizationGap(trigger_shape_s *s, float synchGap); void setTriggerSynchronizationGap(trigger_shape_s *s, float synchGap);

View File

@ -11,7 +11,7 @@
#include "status_loop.h" #include "status_loop.h"
#include "engine_configuration.h" #include "engine_configuration.h"
#if EFI_ANALOG_CHART #if EFI_ANALOG_CHART || defined(__DOXYGEN__)
static char LOGGING_BUFFER[5000]; static char LOGGING_BUFFER[5000];
static Logging logging; static Logging logging;
@ -22,8 +22,9 @@ static int initialized = FALSE;
extern engine_configuration_s *engineConfiguration; extern engine_configuration_s *engineConfiguration;
void acAddData(float angle, float value) { void acAddData(float angle, float value) {
if (!initialized) if (!initialized) {
return; // this is possible because of initialization sequence return; // this is possible because of initialization sequence
}
if( engineConfiguration->analogChartFrequency < 1) { if( engineConfiguration->analogChartFrequency < 1) {
//todofirmwareError() //todofirmwareError()
@ -35,8 +36,9 @@ void acAddData(float angle, float value) {
// message terminator // message terminator
appendPrintf(&logging, DELIMETER); appendPrintf(&logging, DELIMETER);
// output pending data // output pending data
if (getFullLog()) if (getFullLog()) {
scheduleLogging(&logging); scheduleLogging(&logging);
}
pendingData = FALSE; pendingData = FALSE;
} }
return; return;
@ -48,9 +50,10 @@ void acAddData(float angle, float value) {
appendPrintf(&logging, "analog_chart%s", DELIMETER); appendPrintf(&logging, "analog_chart%s", DELIMETER);
} }
if (remainingSize(&logging) > 100) if (remainingSize(&logging) > 100) {
appendPrintf(&logging, "%f|%f|", angle, value); appendPrintf(&logging, "%f|%f|", angle, value);
} }
}
void initAnalogChart(void) { void initAnalogChart(void) {
initLoggingExt(&logging, "analog chart", LOGGING_BUFFER, sizeof(LOGGING_BUFFER)); initLoggingExt(&logging, "analog chart", LOGGING_BUFFER, sizeof(LOGGING_BUFFER));

View File

@ -2,10 +2,10 @@
EMULATIONSRC = emulation/hw_layer/poten.c \ EMULATIONSRC = emulation/hw_layer/poten.c \
emulation/analog_chart.c \ emulation/analog_chart.c \
emulation/test/test.c \ emulation/test/test.c \
emulation/test/testbmk.c \ emulation/test/testbmk.c
emulation/wave_analyzer.c
EMULATIONSRC_CPP = emulation/trigger_emulator.cpp \ EMULATIONSRC_CPP = emulation/trigger_emulator.cpp \
emulation/rfi_perftest.cpp \ emulation/rfi_perftest.cpp \
emulation/engine_emulator.cpp emulation/engine_emulator.cpp \
emulation/wave_analyzer.cpp

View File

@ -38,16 +38,19 @@
SPIDriver * getDigiralPotDevice(spi_device_e spiDevice) { SPIDriver * getDigiralPotDevice(spi_device_e spiDevice) {
#if STM32_SPI_USE_SPI1 || defined(__DOXYGEN__) #if STM32_SPI_USE_SPI1 || defined(__DOXYGEN__)
if (spiDevice == SPI_DEVICE_1) if (spiDevice == SPI_DEVICE_1) {
return &SPID1; return &SPID1;
}
#endif #endif
#if STM32_SPI_USE_SPI2 || defined(__DOXYGEN__) #if STM32_SPI_USE_SPI2 || defined(__DOXYGEN__)
if (spiDevic e== SPI_DEVICE_2) if (spiDevic e== SPI_DEVICE_2) {
return &SPID2; return &SPID2;
}
#endif #endif
#if STM32_SPI_USE_SPI3 || defined(__DOXYGEN__) #if STM32_SPI_USE_SPI3 || defined(__DOXYGEN__)
if (spiDevice == SPI_DEVICE_3) if (spiDevice == SPI_DEVICE_3) {
return &SPID3; return &SPID3;
}
#endif #endif
firmwareError("Unexpected SPI device: %d", spiDevice); firmwareError("Unexpected SPI device: %d", spiDevice);
return NULL; return NULL;
@ -58,7 +61,7 @@ SPIDriver * getDigiralPotDevice(spi_device_e spiDevice) {
static Logging logger; static Logging logger;
#if EFI_POTENTIOMETER #if EFI_POTENTIOMETER || defined(__DOXYGEN__)
Mcp42010Driver config[DIGIPOT_COUNT]; Mcp42010Driver config[DIGIPOT_COUNT];
void initPotentiometer(Mcp42010Driver *driver, SPIDriver *spi, ioportid_t port, ioportmask_t pin) { void initPotentiometer(Mcp42010Driver *driver, SPIDriver *spi, ioportid_t port, ioportmask_t pin) {
@ -102,12 +105,8 @@ void setPotResistance(Mcp42010Driver *driver, int channel, int resistance) {
sendToPot(driver, channel, value); sendToPot(driver, channel, value);
} }
static void setPotResistance0(int value) { static void setPotResistanceCommand(int index, int value) {
setPotResistance(&config[0], 0, value); setPotResistance(&config[index / 2], index % 2, value);
}
static void setPotResistance1(int value) {
setPotResistance(&config[0], 1, value);
} }
static void setPotValue1(int value) { static void setPotValue1(int value) {
@ -127,20 +126,20 @@ void initPotentiometers(board_configuration_s *boardConfiguration) {
for (int i = 0; i < DIGIPOT_COUNT; i++) { for (int i = 0; i < DIGIPOT_COUNT; i++) {
brain_pin_e csPin = boardConfiguration->digitalPotentiometerChipSelect[i]; brain_pin_e csPin = boardConfiguration->digitalPotentiometerChipSelect[i];
if (csPin == GPIO_NONE) if (csPin == GPIO_NONE) {
continue; continue;
}
initPotentiometer(&config[i], getDigiralPotDevice(boardConfiguration->digitalPotentiometerSpiDevice), initPotentiometer(&config[i], getDigiralPotDevice(boardConfiguration->digitalPotentiometerSpiDevice),
getHwPort(csPin), getHwPin(csPin)); getHwPort(csPin), getHwPin(csPin));
} }
addConsoleActionI("pot0", setPotResistance0); addConsoleActionII("pot", setPotResistanceCommand);
addConsoleActionI("pot1", setPotResistance1);
addConsoleActionI("potd1", setPotValue1); addConsoleActionI("potd1", setPotValue1);
setPotResistance0(3000); setPotResistance(&config[0], 0, 3000);
setPotResistance1(7000); setPotResistance(&config[0], 1, 7000);
#else #else
print("digiPot logic disabled\r\n"); print("digiPot logic disabled\r\n");
#endif #endif

View File

@ -18,7 +18,7 @@
#include "console_io.h" #include "console_io.h"
#include "engine.h" #include "engine.h"
#if EFI_PERF_METRICS #if EFI_PERF_METRICS || defined(__DOXYGEN__)
static Logging logger; static Logging logger;

View File

@ -1,5 +1,5 @@
/** /**
* @file trigger_emulator.c * @file trigger_emulator.cpp
* @brief Position sensor(s) emulation code * @brief Position sensor(s) emulation code
* *
* This file is mostly about initialization, the configuration is * This file is mostly about initialization, the configuration is
@ -14,30 +14,81 @@
#include "main_trigger_callback.h" #include "main_trigger_callback.h"
#include "datalogging.h" #include "datalogging.h"
#include "engine_configuration.h" #include "engine_configuration.h"
#if EFI_PROD_CODE
#include "pwm_generator.h" #include "pwm_generator.h"
#include "io_pins.h"
#include "pin_repository.h" #include "pin_repository.h"
#endif
#include "io_pins.h"
#include "trigger_emulator_algo.h" #include "trigger_emulator_algo.h"
#include "trigger_central.h"
extern engine_configuration_s *engineConfiguration; extern engine_configuration_s *engineConfiguration;
extern board_configuration_s *boardConfiguration; extern board_configuration_s *boardConfiguration;
extern PwmConfig triggerSignal; extern PwmConfig triggerSignal;
TriggerEmulatorHelper::TriggerEmulatorHelper() {
primaryWheelState = false;
secondaryWheelState = false;
thirdWheelState = false;
}
void TriggerEmulatorHelper::handleEmulatorCallback(PwmConfig *state, int stateIndex) {
int newPrimaryWheelState = state->multiWave.waves[0].pinStates[stateIndex];
int newSecondaryWheelState = state->multiWave.waves[1].pinStates[stateIndex];
int new3rdWheelState = state->multiWave.waves[2].pinStates[stateIndex];
if (primaryWheelState != newPrimaryWheelState) {
primaryWheelState = newPrimaryWheelState;
hwHandleShaftSignal(primaryWheelState ? SHAFT_PRIMARY_UP : SHAFT_PRIMARY_DOWN);
}
if (secondaryWheelState != newSecondaryWheelState) {
secondaryWheelState = newSecondaryWheelState;
hwHandleShaftSignal(secondaryWheelState ? SHAFT_SECONDARY_UP : SHAFT_SECONDARY_DOWN);
}
if (thirdWheelState != new3rdWheelState) {
thirdWheelState = new3rdWheelState;
hwHandleShaftSignal(thirdWheelState ? SHAFT_3RD_UP : SHAFT_3RD_DOWN);
}
// print("hello %d\r\n", chTimeNow());
}
static TriggerEmulatorHelper helper;
#if EFI_EMULATE_POSITION_SENSORS || defined(__DOXYGEN__)
static void emulatorApplyPinState(PwmConfig *state, int stateIndex) {
if (engineConfiguration->directSelfStimulation) {
helper.handleEmulatorCallback(state, stateIndex);
}
applyPinState(state, stateIndex);
}
#endif /* EFI_EMULATE_POSITION_SENSORS */
void initTriggerEmulator(void) { void initTriggerEmulator(void) {
#if EFI_EMULATE_POSITION_SENSORS #if EFI_EMULATE_POSITION_SENSORS || defined(__DOXYGEN__)
print("Emulating %s\r\n", getConfigurationName(engineConfiguration)); print("Emulating %s\r\n", getConfigurationName(engineConfiguration));
triggerSignal.outputPins[0] = TRIGGER_EMULATOR_PRIMARY; triggerSignal.outputPins[0] = TRIGGER_EMULATOR_PRIMARY;
triggerSignal.outputPins[1] = TRIGGER_EMULATOR_SECONDARY; triggerSignal.outputPins[1] = TRIGGER_EMULATOR_SECONDARY;
triggerSignal.outputPins[2] = TRIGGER_EMULATOR_3RD;
// todo: refactor, make this a loop
outputPinRegisterExt2("distributor ch1", triggerSignal.outputPins[0], boardConfiguration->triggerSimulatorPins[0], outputPinRegisterExt2("distributor ch1", triggerSignal.outputPins[0], boardConfiguration->triggerSimulatorPins[0],
&boardConfiguration->triggerSimulatorPinModes[0]); &boardConfiguration->triggerSimulatorPinModes[0]);
outputPinRegisterExt2("distributor ch2", triggerSignal.outputPins[1], boardConfiguration->triggerSimulatorPins[1], outputPinRegisterExt2("distributor ch2", triggerSignal.outputPins[1], boardConfiguration->triggerSimulatorPins[1],
&boardConfiguration->triggerSimulatorPinModes[1]); &boardConfiguration->triggerSimulatorPinModes[1]);
initTriggerEmulatorLogic(applyPinState); outputPinRegisterExt2("distributor ch3", triggerSignal.outputPins[2], boardConfiguration->triggerSimulatorPins[2],
&boardConfiguration->triggerSimulatorPinModes[2]);
initTriggerEmulatorLogic(emulatorApplyPinState);
#else #else
print("No position sensor(s) emulation\r\n"); print("No position sensor(s) emulation\r\n");
#endif /* EFI_EMULATE_POSITION_SENSORS */ #endif /* EFI_EMULATE_POSITION_SENSORS */

View File

@ -10,23 +10,25 @@
#define DIST_EMULATOR_H_ #define DIST_EMULATOR_H_
#include "main.h" #include "main.h"
#include "wave_math.h"
#include "trigger_structure.h" #include "trigger_structure.h"
#include "engine_configuration.h" #include "engine_configuration.h"
#include "pwm_generator_logic.h"
#ifdef __cplusplus class TriggerEmulatorHelper {
extern "C" public:
{ bool primaryWheelState;
#endif /* __cplusplus */ bool secondaryWheelState;
bool thirdWheelState;
TriggerEmulatorHelper();
void handleEmulatorCallback(PwmConfig *state, int stateIndex);
};
void initTriggerEmulator(void); void initTriggerEmulator(void);
#ifdef __cplusplus
}
#endif /* __cplusplus */
void setTriggerEmulatorRPM(int value); void setTriggerEmulatorRPM(int value);
#endif /* DIST_EMULATOR_H_ */ #endif /* DIST_EMULATOR_H_ */

View File

@ -1,5 +1,5 @@
/** /**
* @file wave_analyzer.c * @file wave_analyzer.cpp
* @brief Initialization of Input Capture pins used for dev console sniffer * @brief Initialization of Input Capture pins used for dev console sniffer
* *
* This file is responsible for sniffing of external digital signals and registering * This file is responsible for sniffing of external digital signals and registering
@ -20,7 +20,10 @@
#include "engine_configuration.h" #include "engine_configuration.h"
#include "trigger_central.h" #include "trigger_central.h"
#include "rfiutil.h" #include "rfiutil.h"
#include "engine_math.h"
#include "engine.h"
extern Engine engine;
#define CHART_RESET_DELAY 1 #define CHART_RESET_DELAY 1
@ -43,7 +46,7 @@ static void ensureInitialized(WaveReader *reader) {
efiAssertVoid(reader->hw.started, "wave analyzer NOT INITIALIZED"); efiAssertVoid(reader->hw.started, "wave analyzer NOT INITIALIZED");
} }
#ifdef EFI_WAVE_ANALYZER #if EFI_WAVE_ANALYZER || defined(__DOXYGEN__)
static void waAnaWidthCallback(WaveReader *reader) { static void waAnaWidthCallback(WaveReader *reader) {
uint64_t nowUs = getTimeNowUs(); uint64_t nowUs = getTimeNowUs();
@ -51,7 +54,7 @@ static void waAnaWidthCallback(WaveReader *reader) {
reader->lastActivityTimeUs = nowUs; reader->lastActivityTimeUs = nowUs;
addWaveChartEvent(reader->name, WC_UP, ""); addWaveChartEvent(reader->name, WC_UP, "");
uint64_t width = nowUs - reader->periodEventTimeUs; uint32_t width = nowUs - reader->periodEventTimeUs;
reader->last_wave_low_widthUs = width; reader->last_wave_low_widthUs = width;
reader->signalPeriodUs = nowUs - reader->widthEventTimeUs; reader->signalPeriodUs = nowUs - reader->widthEventTimeUs;
@ -73,9 +76,9 @@ static void waIcuPeriodCallback(WaveReader *reader) {
// dbAdd(&wavePeriodTime, now); // dbAdd(&wavePeriodTime, now);
int period = ckpPeriodUs; // local copy of volatile variable uint32_t period = ckpPeriodUs; // local copy of volatile variable
uint64_t offset = nowUs - previousCrankSignalStart; uint32_t offset = nowUs - previousCrankSignalStart;
if (offset > period / 2) { if (offset > period / 2) {
/** /**
@ -95,10 +98,10 @@ static void setWaveModeSilent(int index, int mode) {
setWaveReaderMode(&reader->hw, mode); setWaveReaderMode(&reader->hw, mode);
} }
void setWaveMode(int index, int mode) { //void setWaveMode(int index, int mode) {
setWaveModeSilent(index, mode); // setWaveModeSilent(index, mode);
print("wavemode%d:%d\r\n", index, mode); // print("wavemode%d:%d\r\n", index, mode);
} //}
int getWaveMode(int index) { int getWaveMode(int index) {
WaveReader *reader = &readers[index]; WaveReader *reader = &readers[index];
@ -112,7 +115,7 @@ int getEventCounter(int index) {
return reader->eventCounter; return reader->eventCounter;
} }
static void initWave(char *name, int index, ICUDriver *driver, ioportid_t port, int pin, int mode) { static void initWave(const char *name, int index, ICUDriver *driver, ioportid_t port, ioportmask_t pin, int mode) {
waveReaderCount++; waveReaderCount++;
efiAssertVoid(index < MAX_ICU_COUNT, "too many ICUs"); efiAssertVoid(index < MAX_ICU_COUNT, "too many ICUs");
WaveReader *reader = &readers[index]; WaveReader *reader = &readers[index];
@ -120,9 +123,9 @@ static void initWave(char *name, int index, ICUDriver *driver, ioportid_t port,
reader->name = name; reader->name = name;
registerCallback(&hw->widthListeners, (IntListener) waAnaWidthCallback, reader); registerCallback(&hw->widthListeners, (IntListener) waAnaWidthCallback, (void*)reader);
registerCallback(&hw->periodListeners, (IntListener) waIcuPeriodCallback, reader); registerCallback(&hw->periodListeners, (IntListener) waIcuPeriodCallback, (void*)reader);
initWaveAnalyzerDriver(hw, driver, port, pin); initWaveAnalyzerDriver(hw, driver, port, pin);
@ -140,8 +143,9 @@ static void initWave(char *name, int index, ICUDriver *driver, ioportid_t port,
//} //}
static void onWaveShaftSignal(trigger_event_e ckpSignalType, int index, void *arg) { static void onWaveShaftSignal(trigger_event_e ckpSignalType, int index, void *arg) {
if (index != 0) if (index != 0) {
return; return;
}
uint64_t nowUs = getTimeNowUs(); uint64_t nowUs = getTimeNowUs();
ckpPeriodUs = nowUs - previousCrankSignalStart; ckpPeriodUs = nowUs - previousCrankSignalStart;
previousCrankSignalStart = nowUs; previousCrankSignalStart = nowUs;
@ -165,7 +169,7 @@ static msg_t waThread(void *arg) {
#endif #endif
} }
int getWaveLowWidth(int index) { uint32_t getWaveLowWidth(int index) {
WaveReader *reader = &readers[index]; WaveReader *reader = &readers[index];
ensureInitialized(reader); ensureInitialized(reader);
return reader->last_wave_low_widthUs; return reader->last_wave_low_widthUs;
@ -174,9 +178,10 @@ int getWaveLowWidth(int index) {
float getWaveHighWidthMs(int index) { float getWaveHighWidthMs(int index) {
WaveReader *reader = &readers[index]; WaveReader *reader = &readers[index];
ensureInitialized(reader); ensureInitialized(reader);
if (getTimeNowUs() - reader->lastActivityTimeUs > 4 * US_PER_SECOND) if (getTimeNowUs() - reader->lastActivityTimeUs > 4 * US_PER_SECOND) {
return 0; // dwell time has expired return 0.0f; // dwell time has expired
return reader->last_wave_high_widthUs / 1000.0; }
return reader->last_wave_high_widthUs / 1000.0f;
} }
uint64_t getWaveOffset(int index) { uint64_t getWaveOffset(int index) {
@ -188,10 +193,10 @@ uint64_t getWaveOffset(int index) {
float getSignalPeriodMs(int index) { float getSignalPeriodMs(int index) {
WaveReader *reader = &readers[index]; WaveReader *reader = &readers[index];
ensureInitialized(reader); ensureInitialized(reader);
return reader->signalPeriodUs / 1000.0; return reader->signalPeriodUs / 1000.0f;
} }
int getWidthEventTime(int index) { uint64_t getWidthEventTime(int index) {
WaveReader *reader = &readers[index]; WaveReader *reader = &readers[index];
ensureInitialized(reader); ensureInitialized(reader);
return reader->widthEventTimeUs; return reader->widthEventTimeUs;
@ -213,7 +218,7 @@ static void reportWave(Logging *logging, int index) {
float periodMs = getSignalPeriodMs(index); float periodMs = getSignalPeriodMs(index);
appendPrintf(logging, "duty%d%s", index, DELIMETER); appendPrintf(logging, "duty%d%s", index, DELIMETER);
appendFloat(logging, 100.0 * dwellMs / periodMs, 2); appendFloat(logging, 100.0f * dwellMs / periodMs, 2);
appendPrintf(logging, "%s", DELIMETER); appendPrintf(logging, "%s", DELIMETER);
appendPrintf(logging, "dwell%d%s", index, DELIMETER); appendPrintf(logging, "dwell%d%s", index, DELIMETER);
@ -224,11 +229,12 @@ static void reportWave(Logging *logging, int index) {
appendFloat(logging, periodMs, 2); appendFloat(logging, periodMs, 2);
appendPrintf(logging, "%s", DELIMETER); appendPrintf(logging, "%s", DELIMETER);
// int crank = getCrankPeriod(); int offset = getWaveOffset(index);
int crank = getOneDegreeTimeMs(getRpm()) * 360;
// int offset = getWaveOffset(index); appendPrintf(logging, "advance%d%s", index, DELIMETER);
// debugFloat2(logging, "advance", index, 90.0 * offset / crank, 3); appendFloat(logging, 90.0 * offset / crank, 3);
// debugInt2(logging, "offset", index, offset); appendPrintf(logging, "%s", DELIMETER);
} }
void printWave(Logging *logging) { void printWave(Logging *logging) {
@ -237,18 +243,18 @@ void printWave(Logging *logging) {
} }
void initWaveAnalyzer(void) { void initWaveAnalyzer(void) {
#ifdef EFI_WAVE_ANALYZER #if EFI_WAVE_ANALYZER || defined(__DOXYGEN__)
initLogging(&logger, "wave"); initLogging(&logger, "wave");
initWave("input1 A8", 0, getInputCaptureDriver(boardConfiguration->primaryLogicAnalyzerPin), getHwPort(boardConfiguration->primaryLogicAnalyzerPin), getHwPin(boardConfiguration->primaryLogicAnalyzerPin), 1); initWave(WA_CHANNEL_1, 0, getInputCaptureDriver(boardConfiguration->logicAnalyzerPins[0]), getHwPort(boardConfiguration->logicAnalyzerPins[0]), getHwPin(boardConfiguration->logicAnalyzerPins[0]), 1);
initWave("input2 E5", 1, getInputCaptureDriver(boardConfiguration->secondaryLogicAnalyzerPin), getHwPort(boardConfiguration->secondaryLogicAnalyzerPin), getHwPin(boardConfiguration->secondaryLogicAnalyzerPin), 1); initWave(WA_CHANNEL_2, 1, getInputCaptureDriver(boardConfiguration->logicAnalyzerPins[1]), getHwPort(boardConfiguration->logicAnalyzerPins[1]), getHwPin(boardConfiguration->logicAnalyzerPins[1]), 1);
// initWave("input0 C6", 2, &WAVE_TIMER, WAVE_INPUT_PORT, WAVE_INPUT_PIN, 0); // initWave("input0 C6", 2, &WAVE_TIMER, WAVE_INPUT_PORT, WAVE_INPUT_PIN, 0);
addTriggerEventListener(&onWaveShaftSignal, "wave analyzer", NULL); addTriggerEventListener(&onWaveShaftSignal, "wave analyzer", (void*)NULL);
addConsoleActionII("wm", setWaveModeSilent); addConsoleActionII("wm", setWaveModeSilent);
chThdCreateStatic(waThreadStack, sizeof(waThreadStack), NORMALPRIO, waThread, NULL); chThdCreateStatic(waThreadStack, sizeof(waThreadStack), NORMALPRIO, waThread, (void*)NULL);
#else #else
print("wave disabled\r\n"); print("wave disabled\r\n");

View File

@ -15,19 +15,22 @@
#include "wave_analyzer_hw.h" #include "wave_analyzer_hw.h"
#include "wave_chart.h" #include "wave_chart.h"
#define WA_CHANNEL_1 "input1"
#define WA_CHANNEL_2 "input2"
typedef struct { typedef struct {
WaveReaderHw hw; WaveReaderHw hw;
char *name; const char *name;
volatile int eventCounter; volatile int eventCounter;
volatile uint64_t lastActivityTimeUs; // timestamp in microseconds ticks volatile uint64_t lastActivityTimeUs; // timestamp in microseconds ticks
volatile uint64_t periodEventTimeUs; // time of signal fall in microseconds volatile uint64_t periodEventTimeUs; // time of signal fall in microseconds
volatile uint64_t widthEventTimeUs; // time of signal rise in microseconds volatile uint64_t widthEventTimeUs; // time of signal rise in microseconds
volatile uint64_t signalPeriodUs; // period between two signal rises in microseconds volatile uint32_t signalPeriodUs; // period between two signal rises in microseconds
volatile uint64_t waveOffsetUs; // offset from CKP signal in systimer ticks volatile uint64_t waveOffsetUs; // offset from CKP signal in systimer ticks
volatile uint64_t last_wave_low_widthUs; // time period in systimer ticks volatile uint32_t last_wave_low_widthUs; // time period in systimer ticks
volatile uint64_t last_wave_high_widthUs; // time period in systimer ticks volatile uint64_t last_wave_high_widthUs; // time period in systimer ticks
} WaveReader; } WaveReader;
@ -39,7 +42,7 @@ extern "C"
void initWaveAnalyzer(void); void initWaveAnalyzer(void);
void pokeWaveInfo(void); void pokeWaveInfo(void);
void reportWaveInfo(void); void reportWaveInfo(void);
int getWaveLowWidth(int index); uint32_t getWaveLowWidth(int index);
float getWaveHighWidthMs(int index); float getWaveHighWidthMs(int index);
uint64_t getWaveOffset(int index); uint64_t getWaveOffset(int index);
@ -48,7 +51,7 @@ int getWaveMode(int index);
int getEventCounter(int index); int getEventCounter(int index);
float getSignalPeriodMs(int index); float getSignalPeriodMs(int index);
int getWidthEventTime(int index); uint64_t getWidthEventTime(int index);
uint64_t getPeriodEventTime(int index); uint64_t getPeriodEventTime(int index);
//int getCrankStart(); //int getCrankStart();

View File

@ -10,9 +10,15 @@
#include <ch.h> #include <ch.h>
#include <hal.h> #include <hal.h>
#include <time.h>
#include <string.h> #include <string.h>
// this is about MISRA not liking 'time.h'. todo: figure out something
#if defined __GNUC__
#include <sys/types.h>
#else
typedef unsigned int time_t;
#endif
#include "efifeatures.h" #include "efifeatures.h"
#include "rusefi_enums.h" #include "rusefi_enums.h"
#include "obd_error_codes.h" #include "obd_error_codes.h"
@ -27,6 +33,8 @@
// see also PORT_INT_REQUIRED_STACK // see also PORT_INT_REQUIRED_STACK
#define UTILITY_THREAD_STACK_SIZE 128 #define UTILITY_THREAD_STACK_SIZE 128
#define EFI_ERROR_CODE 0xffffffff
#if EFI_USE_CCM && defined __GNUC__ #if EFI_USE_CCM && defined __GNUC__
#define CCM_OPTIONAL __attribute__((section(".ccm"))); #define CCM_OPTIONAL __attribute__((section(".ccm")));
#else #else

View File

@ -10,13 +10,17 @@
class AdcConfiguration { class AdcConfiguration {
public: public:
AdcConfiguration(ADCConversionGroup* hwConfig); AdcConfiguration(ADCConversionGroup* hwConfig);
void addChannel(int hwChannelIndex); void addChannel(adc_channel_e hwChannelIndex);
int getAdcHardwareIndexByInternalIndex(int index); adc_channel_e getAdcHardwareIndexByInternalIndex(int index);
int internalAdcIndexByHardwareIndex[20]; int internalAdcIndexByHardwareIndex[20];
bool isHwUsed(adc_channel_e hwChannel);
int size(); int size();
void init(void); void init(void);
int conversionCount; int conversionCount;
int errorsCount; int errorsCount;
int getAdcValueByIndex(int internalIndex);
adc_state values;
private: private:
ADCConversionGroup* hwConfig; ADCConversionGroup* hwConfig;
/** /**
@ -24,7 +28,7 @@ private:
*/ */
int channelCount; int channelCount;
int hardwareIndexByIndernalAdcIndex[20]; adc_channel_e hardwareIndexByIndernalAdcIndex[20];
}; };

View File

@ -26,6 +26,7 @@ AdcConfiguration::AdcConfiguration(ADCConversionGroup* hwConfig) {
hwConfig->sqr1 = 0; hwConfig->sqr1 = 0;
hwConfig->sqr2 = 0; hwConfig->sqr2 = 0;
hwConfig->sqr3 = 0; hwConfig->sqr3 = 0;
memset(internalAdcIndexByHardwareIndex, 0xFFFFFFFF, sizeof(internalAdcIndexByHardwareIndex));
} }
#define ADC_GRP1_BUF_DEPTH_FAST 1 #define ADC_GRP1_BUF_DEPTH_FAST 1
@ -80,8 +81,6 @@ static adcsample_t getAvgAdcValue(int index, adcsample_t *samples, int bufDepth,
return result / bufDepth; return result / bufDepth;
} }
static adc_state newState;
static void adc_callback_slow(ADCDriver *adcp, adcsample_t *buffer, size_t n); static void adc_callback_slow(ADCDriver *adcp, adcsample_t *buffer, size_t n);
static void adc_callback_fast(ADCDriver *adcp, adcsample_t *buffer, size_t n); static void adc_callback_fast(ADCDriver *adcp, adcsample_t *buffer, size_t n);
@ -147,7 +146,7 @@ ADC_TwoSamplingDelay_5Cycles, // cr1
// Conversion group sequence 1...6 // Conversion group sequence 1...6
}; };
static AdcConfiguration fastAdc(&adcgrpcfg_fast); AdcConfiguration fastAdc(&adcgrpcfg_fast);
static void pwmpcb_slow(PWMDriver *pwmp) { static void pwmpcb_slow(PWMDriver *pwmp) {
#if EFI_INTERNAL_ADC #if EFI_INTERNAL_ADC
@ -202,36 +201,71 @@ static void pwmpcb_fast(PWMDriver *pwmp) {
#endif #endif
} }
int getAdcValueByIndex(int internalIndex) { int getInternalAdcValue(adc_channel_e hwChannel) {
return newState.adc_data[internalIndex];
}
int getInternalAdcValue(int hwChannel) {
if (boardConfiguration->adcHwChannelEnabled[hwChannel] == ADC_FAST) if (boardConfiguration->adcHwChannelEnabled[hwChannel] == ADC_FAST)
return fastAdcValue; return fastAdcValue;
int internalIndex = slowAdc.internalAdcIndexByHardwareIndex[hwChannel]; int internalIndex = slowAdc.internalAdcIndexByHardwareIndex[hwChannel];
return getAdcValueByIndex(internalIndex); return slowAdc.getAdcValueByIndex(internalIndex);
} }
static PWMConfig pwmcfg_slow = { PWM_FREQ_SLOW, PWM_PERIOD_SLOW, pwmpcb_slow, { { static PWMConfig pwmcfg_slow = { PWM_FREQ_SLOW, PWM_PERIOD_SLOW, pwmpcb_slow, { {
PWM_OUTPUT_DISABLED, NULL }, { PWM_OUTPUT_DISABLED, NULL }, { PWM_OUTPUT_DISABLED, NULL }, { PWM_OUTPUT_DISABLED, NULL }, {
PWM_OUTPUT_DISABLED, NULL }, { PWM_OUTPUT_DISABLED, NULL } }, PWM_OUTPUT_DISABLED, NULL }, { PWM_OUTPUT_DISABLED, NULL } },
/* HW dependent part.*/ /* HW dependent part.*/
0 }; 0, 0 };
static PWMConfig pwmcfg_fast = { PWM_FREQ_FAST, PWM_PERIOD_FAST, pwmpcb_fast, { { static PWMConfig pwmcfg_fast = { PWM_FREQ_FAST, PWM_PERIOD_FAST, pwmpcb_fast, { {
PWM_OUTPUT_DISABLED, NULL }, { PWM_OUTPUT_DISABLED, NULL }, { PWM_OUTPUT_DISABLED, NULL }, { PWM_OUTPUT_DISABLED, NULL }, {
PWM_OUTPUT_DISABLED, NULL }, { PWM_OUTPUT_DISABLED, NULL } }, PWM_OUTPUT_DISABLED, NULL }, { PWM_OUTPUT_DISABLED, NULL } },
/* HW dependent part.*/ /* HW dependent part.*/
0 }; 0, 0 };
static void initAdcPin(ioportid_t port, int pin, const char *msg) { static void initAdcPin(ioportid_t port, int pin, const char *msg) {
print("adc %s\r\n", msg); print("adc %s\r\n", msg);
mySetPadMode("adc input", port, pin, PAL_MODE_INPUT_ANALOG); mySetPadMode("adc input", port, pin, PAL_MODE_INPUT_ANALOG);
} }
GPIO_TypeDef* getAdcChannelPort(int hwChannel) { adc_channel_e getAdcChannel(brain_pin_e pin) {
switch(pin) {
case GPIOA_0:
return EFI_ADC_0;
case GPIOA_1:
return EFI_ADC_1;
case GPIOA_2:
return EFI_ADC_2;
case GPIOA_3:
return EFI_ADC_3;
case GPIOA_4:
return EFI_ADC_4;
case GPIOA_5:
return EFI_ADC_5;
case GPIOA_6:
return EFI_ADC_6;
case GPIOA_7:
return EFI_ADC_7;
case GPIOB_0:
return EFI_ADC_8;
case GPIOB_1:
return EFI_ADC_9;
case GPIOC_0:
return EFI_ADC_10;
case GPIOC_1:
return EFI_ADC_11;
case GPIOC_2:
return EFI_ADC_12;
case GPIOC_3:
return EFI_ADC_13;
case GPIOC_4:
return EFI_ADC_14;
case GPIOC_5:
return EFI_ADC_15;
default:
return EFI_ADC_ERROR;
}
}
GPIO_TypeDef* getAdcChannelPort(adc_channel_e hwChannel) {
// todo: replace this with an array :) // todo: replace this with an array :)
switch (hwChannel) { switch (hwChannel) {
case ADC_CHANNEL_IN0: case ADC_CHANNEL_IN0:
@ -272,7 +306,17 @@ GPIO_TypeDef* getAdcChannelPort(int hwChannel) {
} }
} }
int getAdcChannelPin(int hwChannel) { const char * getAdcMode(adc_channel_e hwChannel) {
if (slowAdc.isHwUsed(hwChannel)) {
return "slow";
}
if (fastAdc.isHwUsed(hwChannel)) {
return "fast";
}
return "INACTIVE";
}
int getAdcChannelPin(adc_channel_e hwChannel) {
// todo: replace this with an array :) // todo: replace this with an array :)
switch (hwChannel) { switch (hwChannel) {
case ADC_CHANNEL_IN0: case ADC_CHANNEL_IN0:
@ -313,7 +357,7 @@ int getAdcChannelPin(int hwChannel) {
} }
} }
static void initAdcHwChannel(int hwChannel) { static void initAdcHwChannel(adc_channel_e hwChannel) {
GPIO_TypeDef* port = getAdcChannelPort(hwChannel); GPIO_TypeDef* port = getAdcChannelPort(hwChannel);
int pin = getAdcChannelPin(hwChannel); int pin = getAdcChannelPin(hwChannel);
@ -324,12 +368,25 @@ int AdcConfiguration::size() {
return channelCount; return channelCount;
} }
int AdcConfiguration::getAdcValueByIndex(int internalIndex) {
return values.adc_data[internalIndex];
}
void AdcConfiguration::init(void) { void AdcConfiguration::init(void) {
hwConfig->num_channels = size(); hwConfig->num_channels = size();
hwConfig->sqr1 += ADC_SQR1_NUM_CH(size()); hwConfig->sqr1 += ADC_SQR1_NUM_CH(size());
} }
void AdcConfiguration::addChannel(int hwChannel) { bool AdcConfiguration::isHwUsed(adc_channel_e hwChannelIndex) {
for (int i = 0; i < channelCount; i++) {
if (hardwareIndexByIndernalAdcIndex[i] == hwChannelIndex) {
return true;
}
}
return false;
}
void AdcConfiguration::addChannel(adc_channel_e hwChannel) {
int logicChannel = channelCount++; int logicChannel = channelCount++;
internalAdcIndexByHardwareIndex[hwChannel] = logicChannel; internalAdcIndexByHardwareIndex[hwChannel] = logicChannel;
@ -344,13 +401,13 @@ void AdcConfiguration::addChannel(int hwChannel) {
initAdcHwChannel(hwChannel); initAdcHwChannel(hwChannel);
} }
static void printAdcValue(int channel) { static void printAdcValue(adc_channel_e channel) {
int value = getAdcValue(channel); int value = getAdcValue(channel);
float volts = adcToVoltsDivided(value); float volts = adcToVoltsDivided(value);
scheduleMsg(&logger, "adc voltage : %f", volts); scheduleMsg(&logger, "adc voltage : %f", volts);
} }
int AdcConfiguration::getAdcHardwareIndexByInternalIndex(int index) { adc_channel_e AdcConfiguration::getAdcHardwareIndexByInternalIndex(int index) {
return hardwareIndexByIndernalAdcIndex[index]; return hardwareIndexByIndernalAdcIndex[index];
} }
@ -360,11 +417,11 @@ static void printFullAdcReport(void) {
for (int index = 0; index < slowAdc.size(); index++) { for (int index = 0; index < slowAdc.size(); index++) {
appendMsgPrefix(&logger); appendMsgPrefix(&logger);
int hwIndex = slowAdc.getAdcHardwareIndexByInternalIndex(index); adc_channel_e hwIndex = slowAdc.getAdcHardwareIndexByInternalIndex(index);
GPIO_TypeDef* port = getAdcChannelPort(hwIndex); GPIO_TypeDef* port = getAdcChannelPort(hwIndex);
int pin = getAdcChannelPin(hwIndex); int pin = getAdcChannelPin(hwIndex);
int adcValue = getAdcValueByIndex(index); int adcValue = slowAdc.getAdcValueByIndex(index);
appendPrintf(&logger, " ch%d %s%d", index, portname(port), pin); appendPrintf(&logger, " ch%d %s%d", index, portname(port), pin);
appendPrintf(&logger, " ADC%d 12bit=%d", hwIndex, adcValue); appendPrintf(&logger, " ADC%d 12bit=%d", hwIndex, adcValue);
float volts = adcToVolts(adcValue); float volts = adcToVolts(adcValue);
@ -397,7 +454,7 @@ static void adc_callback_slow(ADCDriver *adcp, adcsample_t *buffer, size_t n) {
// newState.time = chimeNow(); // newState.time = chimeNow();
for (int i = 0; i < slowAdc.size(); i++) { for (int i = 0; i < slowAdc.size(); i++) {
int value = getAvgAdcValue(i, slowAdcState.samples, ADC_GRP1_BUF_DEPTH_SLOW, slowAdc.size()); int value = getAvgAdcValue(i, slowAdcState.samples, ADC_GRP1_BUF_DEPTH_SLOW, slowAdc.size());
newState.adc_data[i] = value; slowAdc.values.adc_data[i] = value;
} }
} }
} }
@ -409,13 +466,16 @@ static void adc_callback_fast(ADCDriver *adcp, adcsample_t *buffer, size_t n) {
// intermediate callback when the buffer is half full.*/ // intermediate callback when the buffer is half full.*/
if (adcp->state == ADC_COMPLETE) { if (adcp->state == ADC_COMPLETE) {
fastAdcValue = getAvgAdcValue(0, samples_fast, ADC_GRP1_BUF_DEPTH_FAST, fastAdc.size()); fastAdcValue = getAvgAdcValue(0, samples_fast, ADC_GRP1_BUF_DEPTH_FAST, fastAdc.size());
fastAdc.values.adc_data[0] = fastAdcValue;
#if EFI_MAP_AVERAGING #if EFI_MAP_AVERAGING
mapAveragingCallback(fastAdcValue); mapAveragingCallback(fastAdcValue);
#endif /* EFI_MAP_AVERAGING */ #endif /* EFI_MAP_AVERAGING */
} }
} }
void initAdcInputs(bool isBoardTestMode) { void initAdcInputs(void) {
initLoggingExt(&logger, "ADC", LOGGING_BUFFER, sizeof(LOGGING_BUFFER)); initLoggingExt(&logger, "ADC", LOGGING_BUFFER, sizeof(LOGGING_BUFFER));
printMsg(&logger, "initAdcInputs()"); printMsg(&logger, "initAdcInputs()");
@ -434,22 +494,20 @@ void initAdcInputs(bool isBoardTestMode) {
for (int adc = 0; adc < HW_MAX_ADC_INDEX; adc++) { for (int adc = 0; adc < HW_MAX_ADC_INDEX; adc++) {
adc_channel_mode_e mode = boardConfiguration->adcHwChannelEnabled[adc]; adc_channel_mode_e mode = boardConfiguration->adcHwChannelEnabled[adc];
if (mode == ADC_SLOW || (isBoardTestMode && mode == ADC_FAST)) { if (mode == ADC_SLOW) {
slowAdc.addChannel(ADC_CHANNEL_IN0 + adc); slowAdc.addChannel((adc_channel_e) (ADC_CHANNEL_IN0 + adc));
} else if (mode == ADC_FAST) { } else if (mode == ADC_FAST) {
fastAdc.addChannel(ADC_CHANNEL_IN0 + adc); fastAdc.addChannel((adc_channel_e) (ADC_CHANNEL_IN0 + adc));
} }
} }
slowAdc.init(); slowAdc.init();
pwmStart(EFI_INTERNAL_SLOW_ADC_PWM, &pwmcfg_slow); pwmStart(EFI_INTERNAL_SLOW_ADC_PWM, &pwmcfg_slow);
if (!isBoardTestMode) {
fastAdc.init(); fastAdc.init();
/* /*
* Initializes the PWM driver. * Initializes the PWM driver.
*/ */
pwmStart(EFI_INTERNAL_FAST_ADC_PWM, &pwmcfg_fast); pwmStart(EFI_INTERNAL_FAST_ADC_PWM, &pwmcfg_fast);
}
// ADC_CHANNEL_IN0 // PA0 // ADC_CHANNEL_IN0 // PA0
// ADC_CHANNEL_IN1 // PA1 // ADC_CHANNEL_IN1 // PA1
@ -470,7 +528,7 @@ void initAdcInputs(bool isBoardTestMode) {
//if(slowAdcChannelCount > ADC_MAX_SLOW_CHANNELS_COUNT) // todo: do we need this logic? do we need this check //if(slowAdcChannelCount > ADC_MAX_SLOW_CHANNELS_COUNT) // todo: do we need this logic? do we need this check
addConsoleActionI("adc", printAdcValue); addConsoleActionI("adc", (VoidInt) printAdcValue);
addConsoleAction("fadc", printFullAdcReport); addConsoleAction("fadc", printFullAdcReport);
#else #else
printMsg(&logger, "ADC disabled"); printMsg(&logger, "ADC disabled");

View File

@ -12,24 +12,25 @@
#include "main.h" #include "main.h"
#include "adc_math.h" #include "adc_math.h"
const char * getAdcMode(adc_channel_e hwChannel);
int getAdcChannelPin(adc_channel_e hwChannel);
void initAdcInputs(void);
GPIO_TypeDef* getAdcChannelPort(adc_channel_e hwChannel);
adc_channel_e getAdcChannel(brain_pin_e pin);
#ifdef __cplusplus #ifdef __cplusplus
extern "C" extern "C"
{ {
#endif /* __cplusplus */ #endif /* __cplusplus */
GPIO_TypeDef* getAdcChannelPort(int hwChannel);
int getAdcChannelPin(int hwChannel);
void initAdcInputs(bool isBoardTestMode);
int getAdcHardwareIndexByInternalIndex(int index); int getAdcHardwareIndexByInternalIndex(int index);
int getAdcValueByIndex(int internalIndex);
void pokeAdcInputs(void); void pokeAdcInputs(void);
int getInternalAdcValue(int index); int getInternalAdcValue(adc_channel_e index);
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif /* __cplusplus */ #endif /* __cplusplus */
/* Depth of the conversion buffer, channels are sampled X times each.*/ /* Depth of the conversion buffer, channels are sampled X times each.*/
#define ADC_GRP1_BUF_DEPTH_SLOW 1 #define ADC_GRP1_BUF_DEPTH_SLOW 1
#define ADC_MAX_CHANNELS_COUNT 16 #define ADC_MAX_CHANNELS_COUNT 16

View File

@ -2,6 +2,8 @@
* @file board_test.cpp * @file board_test.cpp
* @brief This is a simple board testing utility * @brief This is a simple board testing utility
* *
* By default this is enabled by grounding PB0
*
* @date Mar 12, 2014 * @date Mar 12, 2014
* @author Andrey Belomutskiy, (c) 2012-2014 * @author Andrey Belomutskiy, (c) 2012-2014
* *
@ -29,20 +31,45 @@
static volatile int stepCoutner = 0; static volatile int stepCoutner = 0;
static volatile brain_pin_e currentPin = GPIO_NONE; static volatile brain_pin_e currentPin = GPIO_NONE;
static volatile int currentIndex = 0;
extern AdcConfiguration slowAdc; extern AdcConfiguration slowAdc;
extern AdcConfiguration fastAdc;
static int isTimeForNextStep(int copy) { static bool isTimeForNextStep(int copy) {
return copy != stepCoutner; return copy != stepCoutner;
} }
static void processAdcPin(AdcConfiguration *adc, int index, const char *prefix) {
adc_channel_e hwIndex = adc->getAdcHardwareIndexByInternalIndex(index);
GPIO_TypeDef* port = getAdcChannelPort(hwIndex);
int pin = getAdcChannelPin(hwIndex);
int copy = stepCoutner;
int c = 0;
while (!isTimeForNextStep(copy)) {
print("%s ch%d hwIndex=%d %s%d\r\n", prefix, index, hwIndex, portname(port), pin);
int adcValue = adc->getAdcValueByIndex(index);
// print("ADC%d val= %d%s", hwIndex, value, DELIMETER);
float volts = adcToVolts(adcValue) * 2;
print("v=%f adc=%d c=%d (hit 'n'<ENTER> for next step\r\n", volts, adcValue, c++);
chThdSleepMilliseconds(300);
}
}
static volatile int currentIndex = 0;
static void waitForKey(void) { static void waitForKey(void) {
print("Please hit N<ENTER> to continue\r\n"); print("Please hit N<ENTER> to continue\r\n");
int copy = stepCoutner; int copy = stepCoutner;
while (!isTimeForNextStep(copy)) while (!isTimeForNextStep(copy)) {
chThdSleepMilliseconds(200); chThdSleepMilliseconds(200);
} }
}
static void nextStep(void) { static void nextStep(void) {
stepCoutner++; stepCoutner++;
@ -102,12 +129,11 @@ bool isBoardTestMode(void) {
void printBoardTestState(void) { void printBoardTestState(void) {
print("Current index=%d\r\n", currentIndex); print("Current index=%d\r\n", currentIndex);
print("'n' for next step and 'set X' to return to step X\r\n"); print("'n' for next step and 'set X' to return to step X\r\n");
print("ADC count: %d\r\n", slowAdc.size()); print("ADC count: slow %d/fast %d\r\n", slowAdc.size(), fastAdc.size());
if (currentPin != GPIO_NONE) { if (currentPin != GPIO_NONE) {
print("Blinking %s\r\n", hwPortname(currentPin)); print("Blinking %s\r\n", hwPortname(currentPin));
} }
} }
void initBoardTest(void) { void initBoardTest(void) {
@ -119,37 +145,15 @@ void initBoardTest(void) {
// this code is ugly as hell, I had no time to think. Todo: refactor // this code is ugly as hell, I had no time to think. Todo: refactor
int pinsCount = sizeof(BLINK_PINS) / sizeof(brain_pin_e); processAdcPin(&fastAdc, 0, "fast");
while (currentIndex < slowAdc.size()) { while (currentIndex < slowAdc.size()) {
int hwIndex = slowAdc.getAdcHardwareIndexByInternalIndex(currentIndex); processAdcPin(&slowAdc, currentIndex, "slow");
GPIO_TypeDef* port = getAdcChannelPort(hwIndex);
int pin = getAdcChannelPin(hwIndex);
int value = getAdcValueByIndex(currentIndex);
int copy = stepCoutner;
int c = 0;
while (!isTimeForNextStep(copy)) {
print("ch%d hwIndex=%d %s%d\r\n", currentIndex, hwIndex, portname(port), pin);
int adcValue = getAdcValueByIndex(currentIndex);
// print("ADC%d val= %d%s", hwIndex, value, DELIMETER);
float volts = adcToVolts(adcValue) * 2;
print("v=%f adc=%d c=%d (hit 'n'<ENTER> for next step\r\n", volts, adcValue, c++);
chThdSleepMilliseconds(300);
}
currentIndex++; currentIndex++;
} }
currentIndex = 0; currentIndex = 0;
int pinsCount = sizeof(BLINK_PINS) / sizeof(brain_pin_e);
while (currentIndex < pinsCount) { while (currentIndex < pinsCount) {
currentPin = BLINK_PINS[currentIndex]; currentPin = BLINK_PINS[currentIndex];

View File

@ -17,7 +17,7 @@
#include "can_header.h" #include "can_header.h"
#include "engine_configuration.h" #include "engine_configuration.h"
#if EFI_CAN_SUPPORT #if EFI_CAN_SUPPORT || defined(__DOXYGEN__)
static int canReadCounter = 0; static int canReadCounter = 0;
static Logging logger; static Logging logger;

View File

@ -5,7 +5,7 @@
#include "main.h" #include "main.h"
#if EFI_INTERNAL_FLASH #if EFI_INTERNAL_FLASH || defined(__DOXYGEN__)
#include "flash.h" #include "flash.h"
#include <string.h> #include <string.h>

View File

@ -36,7 +36,7 @@ void initOutputPinExt(const char *msg, OutputPin *outputPin, GPIO_TypeDef *port,
// firmwareError("outputPin already assigned to %x%d", outputPin->port, outputPin->pin); // firmwareError("outputPin already assigned to %x%d", outputPin->port, outputPin->pin);
// return; // return;
// } // }
outputPin->currentLogicValue = -1; outputPin->currentLogicValue = INITIAL_PIN_STATE;
outputPin->port = port; outputPin->port = port;
outputPin->pin = pinNumber; outputPin->pin = pinNumber;

View File

@ -9,6 +9,9 @@
#ifndef GPIO_HELPER_H_ #ifndef GPIO_HELPER_H_
#define GPIO_HELPER_H_ #define GPIO_HELPER_H_
#define INITIAL_PIN_STATE -1
/** /**
* @brief Single output pin reference and state * @brief Single output pin reference and state
*/ */

View File

@ -185,7 +185,7 @@ void initHardware(Logging *logger) {
getHwPin(boardConfiguration->boardTestModeJumperPin), PAL_MODE_INPUT_PULLUP); getHwPin(boardConfiguration->boardTestModeJumperPin), PAL_MODE_INPUT_PULLUP);
bool isBoardTestMode = GET_BOARD_TEST_MODE_VALUE(); bool isBoardTestMode = GET_BOARD_TEST_MODE_VALUE();
initAdcInputs(isBoardTestMode); initAdcInputs();
if (isBoardTestMode) { if (isBoardTestMode) {
initBoardTest(); initBoardTest();

Some files were not shown because too many files have changed in this diff Show More