diff --git a/Cards/EaserCAT-7000-DIO+I2C/Firmware/src/main.cpp b/Cards/EaserCAT-7000-DIO+I2C/Firmware/src/main.cpp index 070963b..1e5dbf6 100755 --- a/Cards/EaserCAT-7000-DIO+I2C/Firmware/src/main.cpp +++ b/Cards/EaserCAT-7000-DIO+I2C/Firmware/src/main.cpp @@ -1,7 +1,6 @@ #include #include -extern "C" -{ +extern "C" { #include "ecat_slv.h" #include "utypes.h" }; @@ -13,7 +12,8 @@ volatile uint64_t irqTime = 0; HardwareSerial Serial1(PA10, PA9); -uint8_t inputPin[] = {PD15, PD14, PD13, PD12, PD11, PD10, PD9, PD8, PB15, PB14, PB13, PB12}; +uint8_t inputPin[] = {PD15, PD14, PD13, PD12, PD11, PD10, + PD9, PD8, PB15, PB14, PB13, PB12}; uint8_t outputPin[] = {PE10, PE9, PE8, PE7}; const uint32_t I2C_BUS_SPEED = 400000; @@ -33,30 +33,31 @@ MyMCP3221 *mcp3221_2 = 0; ADS1014 *ads1014_1 = 0; 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 +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 } -void handleVoltageReader(float scale_in, float offset, float &outVoltage, int32_t &outRaw, - float &oldVoltage, float &oldRaw, - uint8_t devType, int8_t &old_devType, uint8_t &readStat, uint32_t &outStatus, - ADS1014 *&ads, MyMCP3221 *&mcp, uint8_t I2C_address, uint32_t &I2C_restarts); +void handleVoltageReader(float scale_in, float offset, float &outVoltage, + int32_t &outRaw, float &oldVoltage, float &oldRaw, + uint8_t devType, int8_t &old_devType, + uint8_t &readStat, uint32_t &outStatus, ADS1014 *&ads, + MyMCP3221 *&mcp, uint8_t I2C_address, + uint32_t &I2C_restarts); void lowpassFilter(float &oldLowPassGain, uint32_t &oldLowpassFilterPoleFrequency, float &oldLowPassFilteredVoltage, uint32_t LowpassFilterPoleFrequency, - float LowPassFilterThresholdVoltage, - float inVoltage, + float LowPassFilterThresholdVoltage, float inVoltage, float &outFilteredVoltage); void handleOhmicSensing(uint8_t &ohmicState, uint8_t voltageState, - float inVoltage, float limitVoltage, - uint8_t enabled, uint8_t &sensed); + float inVoltage, float limitVoltage, uint32_t setupTime, + uint32_t &setupTimeSoFar, uint8_t enabled, + uint8_t &sensed); #define bitset(byte, nbit) ((byte) |= (1 << (nbit))) #define bitclear(byte, nbit) ((byte) &= ~(1 << (nbit))) #define bitflip(byte, nbit) ((byte) ^= (1 << (nbit))) @@ -67,62 +68,69 @@ extern "C" uint32_t ESC_SYNC0cycletime(void); void cb_set_outputs(void) // Get Master outputs, slave inputs, first operation { - // Update digital output pins - for (int i = 0; i < sizeof(outputPin); i++) - digitalWrite(outputPin[i], bitcheck(Obj.Output4, i) ? HIGH : LOW); + // Update digital output pins + for (int i = 0; i < sizeof(outputPin); i++) + digitalWrite(outputPin[i], bitcheck(Obj.Output4, i) ? HIGH : LOW); } float oldLowPassGain_1 = 0, oldLowPassGain_2 = 0; float oldLowPassFilteredVoltage_1 = 0, oldLowPassFilteredVoltage_2 = 0; -uint32_t oldLowpassFilterPoleFrequency_1 = 0, oldLowpassFilterPoleFrequency_2 = 0; +uint32_t oldLowpassFilterPoleFrequency_1 = 0, + oldLowpassFilterPoleFrequency_2 = 0; 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; - uint8_t stat_1, stat_2; + static float validData0_1 = 0.0, validVoltage0_1 = 0.0; + static float validData0_2 = 0.0, validVoltage0_2 = 0.0; + uint8_t stat_1, stat_2; - for (int i = 0; i < sizeof(inputPin); i++) - Obj.Input12 = digitalRead(inputPin[i]) == HIGH ? bitset(Obj.Input12, i) : bitclear(Obj.Input12, i); + for (int i = 0; i < sizeof(inputPin); i++) + Obj.Input12 = digitalRead(inputPin[i]) == HIGH ? bitset(Obj.Input12, i) + : bitclear(Obj.Input12, i); - handleVoltageReader(Obj.In_Unit1.VoltageScale, Obj.In_Unit1.VoltageOffset, Obj.Out_Unit1.CalculatedVoltage, Obj.Out_Unit1.RawData, - validVoltage0_1, validData0_1, - Obj.Settings_Unit1.I2C_devicetype, old_I2Cdevice_1, stat_1, Obj.Out_Unit1.Status, - ads1014_1, mcp3221_1, Obj.Settings_Unit1.I2C_address, I2C_restarts_1); - handleVoltageReader(Obj.In_Unit2.VoltageScale, Obj.In_Unit2.VoltageOffset, Obj.Out_Unit2.CalculatedVoltage, Obj.Out_Unit2.RawData, - validVoltage0_2, validData0_2, - Obj.Settings_Unit2.I2C_devicetype, old_I2Cdevice_2, stat_2, Obj.Out_Unit2.Status, - ads1014_2, mcp3221_2, Obj.Settings_Unit2.I2C_address, I2C_restarts_2); - lowpassFilter(oldLowPassGain_1, - oldLowpassFilterPoleFrequency_1, - oldLowPassFilteredVoltage_1, - Obj.Settings_Unit1.LowpassFilterPoleFrequency, - Obj.In_Unit1.LowPassFilterThresholdVoltage, - Obj.Out_Unit1.CalculatedVoltage, - Obj.Out_Unit1.LowpassFilteredVoltage); - lowpassFilter(oldLowPassGain_2, - oldLowpassFilterPoleFrequency_2, - oldLowPassFilteredVoltage_2, - Obj.Settings_Unit2.LowpassFilterPoleFrequency, - Obj.In_Unit2.LowPassFilterThresholdVoltage, - Obj.Out_Unit2.CalculatedVoltage, - Obj.Out_Unit2.LowpassFilteredVoltage); + handleVoltageReader(Obj.In_Unit1.VoltageScale, Obj.In_Unit1.VoltageOffset, + Obj.Out_Unit1.CalculatedVoltage, Obj.Out_Unit1.RawData, + validVoltage0_1, validData0_1, + Obj.Settings_Unit1.I2C_devicetype, old_I2Cdevice_1, + stat_1, Obj.Out_Unit1.Status, ads1014_1, mcp3221_1, + Obj.Settings_Unit1.I2C_address, I2C_restarts_1); + handleVoltageReader(Obj.In_Unit2.VoltageScale, Obj.In_Unit2.VoltageOffset, + Obj.Out_Unit2.CalculatedVoltage, Obj.Out_Unit2.RawData, + validVoltage0_2, validData0_2, + Obj.Settings_Unit2.I2C_devicetype, old_I2Cdevice_2, + stat_2, Obj.Out_Unit2.Status, ads1014_2, mcp3221_2, + Obj.Settings_Unit2.I2C_address, I2C_restarts_2); + lowpassFilter(oldLowPassGain_1, oldLowpassFilterPoleFrequency_1, + oldLowPassFilteredVoltage_1, + Obj.Settings_Unit1.LowpassFilterPoleFrequency, + Obj.In_Unit1.LowPassFilterThresholdVoltage, + Obj.Out_Unit1.CalculatedVoltage, + Obj.Out_Unit1.LowpassFilteredVoltage); + lowpassFilter(oldLowPassGain_2, oldLowpassFilterPoleFrequency_2, + oldLowPassFilteredVoltage_2, + Obj.Settings_Unit2.LowpassFilterPoleFrequency, + Obj.In_Unit2.LowPassFilterThresholdVoltage, + Obj.Out_Unit2.CalculatedVoltage, + Obj.Out_Unit2.LowpassFilteredVoltage); #define OHMIC_IDLE 0 -#define OHMIC_PROBE 1 - static uint8_t ohmicState_1 = OHMIC_IDLE; - static uint8_t ohmicState_2 = OHMIC_IDLE; +#define OHMIC_SETUP 1 +#define OHMIC_PROBE 2 + static uint8_t ohmicState_1 = OHMIC_IDLE; + static uint8_t ohmicState_2 = OHMIC_IDLE; + static uint32_t setupTimeSoFar_1 = 0; + static uint32_t setupTimeSoFar_2 = 0; - handleOhmicSensing(ohmicState_1, stat_1, - Obj.Out_Unit1.CalculatedVoltage, - Obj.In_Unit1.OhmicSensingVoltageLimit, - Obj.In_Unit1.EnableOhmicSensing, - Obj.Out_Unit1.OhmicSensingSensed); - handleOhmicSensing(ohmicState_2, stat_2, - Obj.Out_Unit2.CalculatedVoltage, - Obj.In_Unit2.OhmicSensingVoltageLimit, - Obj.In_Unit2.EnableOhmicSensing, - Obj.Out_Unit2.OhmicSensingSensed); + handleOhmicSensing(ohmicState_1, stat_1, Obj.Out_Unit1.CalculatedVoltage, + Obj.In_Unit1.OhmicSensingVoltageLimit, + Obj.In_Unit1.OhmicSensingSetupTime, setupTimeSoFar_1, + Obj.In_Unit1.EnableOhmicSensing, + Obj.Out_Unit1.OhmicSensingSensed); + handleOhmicSensing(ohmicState_2, stat_2, Obj.Out_Unit2.CalculatedVoltage, + Obj.In_Unit2.OhmicSensingVoltageLimit, + Obj.In_Unit2.OhmicSensingSetupTime, setupTimeSoFar_2, + Obj.In_Unit2.EnableOhmicSensing, + Obj.Out_Unit2.OhmicSensingSensed); } void ESC_interrupt_enable(uint32_t mask); @@ -130,54 +138,51 @@ void ESC_interrupt_disable(uint32_t mask); uint16_t dc_checker(void); void sync0Handler(void); -static esc_cfg_t config = - { - .user_arg = NULL, - .use_interrupt = 1, - .watchdog_cnt = 150, - .set_defaults_hook = NULL, - .pre_state_change_hook = NULL, - .post_state_change_hook = NULL, - .application_hook = NULL, - .safeoutput_override = NULL, - .pre_object_download_hook = NULL, - .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_eep_handler = NULL, - .esc_check_dc_handler = dc_checker, +static esc_cfg_t config = { + .user_arg = NULL, + .use_interrupt = 1, + .watchdog_cnt = 150, + .set_defaults_hook = NULL, + .pre_state_change_hook = NULL, + .post_state_change_hook = NULL, + .application_hook = NULL, + .safeoutput_override = NULL, + .pre_object_download_hook = NULL, + .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_eep_handler = NULL, + .esc_check_dc_handler = dc_checker, }; volatile byte serveIRQ = 0; -void setup(void) -{ - Serial1.begin(115200); +void setup(void) { + Serial1.begin(115200); - for (int i = 0; i < sizeof(inputPin); i++) - pinMode(inputPin[i], INPUT_PULLDOWN); - for (int i = 0; i < sizeof(outputPin); i++) - { - pinMode(outputPin[i], OUTPUT); - digitalWrite(outputPin[i], LOW); - } - // Debug leds - pinMode(PB4, OUTPUT); - pinMode(PB5, OUTPUT); - pinMode(PB6, OUTPUT); - pinMode(PB7, OUTPUT); - digitalWrite(PB4, LOW); - digitalWrite(PB5, LOW); - digitalWrite(PB6, LOW); - digitalWrite(PB7, LOW); + for (int i = 0; i < sizeof(inputPin); i++) + pinMode(inputPin[i], INPUT_PULLDOWN); + for (int i = 0; i < sizeof(outputPin); i++) { + pinMode(outputPin[i], OUTPUT); + digitalWrite(outputPin[i], LOW); + } + // Debug leds + pinMode(PB4, OUTPUT); + pinMode(PB5, OUTPUT); + pinMode(PB6, OUTPUT); + pinMode(PB7, OUTPUT); + digitalWrite(PB4, LOW); + digitalWrite(PB5, LOW); + digitalWrite(PB6, LOW); + digitalWrite(PB7, LOW); - Wire2.begin(); - Wire2.setClock(I2C_BUS_SPEED); + Wire2.begin(); + Wire2.setClock(I2C_BUS_SPEED); #ifdef ECAT - ecat_slv_init(&config); + ecat_slv_init(&config); #endif #if 0 // Uncomment for commissioning tests @@ -234,220 +239,211 @@ void setup(void) #endif } -void loop(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(); + 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(); #endif } -void sync0Handler(void) -{ - ALEventIRQ = ESC_ALeventread(); - // if (ALEventIRQ & ESCREG_ALEVENT_SM2) - { - irqTime = longTime.extendTime(micros()); - serveIRQ = 1; - } +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); +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); + // 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); - } + // 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; +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); - } + 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 - ESCvar.dcsync = 1; - return 0; +uint16_t dc_checker(void) { + // Indicate we run DC + ESCvar.dcsync = 1; + return 0; } -void handleVoltageReader(float scale_in, float offset, float &outVoltage, int32_t &outRaw, - float &oldVoltage, float &oldRaw, - uint8_t devType, int8_t &old_devType, uint8_t &readStat, uint32_t &outStatus, - ADS1014 *&ads, MyMCP3221 *&mcp, uint8_t I2C_address, uint32_t &I2C_restarts) -{ - float scale = scale_in; - if (scale == 0.0) - scale = 1.0; - int stat = 1, data0; +void handleVoltageReader(float scale_in, float offset, float &outVoltage, + int32_t &outRaw, float &oldVoltage, float &oldRaw, + uint8_t devType, int8_t &old_devType, + uint8_t &readStat, uint32_t &outStatus, ADS1014 *&ads, + MyMCP3221 *&mcp, uint8_t I2C_address, + uint32_t &I2C_restarts) { + float scale = scale_in; + 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; + 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; } - 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; - } + if (mcp) { + delete mcp; + mcp = 0; } - 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 - 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 - 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); - // 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) + 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; + } + } + 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 + 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 + 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); + // 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) } void lowpassFilter(float &oldLowPassGain, uint32_t &oldLowpassFilterPoleFrequency, float &oldLowPassFilteredVoltage, uint32_t LowpassFilterPoleFrequency, - float LowPassFilterThresholdVoltage, - float inVoltage, - float &outFilteredVoltage) -{ - // Low pass filter. See lowpass in linuxcnc doc - float gain = oldLowPassGain; - if (oldLowpassFilterPoleFrequency != LowpassFilterPoleFrequency) - { - gain = 1 - expf(-2.0 * M_PI * LowpassFilterPoleFrequency * 0.001 /*1.0e-9 * ESC_SYNC0cycletime()*/); - oldLowPassGain = gain; - oldLowpassFilterPoleFrequency = LowpassFilterPoleFrequency; - } - if (inVoltage < LowPassFilterThresholdVoltage) - outFilteredVoltage = inVoltage; // Just forward - else - outFilteredVoltage = oldLowPassFilteredVoltage + (inVoltage - oldLowPassFilteredVoltage) * gain; - oldLowPassFilteredVoltage = outFilteredVoltage; + float LowPassFilterThresholdVoltage, float inVoltage, + float &outFilteredVoltage) { + // Low pass filter. See lowpass in linuxcnc doc + float gain = oldLowPassGain; + if (oldLowpassFilterPoleFrequency != LowpassFilterPoleFrequency) { + gain = 1 - expf(-2.0 * M_PI * LowpassFilterPoleFrequency * + 0.001 /*1.0e-9 * ESC_SYNC0cycletime()*/); + oldLowPassGain = gain; + oldLowpassFilterPoleFrequency = LowpassFilterPoleFrequency; + } + if (inVoltage < LowPassFilterThresholdVoltage) + outFilteredVoltage = inVoltage; // Just forward + else + outFilteredVoltage = oldLowPassFilteredVoltage + + (inVoltage - oldLowPassFilteredVoltage) * gain; + oldLowPassFilteredVoltage = outFilteredVoltage; } void handleOhmicSensing(uint8_t &ohmicState, uint8_t voltageState, - float inVoltage, float limitVoltage, - uint8_t enabled, uint8_t &sensed) -{ - sensed = 0; - if (enabled && voltageState == 0) - { - if (ohmicState == OHMIC_IDLE && inVoltage > limitVoltage) - { - ohmicState = OHMIC_PROBE; + float inVoltage, float limitVoltage, uint32_t setupTime, + uint32_t &setupTimeSoFar, uint8_t enabled, + uint8_t &sensed) { + sensed = 0; + if (enabled && voltageState == 0) { + if (ohmicState == OHMIC_IDLE && inVoltage > limitVoltage) { + ohmicState = OHMIC_SETUP; + setupTimeSoFar = 0; + } + if (ohmicState == OHMIC_SETUP) { + if (setupTimeSoFar++ > setupTime) { + ohmicState = OHMIC_PROBE; } - if (ohmicState == OHMIC_PROBE && inVoltage <= limitVoltage) - { - sensed = 1; - } - } - else - { - ohmicState = OHMIC_IDLE; - } + } + if (ohmicState == OHMIC_PROBE && inVoltage <= limitVoltage) { + sensed = 1; + } + } else { + ohmicState = OHMIC_IDLE; + } }