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 <Arduino.h>
#include <stdio.h> #include <stdio.h>
extern "C" extern "C" {
{
#include "ecat_slv.h" #include "ecat_slv.h"
#include "utypes.h" #include "utypes.h"
}; };
@@ -13,7 +12,8 @@ volatile uint64_t irqTime = 0;
HardwareSerial Serial1(PA10, PA9); 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}; uint8_t outputPin[] = {PE10, PE9, PE8, PE7};
const uint32_t I2C_BUS_SPEED = 400000; const uint32_t I2C_BUS_SPEED = 400000;
@@ -33,30 +33,31 @@ MyMCP3221 *mcp3221_2 = 0;
ADS1014 *ads1014_1 = 0; ADS1014 *ads1014_1 = 0;
ADS1014 *ads1014_2 = 0; 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
} }
void handleVoltageReader(float scale_in, float offset, float &outVoltage, int32_t &outRaw, void handleVoltageReader(float scale_in, float offset, float &outVoltage,
float &oldVoltage, float &oldRaw, int32_t &outRaw, float &oldVoltage, float &oldRaw,
uint8_t devType, int8_t &old_devType, uint8_t &readStat, uint32_t &outStatus, uint8_t devType, int8_t &old_devType,
ADS1014 *&ads, MyMCP3221 *&mcp, uint8_t I2C_address, uint32_t &I2C_restarts); uint8_t &readStat, uint32_t &outStatus, ADS1014 *&ads,
MyMCP3221 *&mcp, uint8_t I2C_address,
uint32_t &I2C_restarts);
void lowpassFilter(float &oldLowPassGain, void lowpassFilter(float &oldLowPassGain,
uint32_t &oldLowpassFilterPoleFrequency, uint32_t &oldLowpassFilterPoleFrequency,
float &oldLowPassFilteredVoltage, float &oldLowPassFilteredVoltage,
uint32_t LowpassFilterPoleFrequency, uint32_t LowpassFilterPoleFrequency,
float LowPassFilterThresholdVoltage, float LowPassFilterThresholdVoltage, float inVoltage,
float inVoltage,
float &outFilteredVoltage); float &outFilteredVoltage);
void handleOhmicSensing(uint8_t &ohmicState, uint8_t voltageState, void handleOhmicSensing(uint8_t &ohmicState, uint8_t voltageState,
float inVoltage, float limitVoltage, float inVoltage, float limitVoltage, uint32_t setupTime,
uint8_t enabled, uint8_t &sensed); uint32_t &setupTimeSoFar, uint8_t enabled,
uint8_t &sensed);
#define bitset(byte, nbit) ((byte) |= (1 << (nbit))) #define bitset(byte, nbit) ((byte) |= (1 << (nbit)))
#define bitclear(byte, nbit) ((byte) &= ~(1 << (nbit))) #define bitclear(byte, nbit) ((byte) &= ~(1 << (nbit)))
#define bitflip(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 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++)
digitalWrite(outputPin[i], bitcheck(Obj.Output4, i) ? HIGH : LOW); digitalWrite(outputPin[i], bitcheck(Obj.Output4, i) ? HIGH : LOW);
} }
float oldLowPassGain_1 = 0, oldLowPassGain_2 = 0; float oldLowPassGain_1 = 0, oldLowPassGain_2 = 0;
float oldLowPassFilteredVoltage_1 = 0, oldLowPassFilteredVoltage_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 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;
uint8_t stat_1, stat_2; uint8_t stat_1, stat_2;
for (int i = 0; i < sizeof(inputPin); i++) 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,
validVoltage0_1, validData0_1, Obj.Out_Unit1.CalculatedVoltage, Obj.Out_Unit1.RawData,
Obj.Settings_Unit1.I2C_devicetype, old_I2Cdevice_1, stat_1, Obj.Out_Unit1.Status, validVoltage0_1, validData0_1,
ads1014_1, mcp3221_1, Obj.Settings_Unit1.I2C_address, I2C_restarts_1); Obj.Settings_Unit1.I2C_devicetype, old_I2Cdevice_1,
handleVoltageReader(Obj.In_Unit2.VoltageScale, Obj.In_Unit2.VoltageOffset, Obj.Out_Unit2.CalculatedVoltage, Obj.Out_Unit2.RawData, stat_1, Obj.Out_Unit1.Status, ads1014_1, mcp3221_1,
validVoltage0_2, validData0_2, Obj.Settings_Unit1.I2C_address, I2C_restarts_1);
Obj.Settings_Unit2.I2C_devicetype, old_I2Cdevice_2, stat_2, Obj.Out_Unit2.Status, handleVoltageReader(Obj.In_Unit2.VoltageScale, Obj.In_Unit2.VoltageOffset,
ads1014_2, mcp3221_2, Obj.Settings_Unit2.I2C_address, I2C_restarts_2); Obj.Out_Unit2.CalculatedVoltage, Obj.Out_Unit2.RawData,
lowpassFilter(oldLowPassGain_1, validVoltage0_2, validData0_2,
oldLowpassFilterPoleFrequency_1, Obj.Settings_Unit2.I2C_devicetype, old_I2Cdevice_2,
oldLowPassFilteredVoltage_1, stat_2, Obj.Out_Unit2.Status, ads1014_2, mcp3221_2,
Obj.Settings_Unit1.LowpassFilterPoleFrequency, Obj.Settings_Unit2.I2C_address, I2C_restarts_2);
Obj.In_Unit1.LowPassFilterThresholdVoltage, lowpassFilter(oldLowPassGain_1, oldLowpassFilterPoleFrequency_1,
Obj.Out_Unit1.CalculatedVoltage, oldLowPassFilteredVoltage_1,
Obj.Out_Unit1.LowpassFilteredVoltage); Obj.Settings_Unit1.LowpassFilterPoleFrequency,
lowpassFilter(oldLowPassGain_2, Obj.In_Unit1.LowPassFilterThresholdVoltage,
oldLowpassFilterPoleFrequency_2, Obj.Out_Unit1.CalculatedVoltage,
oldLowPassFilteredVoltage_2, Obj.Out_Unit1.LowpassFilteredVoltage);
Obj.Settings_Unit2.LowpassFilterPoleFrequency, lowpassFilter(oldLowPassGain_2, oldLowpassFilterPoleFrequency_2,
Obj.In_Unit2.LowPassFilterThresholdVoltage, oldLowPassFilteredVoltage_2,
Obj.Out_Unit2.CalculatedVoltage, Obj.Settings_Unit2.LowpassFilterPoleFrequency,
Obj.Out_Unit2.LowpassFilteredVoltage); Obj.In_Unit2.LowPassFilterThresholdVoltage,
Obj.Out_Unit2.CalculatedVoltage,
Obj.Out_Unit2.LowpassFilteredVoltage);
#define OHMIC_IDLE 0 #define OHMIC_IDLE 0
#define OHMIC_PROBE 1 #define OHMIC_SETUP 1
static uint8_t ohmicState_1 = OHMIC_IDLE; #define OHMIC_PROBE 2
static uint8_t ohmicState_2 = OHMIC_IDLE; 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, handleOhmicSensing(ohmicState_1, stat_1, Obj.Out_Unit1.CalculatedVoltage,
Obj.Out_Unit1.CalculatedVoltage, Obj.In_Unit1.OhmicSensingVoltageLimit,
Obj.In_Unit1.OhmicSensingVoltageLimit, Obj.In_Unit1.OhmicSensingSetupTime, setupTimeSoFar_1,
Obj.In_Unit1.EnableOhmicSensing, Obj.In_Unit1.EnableOhmicSensing,
Obj.Out_Unit1.OhmicSensingSensed); Obj.Out_Unit1.OhmicSensingSensed);
handleOhmicSensing(ohmicState_2, stat_2, handleOhmicSensing(ohmicState_2, stat_2, Obj.Out_Unit2.CalculatedVoltage,
Obj.Out_Unit2.CalculatedVoltage, Obj.In_Unit2.OhmicSensingVoltageLimit,
Obj.In_Unit2.OhmicSensingVoltageLimit, Obj.In_Unit2.OhmicSensingSetupTime, setupTimeSoFar_2,
Obj.In_Unit2.EnableOhmicSensing, Obj.In_Unit2.EnableOhmicSensing,
Obj.Out_Unit2.OhmicSensingSensed); Obj.Out_Unit2.OhmicSensingSensed);
} }
void ESC_interrupt_enable(uint32_t mask); void ESC_interrupt_enable(uint32_t mask);
@@ -130,54 +138,51 @@ void ESC_interrupt_disable(uint32_t mask);
uint16_t dc_checker(void); uint16_t dc_checker(void);
void sync0Handler(void); void sync0Handler(void);
static esc_cfg_t config = static esc_cfg_t config = {
{ .user_arg = NULL,
.user_arg = NULL, .use_interrupt = 1,
.use_interrupt = 1, .watchdog_cnt = 150,
.watchdog_cnt = 150, .set_defaults_hook = NULL,
.set_defaults_hook = NULL, .pre_state_change_hook = NULL,
.pre_state_change_hook = NULL, .post_state_change_hook = NULL,
.post_state_change_hook = NULL, .application_hook = NULL,
.application_hook = NULL, .safeoutput_override = NULL,
.safeoutput_override = NULL, .pre_object_download_hook = NULL,
.pre_object_download_hook = NULL, .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 = ESC_interrupt_enable, .esc_hw_interrupt_disable = ESC_interrupt_disable,
.esc_hw_interrupt_disable = ESC_interrupt_disable, .esc_hw_eep_handler = NULL,
.esc_hw_eep_handler = NULL, .esc_check_dc_handler = dc_checker,
.esc_check_dc_handler = dc_checker,
}; };
volatile byte serveIRQ = 0; volatile byte serveIRQ = 0;
void setup(void) void setup(void) {
{ Serial1.begin(115200);
Serial1.begin(115200);
for (int i = 0; i < sizeof(inputPin); i++) for (int i = 0; i < sizeof(inputPin); i++)
pinMode(inputPin[i], INPUT_PULLDOWN); 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);
pinMode(outputPin[i], OUTPUT); digitalWrite(outputPin[i], LOW);
digitalWrite(outputPin[i], LOW); }
} // Debug leds
// Debug leds pinMode(PB4, OUTPUT);
pinMode(PB4, OUTPUT); pinMode(PB5, OUTPUT);
pinMode(PB5, OUTPUT); pinMode(PB6, OUTPUT);
pinMode(PB6, OUTPUT); pinMode(PB7, OUTPUT);
pinMode(PB7, OUTPUT); digitalWrite(PB4, LOW);
digitalWrite(PB4, LOW); digitalWrite(PB5, LOW);
digitalWrite(PB5, LOW); digitalWrite(PB6, LOW);
digitalWrite(PB6, LOW); digitalWrite(PB7, LOW);
digitalWrite(PB7, LOW);
Wire2.begin(); Wire2.begin();
Wire2.setClock(I2C_BUS_SPEED); Wire2.setClock(I2C_BUS_SPEED);
#ifdef ECAT #ifdef ECAT
ecat_slv_init(&config); ecat_slv_init(&config);
#endif #endif
#if 0 // Uncomment for commissioning tests #if 0 // Uncomment for commissioning tests
@@ -234,220 +239,211 @@ void setup(void)
#endif #endif
} }
void loop(void) void loop(void) {
{
#ifdef ECAT #ifdef ECAT
uint64_t dTime; uint64_t dTime;
if (serveIRQ) if (serveIRQ) {
{ DIG_process(ALEventIRQ, DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
DIG_process(ALEventIRQ, DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG | DIG_PROCESS_APP_HOOK_FLAG |
DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG); DIG_PROCESS_INPUTS_FLAG);
serveIRQ = 0; serveIRQ = 0;
ESCvar.PrevTime = ESCvar.Time; ESCvar.PrevTime = ESCvar.Time;
ecat_slv_poll(); ecat_slv_poll();
} }
dTime = longTime.extendTime(micros()) - irqTime; dTime = longTime.extendTime(micros()) - irqTime;
if (dTime > 5000) // Not doing interrupts - handle free-run if (dTime > 5000) // Not doing interrupts - handle free-run
ecat_slv(); ecat_slv();
#endif #endif
} }
void sync0Handler(void) void sync0Handler(void) {
{ ALEventIRQ = ESC_ALeventread();
ALEventIRQ = ESC_ALeventread(); // if (ALEventIRQ & ESCREG_ALEVENT_SM2)
// if (ALEventIRQ & ESCREG_ALEVENT_SM2) {
{ irqTime = longTime.extendTime(micros());
irqTime = longTime.extendTime(micros()); serveIRQ = 1;
serveIRQ = 1; }
}
} }
// Enable SM2 interrupts // 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
// Enable interrupt for SYNC0 or SM2 or SM3 uint32_t user_int_mask =
uint32_t user_int_mask = ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM2 | ESCREG_ALEVENT_SM3; ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM2 | ESCREG_ALEVENT_SM3;
if (mask & user_int_mask) if (mask & user_int_mask) {
{ ESC_ALeventmaskwrite(ESC_ALeventmaskread() | (mask & user_int_mask));
ESC_ALeventmaskwrite(ESC_ALeventmaskread() | (mask & user_int_mask)); ESC_ALeventmaskwrite(ESC_ALeventmaskread() &
ESC_ALeventmaskwrite(ESC_ALeventmaskread() & ~(ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM3)); ~(ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM3));
attachInterrupt(digitalPinToInterrupt(PC3), sync0Handler, RISING); attachInterrupt(digitalPinToInterrupt(PC3), sync0Handler, RISING);
// Set LAN9252 interrupt pin driver as push-pull active high // Set LAN9252 interrupt pin driver as push-pull active high
uint32_t bits = 0x00000111; uint32_t bits = 0x00000111;
ESC_write(0x54, &bits, 4); ESC_write(0x54, &bits, 4);
// Enable LAN9252 interrupt // Enable LAN9252 interrupt
bits = 0x00000001; bits = 0x00000001;
ESC_write(0x5c, &bits, 4); ESC_write(0x5c, &bits, 4);
} }
} }
// Disable SM2 interrupts // 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
// Enable interrupt for SYNC0 or SM2 or SM3 // uint32_t user_int_mask = ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM2 |
// uint32_t user_int_mask = ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM2 | ESCREG_ALEVENT_SM3; // ESCREG_ALEVENT_SM3;
uint32_t user_int_mask = ESCREG_ALEVENT_SM2; uint32_t user_int_mask = ESCREG_ALEVENT_SM2;
if (mask & user_int_mask) if (mask & user_int_mask) {
{ // Disable interrupt from SYNC0
// Disable interrupt from SYNC0 ESC_ALeventmaskwrite(ESC_ALeventmaskread() & ~(mask & user_int_mask));
ESC_ALeventmaskwrite(ESC_ALeventmaskread() & ~(mask & user_int_mask)); detachInterrupt(digitalPinToInterrupt(PC3));
detachInterrupt(digitalPinToInterrupt(PC3)); // Disable LAN9252 interrupt
// Disable LAN9252 interrupt uint32_t bits = 0x00000000;
uint32_t bits = 0x00000000; ESC_write(0x5c, &bits, 4);
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 ESCvar.dcsync = 1;
ESCvar.dcsync = 1; return 0;
return 0;
} }
void handleVoltageReader(float scale_in, float offset, float &outVoltage, int32_t &outRaw, void handleVoltageReader(float scale_in, float offset, float &outVoltage,
float &oldVoltage, float &oldRaw, int32_t &outRaw, float &oldVoltage, float &oldRaw,
uint8_t devType, int8_t &old_devType, uint8_t &readStat, uint32_t &outStatus, uint8_t devType, int8_t &old_devType,
ADS1014 *&ads, MyMCP3221 *&mcp, uint8_t I2C_address, uint32_t &I2C_restarts) uint8_t &readStat, uint32_t &outStatus, ADS1014 *&ads,
{ MyMCP3221 *&mcp, uint8_t I2C_address,
float scale = scale_in; uint32_t &I2C_restarts) {
if (scale == 0.0) float scale = scale_in;
scale = 1.0; if (scale == 0.0)
int stat = 1, data0; scale = 1.0;
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;
{ ads = 0;
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;
} }
data0 = mcp->getData(); if (mcp) {
stat = mcp->ping(); delete mcp;
break; mcp = 0;
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.end();
Wire2.begin(); Wire2.begin();
Wire2.setClock(I2C_BUS_SPEED); Wire2.setClock(I2C_BUS_SPEED);
I2C_restarts++; mcp = new MyMCP3221(I2C_address, &Wire2);
if (devType == ADS1014_TYPE && ads != nullptr) old_devType = mcp ? MCP3221_TYPE : -1;
ads1014_reset(ads); }
// mcp3221 has no reset, reset the I2C bus is the best we can do data0 = mcp->getData();
} stat = mcp->ping();
readStat = stat; break;
outStatus = I2C_restarts + (stat << 28); // Put status as bits 28-31, the lower are number of restarts (restart attempts) 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, void lowpassFilter(float &oldLowPassGain,
uint32_t &oldLowpassFilterPoleFrequency, uint32_t &oldLowpassFilterPoleFrequency,
float &oldLowPassFilteredVoltage, float &oldLowPassFilteredVoltage,
uint32_t LowpassFilterPoleFrequency, uint32_t LowpassFilterPoleFrequency,
float LowPassFilterThresholdVoltage, float LowPassFilterThresholdVoltage, float inVoltage,
float inVoltage, float &outFilteredVoltage) {
float &outFilteredVoltage) // Low pass filter. See lowpass in linuxcnc doc
{ float gain = oldLowPassGain;
// Low pass filter. See lowpass in linuxcnc doc if (oldLowpassFilterPoleFrequency != LowpassFilterPoleFrequency) {
float gain = oldLowPassGain; gain = 1 - expf(-2.0 * M_PI * LowpassFilterPoleFrequency *
if (oldLowpassFilterPoleFrequency != LowpassFilterPoleFrequency) 0.001 /*1.0e-9 * ESC_SYNC0cycletime()*/);
{ oldLowPassGain = gain;
gain = 1 - expf(-2.0 * M_PI * LowpassFilterPoleFrequency * 0.001 /*1.0e-9 * ESC_SYNC0cycletime()*/); oldLowpassFilterPoleFrequency = LowpassFilterPoleFrequency;
oldLowPassGain = gain; }
oldLowpassFilterPoleFrequency = LowpassFilterPoleFrequency; if (inVoltage < LowPassFilterThresholdVoltage)
} outFilteredVoltage = inVoltage; // Just forward
if (inVoltage < LowPassFilterThresholdVoltage) else
outFilteredVoltage = inVoltage; // Just forward outFilteredVoltage = oldLowPassFilteredVoltage +
else (inVoltage - oldLowPassFilteredVoltage) * gain;
outFilteredVoltage = oldLowPassFilteredVoltage + (inVoltage - oldLowPassFilteredVoltage) * gain; oldLowPassFilteredVoltage = outFilteredVoltage;
oldLowPassFilteredVoltage = outFilteredVoltage;
} }
void handleOhmicSensing(uint8_t &ohmicState, uint8_t voltageState, void handleOhmicSensing(uint8_t &ohmicState, uint8_t voltageState,
float inVoltage, float limitVoltage, float inVoltage, float limitVoltage, uint32_t setupTime,
uint8_t enabled, uint8_t &sensed) uint32_t &setupTimeSoFar, uint8_t enabled,
{ uint8_t &sensed) {
sensed = 0; sensed = 0;
if (enabled && voltageState == 0) if (enabled && voltageState == 0) {
{ if (ohmicState == OHMIC_IDLE && inVoltage > limitVoltage) {
if (ohmicState == OHMIC_IDLE && inVoltage > limitVoltage) ohmicState = OHMIC_SETUP;
{ setupTimeSoFar = 0;
ohmicState = OHMIC_PROBE; }
if (ohmicState == OHMIC_SETUP) {
if (setupTimeSoFar++ > setupTime) {
ohmicState = OHMIC_PROBE;
} }
if (ohmicState == OHMIC_PROBE && inVoltage <= limitVoltage) }
{ if (ohmicState == OHMIC_PROBE && inVoltage <= limitVoltage) {
sensed = 1; sensed = 1;
} }
} } else {
else ohmicState = OHMIC_IDLE;
{ }
ohmicState = OHMIC_IDLE;
}
} }