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

198
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,8 +33,7 @@ MyMCP3221 *mcp3221_2 = 0;
ADS1014 *ads1014_1 = 0;
ADS1014 *ads1014_2 = 0;
void ads1014_reset(ADS1014 *ads)
{
void ads1014_reset(ADS1014 *ads) {
ads->reset();
ads->begin();
ads->setGain(1); // 1=4.096V
@@ -43,20 +42,22 @@ void ads1014_reset(ADS1014 *ads)
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)))
@@ -74,7 +75,8 @@ void cb_set_outputs(void) // Get Master outputs, slave inputs, first operation
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
{
@@ -83,25 +85,28 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
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);
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,
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,
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,
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,
lowpassFilter(oldLowPassGain_2, oldLowpassFilterPoleFrequency_2,
oldLowPassFilteredVoltage_2,
Obj.Settings_Unit2.LowpassFilterPoleFrequency,
Obj.In_Unit2.LowPassFilterThresholdVoltage,
@@ -109,18 +114,21 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
Obj.Out_Unit2.LowpassFilteredVoltage);
#define OHMIC_IDLE 0
#define OHMIC_PROBE 1
#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,
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,
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);
}
@@ -130,8 +138,7 @@ void ESC_interrupt_disable(uint32_t mask);
uint16_t dc_checker(void);
void sync0Handler(void);
static esc_cfg_t config =
{
static esc_cfg_t config = {
.user_arg = NULL,
.use_interrupt = 1,
.watchdog_cnt = 150,
@@ -152,14 +159,12 @@ static esc_cfg_t config =
volatile byte serveIRQ = 0;
void setup(void)
{
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++)
{
for (int i = 0; i < sizeof(outputPin); i++) {
pinMode(outputPin[i], OUTPUT);
digitalWrite(outputPin[i], LOW);
}
@@ -234,14 +239,13 @@ void setup(void)
#endif
}
void loop(void)
{
void loop(void) {
#ifdef ECAT
uint64_t dTime;
if (serveIRQ)
{
if (serveIRQ) {
DIG_process(ALEventIRQ, DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG);
DIG_PROCESS_APP_HOOK_FLAG |
DIG_PROCESS_INPUTS_FLAG);
serveIRQ = 0;
ESCvar.PrevTime = ESCvar.Time;
ecat_slv_poll();
@@ -253,8 +257,7 @@ void loop(void)
#endif
}
void sync0Handler(void)
{
void sync0Handler(void) {
ALEventIRQ = ESC_ALeventread();
// if (ALEventIRQ & ESCREG_ALEVENT_SM2)
{
@@ -264,14 +267,14 @@ void sync0Handler(void)
}
// Enable SM2 interrupts
void ESC_interrupt_enable(uint32_t mask)
{
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)
{
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));
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
@@ -285,14 +288,13 @@ void ESC_interrupt_enable(uint32_t mask)
}
// Disable SM2 interrupts
void ESC_interrupt_disable(uint32_t mask)
{
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_DC_SYNC0 | ESCREG_ALEVENT_SM2 |
// ESCREG_ALEVENT_SM3;
uint32_t user_int_mask = ESCREG_ALEVENT_SM2;
if (mask & user_int_mask)
{
if (mask & user_int_mask) {
// Disable interrupt from SYNC0
ESC_ALeventmaskwrite(ESC_ALeventmaskread() & ~(mask & user_int_mask));
detachInterrupt(digitalPinToInterrupt(PC3));
@@ -303,25 +305,24 @@ void ESC_interrupt_disable(uint32_t mask)
}
// Setup of DC
uint16_t dc_checker(void)
{
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)
{
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)
{
switch (devType) {
case 0: // Not configured.
outStatus = 0;
stat = data0 = 0;
@@ -329,13 +330,11 @@ void handleVoltageReader(float scale_in, float offset, float &outVoltage, int32_
case MCP3221_TYPE:
if (old_devType != devType) // Initilize and make ready
{
if (ads)
{
if (ads) {
delete ads;
ads = 0;
}
if (mcp)
{
if (mcp) {
delete mcp;
mcp = 0;
}
@@ -351,13 +350,11 @@ void handleVoltageReader(float scale_in, float offset, float &outVoltage, int32_
case ADS1014_TYPE:
if (old_devType != devType) // Initilize and make ready
{
if (ads)
{
if (ads) {
delete ads;
ads = 0;
}
if (mcp)
{
if (mcp) {
delete mcp;
mcp = 0;
}
@@ -367,14 +364,12 @@ void handleVoltageReader(float scale_in, float offset, float &outVoltage, int32_
Wire2.begin();
Wire2.setClock(I2C_BUS_SPEED);
ads = new ADS1014(I2C_address, &Wire2);
if (ads != nullptr)
{
if (ads != nullptr) {
ads1014_reset(ads);
old_devType = ADS1014_TYPE;
}
}
if (ads != nullptr)
{
if (ads != nullptr) {
data0 = ads->getValue();
stat = ads->isConnected() == 1 ? 0 : 1;
}
@@ -383,15 +378,14 @@ void handleVoltageReader(float scale_in, float offset, float &outVoltage, int32_
break;
}
if (stat == 0)
{ // Read good value
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
} 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
@@ -404,50 +398,52 @@ void handleVoltageReader(float scale_in, float offset, float &outVoltage, int32_
// 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)
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)
{
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()*/);
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;
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)
{
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)
{
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)
{
}
if (ohmicState == OHMIC_PROBE && inVoltage <= limitVoltage) {
sensed = 1;
}
}
else
{
} else {
ohmicState = OHMIC_IDLE;
}
}