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

Only free run, no DC or SM2 sync

Browse Source
This commit is contained in:
Hakan Bastedt
2025-10-13 22:07:05 +02:00
parent 93c48940b8
commit f253ce8482
1 changed files with 102 additions and 149 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -36,15 +36,15 @@ ADS1014 *ads1014_2 = 0;
void ads1014_reset(ADS1014 *ads) { void ads1014_reset(ADS1014 *ads) {
ads->reset(); ads->reset();
ads->begin(); ads->begin();
ads->setGain(1); // 1=4.096V ads->setGain(1); // 1=4.096V
ads->setMode(0); // 0 continuous ads->setMode(0); // 0 continuous
ads->setDataRate(6); // Max for ads101x ads->setDataRate(6); // Max for ads101x
ads->readADC_Differential_0_1(); // This is the value we are interested in ads->readADC_Differential_0_1(); // This is the value we are interested in
} }
#include <queue> #include <queue>
class OhmicSensing { class OhmicSensing {
public: public:
void handle(uint8_t voltageState, float inVoltage, float limitVoltage, void handle(uint8_t voltageState, float inVoltage, float limitVoltage,
float voltageDropLimit, uint32_t setupTime, uint8_t enabled, float voltageDropLimit, uint32_t setupTime, uint8_t enabled,
uint8_t &sensed); uint8_t &sensed);
@@ -58,9 +58,7 @@ class OhmicSensing {
OHMIC_PROBE_DONE OHMIC_PROBE_DONE
}; };
OhmicStates ohmicState = OHMIC_IDLE; OhmicStates ohmicState = OHMIC_IDLE;
uint32_t setupTimeSoFar = 0; uint64_t startTime;
uint32_t probingTime = 0;
uint16_t senseOnTime = 0;
float_t oldVoltage = 0.0; float_t oldVoltage = 0.0;
std::queue<float> voltages; std::queue<float> voltages;
float_t refVoltage; float_t refVoltage;
@@ -68,6 +66,9 @@ class OhmicSensing {
OhmicSensing Ohm1; OhmicSensing Ohm1;
OhmicSensing Ohm2; OhmicSensing Ohm2;
// DEBUG
volatile int saveALEvent;
void handleVoltageReader(float scale_in, float offset, float &outVoltage, void handleVoltageReader(float scale_in, float offset, float &outVoltage,
int32_t &outRaw, float &oldVoltage, float &oldRaw, int32_t &outRaw, float &oldVoltage, float &oldRaw,
uint8_t devType, int8_t &old_devType, uint8_t devType, int8_t &old_devType,
@@ -86,10 +87,10 @@ void lowpassFilter(float &oldLowPassGain,
#define bitflip(byte, nbit) ((byte) ^= (1 << (nbit))) #define bitflip(byte, nbit) ((byte) ^= (1 << (nbit)))
#define bitcheck(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); 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 // Update digital output pins
for (int i = 0; i < sizeof(outputPin); i++) for (int i = 0; i < sizeof(outputPin); i++)
@@ -101,7 +102,7 @@ float oldLowPassFilteredVoltage_1 = 0, oldLowPassFilteredVoltage_2 = 0;
uint32_t oldLowpassFilterPoleFrequency_1 = 0, uint32_t oldLowpassFilterPoleFrequency_1 = 0,
oldLowpassFilterPoleFrequency_2 = 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_1 = 0.0, validVoltage0_1 = 0.0;
static float validData0_2 = 0.0, validVoltage0_2 = 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, .post_object_download_hook = NULL,
.rxpdo_override = NULL, .rxpdo_override = NULL,
.txpdo_override = NULL, .txpdo_override = NULL,
.esc_hw_interrupt_enable = ESC_interrupt_enable, .esc_hw_interrupt_enable = NULL,
.esc_hw_interrupt_disable = ESC_interrupt_disable, .esc_hw_interrupt_disable = NULL,
.esc_hw_eep_handler = NULL, .esc_hw_eep_handler = NULL,
.esc_check_dc_handler = dc_checker, .esc_check_dc_handler = dc_checker,
}; };
@@ -200,7 +201,7 @@ void setup(void) {
ecat_slv_init(&config); ecat_slv_init(&config);
#endif #endif
#if 0 // Uncomment for commissioning tests #if 0 // Uncomment for commissioning tests
// #define only one of the below // #define only one of the below
#define ADS1xxx #define ADS1xxx
#undef ADC_MCP3221 #undef ADC_MCP3221
@@ -256,69 +257,10 @@ void setup(void) {
void loop(void) { void loop(void) {
#ifdef ECAT #ifdef ECAT
uint64_t dTime; ecat_slv();
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 #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 // Setup of DC
uint16_t dc_checker(void) { uint16_t dc_checker(void) {
// Indicate we run DC // Indicate we run DC
@@ -333,87 +275,89 @@ void handleVoltageReader(float scale_in, float offset, float &outVoltage,
MyMCP3221 *&mcp, uint8_t I2C_address, MyMCP3221 *&mcp, uint8_t I2C_address,
uint32_t &I2C_restarts) { uint32_t &I2C_restarts) {
float scale = scale_in; float scale = scale_in;
if (scale == 0.0) scale = 1.0; if (scale == 0.0)
scale = 1.0;
int stat = 1, data0; int stat = 1, data0;
switch (devType) { switch (devType) {
case 0: // Not configured. case 0: // Not configured.
outStatus = 0; outStatus = 0;
stat = data0 = 0; stat = data0 = 0;
break; break;
case MCP3221_TYPE: case MCP3221_TYPE:
if (old_devType != devType) // Initilize and make ready if (old_devType != devType) // Initilize and make ready
{ {
if (ads) { if (ads) {
delete ads; delete ads;
ads = 0; 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;
} }
data0 = mcp->getData(); if (mcp) {
stat = mcp->ping(); delete mcp;
break; mcp = 0;
case ADS1014_TYPE: }
if (old_devType != devType) // Initilize and make ready Wire2.end();
{ Wire2.begin();
if (ads) { Wire2.setClock(I2C_BUS_SPEED);
delete ads; mcp = new MyMCP3221(I2C_address, &Wire2);
ads = 0; old_devType = mcp ? MCP3221_TYPE : -1;
} }
if (mcp) { data0 = mcp->getData();
delete mcp; stat = mcp->ping();
mcp = 0; break;
} case ADS1014_TYPE:
old_devType = 0; 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.end();
Wire2.begin(); Wire2.begin();
Wire2.setClock(I2C_BUS_SPEED); Wire2.setClock(I2C_BUS_SPEED);
ads = new ADS1014(I2C_address, &Wire2); ads = new ADS1014(I2C_address, &Wire2);
if (ads != nullptr) {
ads1014_reset(ads);
old_devType = ADS1014_TYPE;
}
}
if (ads != nullptr) { if (ads != nullptr) {
data0 = ads->getValue(); ads1014_reset(ads);
stat = ads->isConnected() == 1 ? 0 : 1; old_devType = ADS1014_TYPE;
} }
break; }
default: // Not supported if (ads != nullptr) {
break; data0 = ads->getValue();
stat = ads->isConnected() == 1 ? 0 : 1;
}
break;
default: // Not supported
break;
} }
if (stat == 0) { // Read good value if (stat == 0) { // Read good value
outVoltage = scale * data0 + offset; // outVoltage = scale * data0 + offset; //
outRaw = data0; // Raw voltage, read by ADC outRaw = data0; // Raw voltage, read by ADC
oldVoltage = outVoltage; oldVoltage = outVoltage;
oldRaw = data0; oldRaw = data0;
} else { // Didn't read a good value. Return a hopefully useful value and } else { // Didn't read a good value. Return a hopefully useful value and
// restart // restart
// the I2C bus // the I2C bus
outVoltage = oldVoltage; // Use value from previous call outVoltage = oldVoltage; // Use value from previous call
outRaw = oldRaw; outRaw = oldRaw;
// Reset wire here // Reset wire here
Wire2.end(); Wire2.end();
Wire2.begin(); Wire2.begin();
Wire2.setClock(I2C_BUS_SPEED); Wire2.setClock(I2C_BUS_SPEED);
I2C_restarts++; 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 // mcp3221 has no reset, reset the I2C bus is the best we can do
} }
readStat = stat; readStat = stat;
outStatus = outStatus =
I2C_restarts + (stat << 28); // Put status as bits 28-31, the lower are I2C_restarts + (stat << 28); // Put status as bits 28-31, the lower are
// number of restarts (restart attempts) // number of restarts (restart attempts)
} }
void lowpassFilter(float &oldLowPassGain, void lowpassFilter(float &oldLowPassGain,
@@ -431,7 +375,7 @@ void lowpassFilter(float &oldLowPassGain,
oldLowpassFilterPoleFrequency = LowpassFilterPoleFrequency; oldLowpassFilterPoleFrequency = LowpassFilterPoleFrequency;
} }
if (inVoltage < LowPassFilterThresholdVoltage) if (inVoltage < LowPassFilterThresholdVoltage)
outFilteredVoltage = inVoltage; // Just forward outFilteredVoltage = inVoltage; // Just forward
else else
outFilteredVoltage = oldLowPassFilteredVoltage + outFilteredVoltage = oldLowPassFilteredVoltage +
(inVoltage - oldLowPassFilteredVoltage) * gain; (inVoltage - oldLowPassFilteredVoltage) * gain;
@@ -444,39 +388,47 @@ void OhmicSensing::handle(uint8_t voltageState, float inVoltage,
uint32_t setupTime, uint8_t enabled, uint32_t setupTime, uint8_t enabled,
uint8_t &sensed) { uint8_t &sensed) {
sensed = 0; sensed = 0;
uint64_t dTime;
Obj.Out_Unit1.RawData = ohmicState;
if (enabled && voltageState == 0) { if (enabled && voltageState == 0) {
if (ohmicState == OHMIC_IDLE && inVoltage > limitVoltage) { if (ohmicState == OHMIC_IDLE && inVoltage > limitVoltage) {
ohmicState = OHMIC_SETUP; ohmicState = OHMIC_SETUP;
setupTimeSoFar = 0; startTime = longTime.extendTime(micros());
while (!voltages.empty()) voltages.pop(); // Remove history while (!voltages.empty())
voltages.pop(); // Remove history
return; return;
} }
if (ohmicState == OHMIC_SETUP) { if (ohmicState == OHMIC_SETUP) {
if (setupTimeSoFar++ > setupTime) { dTime = longTime.extendTime(micros()) - startTime;
Obj.Out_Unit2.RawData = dTime;
if (dTime > setupTime * 1000) {
ohmicState = OHMIC_PROBE; ohmicState = OHMIC_PROBE;
probingTime = 0; startTime = longTime.extendTime(micros());
oldVoltage = 0.0; oldVoltage = 0.0;
refVoltage = inVoltage; // RefVoltage = voltage at end of setup refVoltage = inVoltage; // RefVoltage = voltage at end of setup
return; return;
} }
} }
if (ohmicState == OHMIC_PROBE) { if (ohmicState == OHMIC_PROBE) {
dTime = longTime.extendTime(micros()) - startTime;
Obj.Out_Unit2.RawData = dTime;
voltages.push(inVoltage); voltages.push(inVoltage);
while (voltages.size() > N_VOLTAGES) voltages.pop(); // Only N_VOLTAGES while (voltages.size() > N_VOLTAGES)
if (probingTime++ > 30000) { // Go to IDLE after 30 seconds voltages.pop(); // Only N_VOLTAGES
if (dTime > 30000000) { // Go to IDLE after 30 seconds
ohmicState = OHMIC_IDLE; ohmicState = OHMIC_IDLE;
return; return;
} }
if ((inVoltage <= limitVoltage) || // Below starting threshold if ((inVoltage <= limitVoltage) || // Below starting threshold
(fabs(voltageDropLimit) > 1e-3 && (fabs(voltageDropLimit) > 1e-3 &&
refVoltage - inVoltage >= refVoltage - inVoltage >=
voltageDropLimit) || // Delta below refVoltage voltageDropLimit) || // Delta below refVoltage
(fabs(voltageDropLimit) > 1e-3 && // Immediate drop (fabs(voltageDropLimit) > 1e-3 && // Immediate drop
oldVoltage - inVoltage >= voltageDropLimit) || oldVoltage - inVoltage >= voltageDropLimit) ||
(fabs(voltageDropLimit) > 1e-3 && // Drop over 3 cycles (fabs(voltageDropLimit) > 1e-3 && // Drop over 3 cycles
voltages.front() - voltages.back() > voltageDropLimit)) { voltages.front() - voltages.back() > voltageDropLimit)) {
sensed = 1; sensed = 1;
senseOnTime = 0; startTime = longTime.extendTime(micros());
ohmicState = OHMIC_PROBED_ON; ohmicState = OHMIC_PROBED_ON;
} }
oldVoltage = inVoltage; oldVoltage = inVoltage;
@@ -484,7 +436,9 @@ void OhmicSensing::handle(uint8_t voltageState, float inVoltage,
} }
if (ohmicState == OHMIC_PROBED_ON) { if (ohmicState == OHMIC_PROBED_ON) {
sensed = 1; sensed = 1;
if (senseOnTime++ >= 100) { dTime = longTime.extendTime(micros()) - startTime;
Obj.Out_Unit2.RawData = dTime;
if (dTime > 100000) {
sensed = 0; sensed = 0;
ohmicState = OHMIC_PROBE_DONE; ohmicState = OHMIC_PROBE_DONE;
} }
@@ -492,7 +446,6 @@ void OhmicSensing::handle(uint8_t voltageState, float inVoltage,
} }
if (ohmicState == OHMIC_PROBE_DONE) { if (ohmicState == OHMIC_PROBE_DONE) {
sensed = 0; sensed = 0;
if (!enabled) ohmicState = OHMIC_IDLE;
return; return;
} }
} else { } else {