From f253ce8482e431ddbefafb5fc5e680f134d29b40 Mon Sep 17 00:00:00 2001 From: Hakan Bastedt Date: Mon, 13 Oct 2025 22:07:05 +0200 Subject: [PATCH] Only free run, no DC or SM2 sync --- .../Firmware/src/main.cpp | 251 +++++++----------- 1 file changed, 102 insertions(+), 149 deletions(-) diff --git a/Cards/EaserCAT-7000-DIO+I2C/Firmware/src/main.cpp b/Cards/EaserCAT-7000-DIO+I2C/Firmware/src/main.cpp index 0166a29..05b31ff 100755 --- a/Cards/EaserCAT-7000-DIO+I2C/Firmware/src/main.cpp +++ b/Cards/EaserCAT-7000-DIO+I2C/Firmware/src/main.cpp @@ -36,15 +36,15 @@ ADS1014 *ads1014_2 = 0; void ads1014_reset(ADS1014 *ads) { ads->reset(); ads->begin(); - ads->setGain(1); // 1=4.096V - ads->setMode(0); // 0 continuous - ads->setDataRate(6); // Max for ads101x - ads->readADC_Differential_0_1(); // This is the value we are interested in + ads->setGain(1); // 1=4.096V + ads->setMode(0); // 0 continuous + ads->setDataRate(6); // Max for ads101x + ads->readADC_Differential_0_1(); // This is the value we are interested in } #include class OhmicSensing { - public: +public: void handle(uint8_t voltageState, float inVoltage, float limitVoltage, float voltageDropLimit, uint32_t setupTime, uint8_t enabled, uint8_t &sensed); @@ -58,9 +58,7 @@ class OhmicSensing { OHMIC_PROBE_DONE }; OhmicStates ohmicState = OHMIC_IDLE; - uint32_t setupTimeSoFar = 0; - uint32_t probingTime = 0; - uint16_t senseOnTime = 0; + uint64_t startTime; float_t oldVoltage = 0.0; std::queue voltages; float_t refVoltage; @@ -68,6 +66,9 @@ class OhmicSensing { OhmicSensing Ohm1; OhmicSensing Ohm2; +// DEBUG +volatile int saveALEvent; + void handleVoltageReader(float scale_in, float offset, float &outVoltage, int32_t &outRaw, float &oldVoltage, float &oldRaw, uint8_t devType, int8_t &old_devType, @@ -86,10 +87,10 @@ void lowpassFilter(float &oldLowPassGain, #define bitflip(byte, nbit) ((byte) ^= (1 << (nbit))) #define bitcheck(byte, nbit) ((byte) & (1 << (nbit))) -volatile uint16_t ALEventIRQ; // ALEvent that caused the interrupt +volatile uint16_t ALEventIRQ; // ALEvent that caused the interrupt extern "C" uint32_t ESC_SYNC0cycletime(void); -void cb_set_outputs(void) // Get Master outputs, slave inputs, first operation +void cb_set_outputs(void) // Get Master outputs, slave inputs, first operation { // Update digital output pins for (int i = 0; i < sizeof(outputPin); i++) @@ -101,7 +102,7 @@ float oldLowPassFilteredVoltage_1 = 0, oldLowPassFilteredVoltage_2 = 0; uint32_t oldLowpassFilterPoleFrequency_1 = 0, oldLowpassFilterPoleFrequency_2 = 0; -void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation +void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation { static float validData0_1 = 0.0, validVoltage0_1 = 0.0; static float validData0_2 = 0.0, validVoltage0_2 = 0.0; @@ -166,8 +167,8 @@ static esc_cfg_t config = { .post_object_download_hook = NULL, .rxpdo_override = NULL, .txpdo_override = NULL, - .esc_hw_interrupt_enable = ESC_interrupt_enable, - .esc_hw_interrupt_disable = ESC_interrupt_disable, + .esc_hw_interrupt_enable = NULL, + .esc_hw_interrupt_disable = NULL, .esc_hw_eep_handler = NULL, .esc_check_dc_handler = dc_checker, }; @@ -200,7 +201,7 @@ void setup(void) { ecat_slv_init(&config); #endif -#if 0 // Uncomment for commissioning tests +#if 0 // Uncomment for commissioning tests // #define only one of the below #define ADS1xxx #undef ADC_MCP3221 @@ -256,69 +257,10 @@ void setup(void) { void loop(void) { #ifdef ECAT - uint64_t dTime; - if (serveIRQ) { - DIG_process(ALEventIRQ, DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG | - DIG_PROCESS_APP_HOOK_FLAG | - DIG_PROCESS_INPUTS_FLAG); - serveIRQ = 0; - ESCvar.PrevTime = ESCvar.Time; - ecat_slv_poll(); - } - dTime = longTime.extendTime(micros()) - irqTime; - if (dTime > 5000) // Not doing interrupts - handle free-run - ecat_slv(); - + ecat_slv(); #endif } -void sync0Handler(void) { - ALEventIRQ = ESC_ALeventread(); - // if (ALEventIRQ & ESCREG_ALEVENT_SM2) - { - irqTime = longTime.extendTime(micros()); - serveIRQ = 1; - } -} - -// Enable SM2 interrupts -void ESC_interrupt_enable(uint32_t mask) { - // Enable interrupt for SYNC0 or SM2 or SM3 - uint32_t user_int_mask = - ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM2 | ESCREG_ALEVENT_SM3; - if (mask & user_int_mask) { - ESC_ALeventmaskwrite(ESC_ALeventmaskread() | (mask & user_int_mask)); - ESC_ALeventmaskwrite(ESC_ALeventmaskread() & - ~(ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM3)); - attachInterrupt(digitalPinToInterrupt(PC3), sync0Handler, RISING); - - // Set LAN9252 interrupt pin driver as push-pull active high - uint32_t bits = 0x00000111; - ESC_write(0x54, &bits, 4); - - // Enable LAN9252 interrupt - bits = 0x00000001; - ESC_write(0x5c, &bits, 4); - } -} - -// Disable SM2 interrupts -void ESC_interrupt_disable(uint32_t mask) { - // Enable interrupt for SYNC0 or SM2 or SM3 - // uint32_t user_int_mask = ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM2 | - // ESCREG_ALEVENT_SM3; - uint32_t user_int_mask = ESCREG_ALEVENT_SM2; - - if (mask & user_int_mask) { - // Disable interrupt from SYNC0 - ESC_ALeventmaskwrite(ESC_ALeventmaskread() & ~(mask & user_int_mask)); - detachInterrupt(digitalPinToInterrupt(PC3)); - // Disable LAN9252 interrupt - uint32_t bits = 0x00000000; - ESC_write(0x5c, &bits, 4); - } -} - // Setup of DC uint16_t dc_checker(void) { // Indicate we run DC @@ -333,87 +275,89 @@ void handleVoltageReader(float scale_in, float offset, float &outVoltage, MyMCP3221 *&mcp, uint8_t I2C_address, uint32_t &I2C_restarts) { float scale = scale_in; - if (scale == 0.0) scale = 1.0; + if (scale == 0.0) + scale = 1.0; int stat = 1, data0; switch (devType) { - case 0: // Not configured. - outStatus = 0; - stat = data0 = 0; - break; - case MCP3221_TYPE: - if (old_devType != devType) // Initilize and make ready - { - if (ads) { - delete ads; - ads = 0; - } - if (mcp) { - delete mcp; - mcp = 0; - } - Wire2.end(); - Wire2.begin(); - Wire2.setClock(I2C_BUS_SPEED); - mcp = new MyMCP3221(I2C_address, &Wire2); - old_devType = mcp ? MCP3221_TYPE : -1; + case 0: // Not configured. + outStatus = 0; + stat = data0 = 0; + break; + case MCP3221_TYPE: + if (old_devType != devType) // Initilize and make ready + { + if (ads) { + delete ads; + ads = 0; } - data0 = mcp->getData(); - stat = mcp->ping(); - break; - case ADS1014_TYPE: - if (old_devType != devType) // Initilize and make ready - { - if (ads) { - delete ads; - ads = 0; - } - if (mcp) { - delete mcp; - mcp = 0; - } - old_devType = 0; + if (mcp) { + delete mcp; + mcp = 0; + } + Wire2.end(); + Wire2.begin(); + Wire2.setClock(I2C_BUS_SPEED); + mcp = new MyMCP3221(I2C_address, &Wire2); + old_devType = mcp ? MCP3221_TYPE : -1; + } + data0 = mcp->getData(); + stat = mcp->ping(); + break; + case ADS1014_TYPE: + if (old_devType != devType) // Initilize and make ready + { + if (ads) { + delete ads; + ads = 0; + } + if (mcp) { + delete mcp; + mcp = 0; + } + old_devType = 0; - Wire2.end(); - Wire2.begin(); - Wire2.setClock(I2C_BUS_SPEED); - ads = new ADS1014(I2C_address, &Wire2); - if (ads != nullptr) { - ads1014_reset(ads); - old_devType = ADS1014_TYPE; - } - } + Wire2.end(); + Wire2.begin(); + Wire2.setClock(I2C_BUS_SPEED); + ads = new ADS1014(I2C_address, &Wire2); if (ads != nullptr) { - data0 = ads->getValue(); - stat = ads->isConnected() == 1 ? 0 : 1; + ads1014_reset(ads); + old_devType = ADS1014_TYPE; } - break; - default: // Not supported - break; + } + if (ads != nullptr) { + data0 = ads->getValue(); + stat = ads->isConnected() == 1 ? 0 : 1; + } + break; + default: // Not supported + break; } - if (stat == 0) { // Read good value - outVoltage = scale * data0 + offset; // - outRaw = data0; // Raw voltage, read by ADC + if (stat == 0) { // Read good value + outVoltage = scale * data0 + offset; // + outRaw = data0; // Raw voltage, read by ADC oldVoltage = outVoltage; oldRaw = data0; - } else { // Didn't read a good value. Return a hopefully useful value and - // restart - // the I2C bus - outVoltage = oldVoltage; // Use value from previous call + } else { // Didn't read a good value. Return a hopefully useful value and + // restart + // the I2C bus + outVoltage = oldVoltage; // Use value from previous call outRaw = oldRaw; // Reset wire here Wire2.end(); Wire2.begin(); Wire2.setClock(I2C_BUS_SPEED); I2C_restarts++; - if (devType == ADS1014_TYPE && ads != nullptr) ads1014_reset(ads); + if (devType == ADS1014_TYPE && ads != nullptr) + ads1014_reset(ads); // mcp3221 has no reset, reset the I2C bus is the best we can do } readStat = stat; outStatus = - I2C_restarts + (stat << 28); // Put status as bits 28-31, the lower are - // number of restarts (restart attempts) + I2C_restarts + (stat << 28); // Put status as bits 28-31, the lower are + // number of restarts (restart attempts) } void lowpassFilter(float &oldLowPassGain, @@ -431,7 +375,7 @@ void lowpassFilter(float &oldLowPassGain, oldLowpassFilterPoleFrequency = LowpassFilterPoleFrequency; } if (inVoltage < LowPassFilterThresholdVoltage) - outFilteredVoltage = inVoltage; // Just forward + outFilteredVoltage = inVoltage; // Just forward else outFilteredVoltage = oldLowPassFilteredVoltage + (inVoltage - oldLowPassFilteredVoltage) * gain; @@ -444,39 +388,47 @@ void OhmicSensing::handle(uint8_t voltageState, float inVoltage, uint32_t setupTime, uint8_t enabled, uint8_t &sensed) { sensed = 0; + uint64_t dTime; + Obj.Out_Unit1.RawData = ohmicState; if (enabled && voltageState == 0) { if (ohmicState == OHMIC_IDLE && inVoltage > limitVoltage) { ohmicState = OHMIC_SETUP; - setupTimeSoFar = 0; - while (!voltages.empty()) voltages.pop(); // Remove history + startTime = longTime.extendTime(micros()); + while (!voltages.empty()) + voltages.pop(); // Remove history return; } if (ohmicState == OHMIC_SETUP) { - if (setupTimeSoFar++ > setupTime) { + dTime = longTime.extendTime(micros()) - startTime; + Obj.Out_Unit2.RawData = dTime; + if (dTime > setupTime * 1000) { ohmicState = OHMIC_PROBE; - probingTime = 0; + startTime = longTime.extendTime(micros()); oldVoltage = 0.0; - refVoltage = inVoltage; // RefVoltage = voltage at end of setup + refVoltage = inVoltage; // RefVoltage = voltage at end of setup return; } } if (ohmicState == OHMIC_PROBE) { + dTime = longTime.extendTime(micros()) - startTime; + Obj.Out_Unit2.RawData = dTime; voltages.push(inVoltage); - while (voltages.size() > N_VOLTAGES) voltages.pop(); // Only N_VOLTAGES - if (probingTime++ > 30000) { // Go to IDLE after 30 seconds + while (voltages.size() > N_VOLTAGES) + voltages.pop(); // Only N_VOLTAGES + if (dTime > 30000000) { // Go to IDLE after 30 seconds ohmicState = OHMIC_IDLE; return; } - if ((inVoltage <= limitVoltage) || // Below starting threshold + if ((inVoltage <= limitVoltage) || // Below starting threshold (fabs(voltageDropLimit) > 1e-3 && refVoltage - inVoltage >= - voltageDropLimit) || // Delta below refVoltage - (fabs(voltageDropLimit) > 1e-3 && // Immediate drop + voltageDropLimit) || // Delta below refVoltage + (fabs(voltageDropLimit) > 1e-3 && // Immediate drop oldVoltage - inVoltage >= voltageDropLimit) || - (fabs(voltageDropLimit) > 1e-3 && // Drop over 3 cycles + (fabs(voltageDropLimit) > 1e-3 && // Drop over 3 cycles voltages.front() - voltages.back() > voltageDropLimit)) { sensed = 1; - senseOnTime = 0; + startTime = longTime.extendTime(micros()); ohmicState = OHMIC_PROBED_ON; } oldVoltage = inVoltage; @@ -484,7 +436,9 @@ void OhmicSensing::handle(uint8_t voltageState, float inVoltage, } if (ohmicState == OHMIC_PROBED_ON) { sensed = 1; - if (senseOnTime++ >= 100) { + dTime = longTime.extendTime(micros()) - startTime; + Obj.Out_Unit2.RawData = dTime; + if (dTime > 100000) { sensed = 0; ohmicState = OHMIC_PROBE_DONE; } @@ -492,7 +446,6 @@ void OhmicSensing::handle(uint8_t voltageState, float inVoltage, } if (ohmicState == OHMIC_PROBE_DONE) { sensed = 0; - if (!enabled) ohmicState = OHMIC_IDLE; return; } } else {