jeka/MyOwnEtherCATDevice
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Implemented setup time for sensing.

Browse Source 
That is the time to wait from sensing signal received until measuring to allow
relay to activate, debounce and all that.

And changed to indentation style so the whole file looks edited buts it's only partially.
This commit is contained in:
Hakan Bastedt
2025-10-07 11:17:30 +02:00
parent c31249b603
commit 719f1755fe
1 changed files with 280 additions and 284 deletions
Download Patch File Download Diff File Expand all files Collapse all files

564
Cards/EaserCAT-7000-DIO+I2C/Firmware/src/main.cpp
View File

@@ -1,7 +1,6 @@
#include <Arduino.h>
#include <stdio.h>
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;
}
}