mirror of
https://github.com/dalathegreat/Battery-Emulator.git
synced 2025-10-06 03:50:13 +02:00
Final fixes from andy
This commit is contained in:
Daniel
parent
ca21dae6ba
commit
90bb3fe843
7 changed files with 435 additions and 241 deletions
|
|
@ -11,10 +11,22 @@ const uint8_t sendingNumVariables = INVERTER_SEND_NUM_VARIABLES;
|
|||
const uint8_t sendingNumVariables = 0;
|
||||
#endif
|
||||
|
||||
// txid,rxid, num_send,num_recv
|
||||
SerialDataLink dataLinkReceive(Serial2, 0, 0x01, sendingNumVariables,
|
||||
#ifdef TESTBENCH
|
||||
// In the testbench environment, the receiver uses Serial3
|
||||
#define SerialReceiver Serial3
|
||||
#else
|
||||
// In the production environment, the receiver uses Serial2
|
||||
#define SerialReceiver Serial2
|
||||
#endif
|
||||
|
||||
#define REPORT_SDL_DATA 1
|
||||
|
||||
// txid,rxid, num_send,num_recv
|
||||
SerialDataLink dataLinkReceive(SerialReceiver, 0, 0x01, sendingNumVariables,
|
||||
INVERTER_RECV_NUM_VARIABLES); // ...
|
||||
|
||||
static bool batteryFault = false; // used locally - mainly to indicate Battery CAN failure
|
||||
|
||||
void __getData() {
|
||||
SOC = (uint16_t)dataLinkReceive.getReceivedData(0);
|
||||
StateOfHealth = (uint16_t)dataLinkReceive.getReceivedData(1);
|
||||
|
|
@ -24,14 +36,20 @@ void __getData() {
|
|||
remaining_capacity_Wh = (uint16_t)dataLinkReceive.getReceivedData(5);
|
||||
max_target_discharge_power = (uint16_t)dataLinkReceive.getReceivedData(6);
|
||||
max_target_charge_power = (uint16_t)dataLinkReceive.getReceivedData(7);
|
||||
bms_status = (uint16_t)dataLinkReceive.getReceivedData(8);
|
||||
uint16_t _bms_status = (uint16_t)dataLinkReceive.getReceivedData(8);
|
||||
bms_status = _bms_status;
|
||||
bms_char_dis_status = (uint16_t)dataLinkReceive.getReceivedData(9);
|
||||
stat_batt_power = (uint16_t)dataLinkReceive.getReceivedData(10);
|
||||
temperature_min = (uint16_t)dataLinkReceive.getReceivedData(11);
|
||||
temperature_max = (uint16_t)dataLinkReceive.getReceivedData(12);
|
||||
cell_max_voltage = (uint16_t)dataLinkReceive.getReceivedData(13);
|
||||
cell_min_voltage = (uint16_t)dataLinkReceive.getReceivedData(14);
|
||||
batteryAllowsContactorClosing = (uint16_t)dataLinkReceive.getReceivedData(15);
|
||||
LFP_Chemistry = (bool)dataLinkReceive.getReceivedData(15);
|
||||
batteryAllowsContactorClosing = (uint16_t)dataLinkReceive.getReceivedData(16);
|
||||
|
||||
batteryFault = false;
|
||||
if (_bms_status == FAULT)
|
||||
batteryFault = true;
|
||||
}
|
||||
|
||||
void updateData() {
|
||||
|
|
@ -47,13 +65,16 @@ void updateData() {
|
|||
*/
|
||||
|
||||
void manageSerialLinkReceiver() {
|
||||
|
||||
static bool lasterror = false;
|
||||
static unsigned long last_minutesLost = 0;
|
||||
static unsigned long lastGood;
|
||||
static uint16_t lastGoodMaxCharge;
|
||||
static uint16_t lastGoodMaxDischarge;
|
||||
static bool initLink = false;
|
||||
|
||||
static unsigned long reportTime = 0;
|
||||
static uint16_t reads = 0;
|
||||
static uint16_t errors = 0;
|
||||
unsigned long currentTime = millis();
|
||||
|
||||
if (!initLink) {
|
||||
|
|
@ -69,21 +90,26 @@ void manageSerialLinkReceiver() {
|
|||
|
||||
if (readError) {
|
||||
Serial.print(currentTime);
|
||||
Serial.println(" - ERROR: Serial Data Link - Read Error");
|
||||
Serial.println(" - ERROR: SerialDataLink - Read Error");
|
||||
lasterror = true;
|
||||
} else {
|
||||
if (lasterror) {
|
||||
lasterror = false;
|
||||
Serial.print(currentTime);
|
||||
Serial.println(" - RECOVERY: Serial Data Link - Read GOOD");
|
||||
}
|
||||
errors++;
|
||||
}
|
||||
|
||||
if (dataLinkReceive.checkNewData(true)) // true = clear Flag
|
||||
{
|
||||
__getData();
|
||||
reads++;
|
||||
lastGoodMaxCharge = max_target_charge_power;
|
||||
lastGoodMaxDischarge = max_target_discharge_power;
|
||||
lastGood = currentTime;
|
||||
//--- if BatteryFault then assume Data is stale
|
||||
if (!batteryFault)
|
||||
lastGood = currentTime;
|
||||
//bms_status = ACTIVE; // just testing
|
||||
if (lasterror) {
|
||||
lasterror = false;
|
||||
Serial.print(currentTime);
|
||||
Serial.println(" - RECOVERY: SerialDataLink - Read GOOD");
|
||||
}
|
||||
}
|
||||
|
||||
unsigned long minutesLost = (currentTime - lastGood) / 60000UL;
|
||||
|
|
@ -93,6 +119,7 @@ void manageSerialLinkReceiver() {
|
|||
max_target_charge_power = (lastGoodMaxCharge * (4 - minutesLost)) / 4;
|
||||
max_target_discharge_power = (lastGoodMaxDischarge * (4 - minutesLost)) / 4;
|
||||
} else {
|
||||
// Times Up -
|
||||
max_target_charge_power = 0;
|
||||
max_target_discharge_power = 0;
|
||||
bms_status = 4; //Fault state
|
||||
|
|
@ -103,7 +130,11 @@ void manageSerialLinkReceiver() {
|
|||
if (minutesLost != last_minutesLost) {
|
||||
last_minutesLost = minutesLost;
|
||||
Serial.print(currentTime);
|
||||
Serial.print(" - Minutes without data : ");
|
||||
if (batteryFault) {
|
||||
Serial.print("Battery Fault (minutes) : ");
|
||||
} else {
|
||||
Serial.print(" - Minutes without data : ");
|
||||
}
|
||||
Serial.print(minutesLost);
|
||||
Serial.print(", max Charge = ");
|
||||
Serial.print(max_target_charge_power);
|
||||
|
|
@ -112,6 +143,24 @@ void manageSerialLinkReceiver() {
|
|||
}
|
||||
}
|
||||
|
||||
if (currentTime - reportTime > 59999) {
|
||||
reportTime = currentTime;
|
||||
Serial.print(currentTime);
|
||||
Serial.print(" SerialDataLink-Receiver - NewData :");
|
||||
Serial.print(reads);
|
||||
Serial.print(" Errors : ");
|
||||
Serial.println(errors);
|
||||
reads = 0;
|
||||
errors = 0;
|
||||
|
||||
// --- printUsefullData();
|
||||
//Serial.print("SOC = ");
|
||||
//Serial.println(SOC);
|
||||
#ifdef REPORT_SDL_DATA
|
||||
update_values_serial_link();
|
||||
#endif
|
||||
}
|
||||
|
||||
static unsigned long updateTime = 0;
|
||||
|
||||
#ifdef INVERTER_SEND_NUM_VARIABLES
|
||||
|
|
@ -156,8 +205,12 @@ void update_values_serial_link() {
|
|||
Serial.print(cell_max_voltage);
|
||||
Serial.print(" Cell min: ");
|
||||
Serial.print(cell_min_voltage);
|
||||
Serial.print(" LFP : ");
|
||||
Serial.print(LFP_Chemistry);
|
||||
Serial.print(" batteryAllowsContactorClosing: ");
|
||||
Serial.print(batteryAllowsContactorClosing);
|
||||
Serial.print(" inverterAllowsContactorClosing: ");
|
||||
Serial.print(inverterAllowsContactorClosing);
|
||||
|
||||
Serial.println("");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -31,9 +31,23 @@ extern uint16_t temperature_max; //C+1, Goes thru convert2unsignedint16 funct
|
|||
extern uint16_t cell_max_voltage; //mV, 0-4350
|
||||
extern uint16_t cell_min_voltage; //mV, 0-4350
|
||||
extern uint8_t LEDcolor; //Enum, 0-10
|
||||
extern bool batteryAllowsContactorClosing; //Bool, 1=true, 0=false
|
||||
|
||||
extern bool LFP_Chemistry;
|
||||
extern uint16_t CANerror;
|
||||
|
||||
extern bool batteryAllowsContactorClosing; //Bool, 1=true, 0=false
|
||||
|
||||
// Parameters to send to the transmitter
|
||||
extern bool inverterAllowsContactorClosing; //Bool, 1=true, 0=false
|
||||
|
||||
// Definitions for bms_status
|
||||
#define STANDBY 0
|
||||
#define INACTIVE 1
|
||||
#define DARKSTART 2
|
||||
#define ACTIVE 3
|
||||
#define FAULT 4
|
||||
#define UPDATING 5
|
||||
|
||||
void manageSerialLinkReceiver();
|
||||
void update_values_serial_link();
|
||||
|
||||
|
|
|
|||
|
|
@ -8,7 +8,7 @@
|
|||
* Will transmit max 16 int variable - receive none
|
||||
*/
|
||||
|
||||
#define BATTERY_SEND_NUM_VARIABLES 16
|
||||
#define BATTERY_SEND_NUM_VARIABLES 17
|
||||
#define BATTERY_RECV_NUM_VARIABLES 1
|
||||
|
||||
#ifdef BATTERY_RECV_NUM_VARIABLES
|
||||
|
|
@ -17,9 +17,13 @@ const uint8_t receivingNumVariables = BATTERY_RECV_NUM_VARIABLES;
|
|||
const uint8_t receivingNumVariables = 0;
|
||||
#endif
|
||||
|
||||
//#define REPORT_SDL_DATA 1
|
||||
|
||||
// txid,rxid,num_tx,num_rx
|
||||
SerialDataLink dataLinkTransmit(Serial2, 0x01, 0, BATTERY_SEND_NUM_VARIABLES, receivingNumVariables);
|
||||
|
||||
void printSendingValues();
|
||||
|
||||
void _getData() {
|
||||
inverterAllowsContactorClosing = dataLinkTransmit.getReceivedData(0);
|
||||
//var2 = dataLinkTransmit.getReceivedData(1);
|
||||
|
|
@ -30,8 +34,9 @@ void manageSerialLinkTransmitter() {
|
|||
static bool initLink = false;
|
||||
static unsigned long updateTime = 0;
|
||||
static bool lasterror = false;
|
||||
static unsigned long lastNoError = 0;
|
||||
//static unsigned long lastNoError = 0;
|
||||
static unsigned long transmitGoodSince = 0;
|
||||
static unsigned long lastGood = 0;
|
||||
|
||||
unsigned long currentTime = millis();
|
||||
|
||||
|
|
@ -49,8 +54,9 @@ void manageSerialLinkTransmitter() {
|
|||
updateTime = currentTime;
|
||||
if (!initLink) {
|
||||
initLink = true;
|
||||
transmitGoodSince = currentTime;
|
||||
// sends variables every 5000mS even if no change
|
||||
dataLinkTransmit.setUpdateInterval(5000);
|
||||
dataLinkTransmit.setUpdateInterval(10000);
|
||||
}
|
||||
bool sendError = dataLinkTransmit.checkTransmissionError(true);
|
||||
if (sendError) {
|
||||
|
|
@ -58,13 +64,26 @@ void manageSerialLinkTransmitter() {
|
|||
Serial.println(" - ERROR: Serial Data Link - SEND Error");
|
||||
lasterror = true;
|
||||
transmitGoodSince = currentTime;
|
||||
} else {
|
||||
if (lasterror) {
|
||||
lasterror = false;
|
||||
Serial.print(currentTime);
|
||||
Serial.println(" - RECOVERY: Serial Data Link - Send GOOD");
|
||||
}
|
||||
lastNoError = currentTime;
|
||||
}
|
||||
|
||||
/* new feature */
|
||||
/* @getLastAcknowledge(bool resetFlag)
|
||||
* - returns:
|
||||
* -2 NACK received from receiver
|
||||
* -1 no ACK received
|
||||
* 0 no activity
|
||||
* 1 ACK received
|
||||
* resetFlag = true will clear to 0
|
||||
*/
|
||||
|
||||
int ackReceived = dataLinkTransmit.getLastAcknowledge(true);
|
||||
if (ackReceived > 0)
|
||||
lastGood = currentTime;
|
||||
|
||||
if (lasterror && (ackReceived > 0)) {
|
||||
lasterror = false;
|
||||
Serial.print(currentTime);
|
||||
Serial.println(" - RECOVERY: Serial Data Link - Send GOOD");
|
||||
}
|
||||
|
||||
//--- reporting every 60 seconds that transmission is good
|
||||
|
|
@ -72,13 +91,24 @@ void manageSerialLinkTransmitter() {
|
|||
transmitGoodSince = currentTime;
|
||||
Serial.print(currentTime);
|
||||
Serial.println(" - Transmit Good");
|
||||
// printUsefullData();
|
||||
#ifdef REPORT_SDL_DATA
|
||||
void printSendingValues();
|
||||
#endif
|
||||
}
|
||||
|
||||
//--- report that Errors been ocurring for > 60 seconds
|
||||
if (currentTime - lastNoError > 60000) // 60 seconds
|
||||
if (currentTime - lastGood > 60000) // 60 seconds
|
||||
{
|
||||
lastGood = currentTime;
|
||||
Serial.print(currentTime);
|
||||
Serial.println(" - Transmit Failed : 60 seconds");
|
||||
// print the max_ data
|
||||
Serial.println("SerialDataLink : bms_status=4");
|
||||
Serial.println("SerialDataLink : LEDcolor = RED");
|
||||
Serial.println("SerialDataLink : max_target_discharge_power = 0");
|
||||
Serial.println("SerialDataLink : max_target_charge_power = 0");
|
||||
|
||||
bms_status = 4; //FAULT
|
||||
max_target_discharge_power = 0;
|
||||
max_target_charge_power = 0;
|
||||
|
|
@ -97,21 +127,69 @@ void manageSerialLinkTransmitter() {
|
|||
}
|
||||
*/
|
||||
|
||||
dataLinkTransmit.updateData(0, SOC);
|
||||
dataLinkTransmit.updateData(1, StateOfHealth);
|
||||
dataLinkTransmit.updateData(2, battery_voltage);
|
||||
dataLinkTransmit.updateData(3, battery_current);
|
||||
dataLinkTransmit.updateData(4, capacity_Wh);
|
||||
dataLinkTransmit.updateData(5, remaining_capacity_Wh);
|
||||
dataLinkTransmit.updateData(6, max_target_discharge_power);
|
||||
dataLinkTransmit.updateData(7, max_target_charge_power);
|
||||
dataLinkTransmit.updateData(8, bms_status);
|
||||
dataLinkTransmit.updateData(9, bms_char_dis_status);
|
||||
dataLinkTransmit.updateData(10, stat_batt_power);
|
||||
dataLinkTransmit.updateData(11, temperature_min);
|
||||
dataLinkTransmit.updateData(12, temperature_max);
|
||||
dataLinkTransmit.updateData(13, cell_max_voltage);
|
||||
dataLinkTransmit.updateData(14, cell_min_voltage);
|
||||
dataLinkTransmit.updateData(15, batteryAllowsContactorClosing);
|
||||
static unsigned long updateDataTime = 0;
|
||||
|
||||
if (currentTime - updateDataTime > 999) {
|
||||
updateDataTime = currentTime;
|
||||
dataLinkTransmit.updateData(0, SOC);
|
||||
dataLinkTransmit.updateData(1, StateOfHealth);
|
||||
dataLinkTransmit.updateData(2, battery_voltage);
|
||||
dataLinkTransmit.updateData(3, battery_current);
|
||||
dataLinkTransmit.updateData(4, capacity_Wh);
|
||||
dataLinkTransmit.updateData(5, remaining_capacity_Wh);
|
||||
dataLinkTransmit.updateData(6, max_target_discharge_power);
|
||||
dataLinkTransmit.updateData(7, max_target_charge_power);
|
||||
dataLinkTransmit.updateData(8, bms_status);
|
||||
dataLinkTransmit.updateData(9, bms_char_dis_status);
|
||||
dataLinkTransmit.updateData(10, stat_batt_power);
|
||||
dataLinkTransmit.updateData(11, temperature_min);
|
||||
dataLinkTransmit.updateData(12, temperature_max);
|
||||
dataLinkTransmit.updateData(13, cell_max_voltage);
|
||||
dataLinkTransmit.updateData(14, cell_min_voltage);
|
||||
dataLinkTransmit.updateData(15, (int16_t)LFP_Chemistry);
|
||||
dataLinkTransmit.updateData(16, batteryAllowsContactorClosing);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void printSendingValues() {
|
||||
Serial.println("Values from battery: ");
|
||||
Serial.print("SOC: ");
|
||||
Serial.print(SOC);
|
||||
Serial.print(" SOH: ");
|
||||
Serial.print(StateOfHealth);
|
||||
Serial.print(" Voltage: ");
|
||||
Serial.print(battery_voltage);
|
||||
Serial.print(" Current: ");
|
||||
Serial.print(battery_current);
|
||||
Serial.print(" Capacity: ");
|
||||
Serial.print(capacity_Wh);
|
||||
Serial.print(" Remain cap: ");
|
||||
Serial.print(remaining_capacity_Wh);
|
||||
Serial.print(" Max discharge W: ");
|
||||
Serial.print(max_target_discharge_power);
|
||||
Serial.print(" Max charge W: ");
|
||||
Serial.print(max_target_charge_power);
|
||||
Serial.print(" BMS status: ");
|
||||
Serial.print(bms_status);
|
||||
Serial.print(" BMS status dis/cha: ");
|
||||
Serial.print(bms_char_dis_status);
|
||||
Serial.print(" Power: ");
|
||||
Serial.print(stat_batt_power);
|
||||
Serial.print(" Temp min: ");
|
||||
Serial.print(temperature_min);
|
||||
Serial.print(" Temp max: ");
|
||||
Serial.print(temperature_max);
|
||||
Serial.print(" Cell max: ");
|
||||
Serial.print(cell_max_voltage);
|
||||
Serial.print(" Cell min: ");
|
||||
Serial.print(cell_min_voltage);
|
||||
Serial.print(" LFP : ");
|
||||
Serial.print(LFP_Chemistry);
|
||||
Serial.print(" batteryAllowsContactorClosing: ");
|
||||
Serial.print(batteryAllowsContactorClosing);
|
||||
Serial.print(" inverterAllowsContactorClosing: ");
|
||||
Serial.print(inverterAllowsContactorClosing);
|
||||
|
||||
Serial.println("");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -25,9 +25,22 @@ extern uint16_t temperature_max; //C+1, Goes thru convert2unsignedint16 funct
|
|||
extern uint16_t cell_max_voltage; //mV, 0-4350
|
||||
extern uint16_t cell_min_voltage; //mV, 0-4350
|
||||
extern uint8_t LEDcolor; //Enum, 0-10
|
||||
extern bool batteryAllowsContactorClosing; //Bool, 1=true, 0=false
|
||||
extern bool batteryAllowsContactorClosing; //Bool, 1=true, 0=false
|
||||
|
||||
extern bool LFP_Chemistry;
|
||||
extern uint16_t CANerror;
|
||||
|
||||
// parameters received from receiver
|
||||
extern bool inverterAllowsContactorClosing; //Bool, 1=true, 0=false
|
||||
|
||||
// Definitions for BMS status
|
||||
#define STANDBY 0
|
||||
#define INACTIVE 1
|
||||
#define DARKSTART 2
|
||||
#define ACTIVE 3
|
||||
#define FAULT 4
|
||||
#define UPDATING 5
|
||||
|
||||
void manageSerialLinkTransmitter();
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -109,6 +109,16 @@ bool SerialDataLink::checkTransmissionError(bool resetFlag)
|
|||
return currentStatus;
|
||||
}
|
||||
|
||||
int SerialDataLink::getLastAcknowledge(bool resetFlag)
|
||||
{
|
||||
int result = lastAcknowledgeStatus;
|
||||
if (resetFlag)
|
||||
{
|
||||
lastAcknowledgeStatus = 0; // Reset to default state
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
bool SerialDataLink::checkReadError(bool reset)
|
||||
{
|
||||
bool error = readError;
|
||||
|
|
@ -134,115 +144,135 @@ void SerialDataLink::muteACK(bool mute)
|
|||
|
||||
void SerialDataLink::run()
|
||||
{
|
||||
unsigned long currentTime = millis();
|
||||
static DataLinkState oldstate;
|
||||
|
||||
|
||||
// Check if state has not changed for a prolonged period
|
||||
if (oldstate != currentState)
|
||||
{
|
||||
lastStateChangeTime = currentTime;
|
||||
oldstate = currentState;
|
||||
}
|
||||
if ((currentTime - lastStateChangeTime) > stateChangeTimeout) {
|
||||
// Reset the state to Idle and perform necessary cleanup
|
||||
currentState = DataLinkState::Idle;
|
||||
// Perform any additional cleanup or reinitialization here
|
||||
// ...
|
||||
|
||||
lastStateChangeTime = currentTime; // Reset the last state change time
|
||||
}
|
||||
switch (currentState)
|
||||
{
|
||||
case DataLinkState::Idle:
|
||||
// Decide if the device should start transmitting
|
||||
currentState = DataLinkState::Receiving;
|
||||
if (shouldTransmit())
|
||||
{
|
||||
currentState = DataLinkState::Transmitting;
|
||||
}
|
||||
break;
|
||||
|
||||
case DataLinkState::Transmitting:
|
||||
if (isTransmitting)
|
||||
{
|
||||
sendNextByte(); // Continue sending the current data
|
||||
}
|
||||
else
|
||||
{
|
||||
constructPacket(); // Construct a new packet if not currently transmitting
|
||||
|
||||
if (muteAcknowledgement && (needToACK || needToNACK))
|
||||
{
|
||||
needToACK = false;
|
||||
needToNACK = false;
|
||||
}
|
||||
uint8_t ack;
|
||||
// now it is known which acknoledge need sending since last Reception
|
||||
if (needToACK)
|
||||
{
|
||||
needToACK = false;
|
||||
ack = (txBufferIndex > 5) ? ACK_RTT_CODE : ACK_CODE;
|
||||
serial.write(ack);
|
||||
}
|
||||
if (needToNACK)
|
||||
{
|
||||
needToNACK = false;
|
||||
ack = (txBufferIndex > 5) ? NACK_RTT_CODE : NACK_CODE;
|
||||
serial.write(ack);
|
||||
}
|
||||
}
|
||||
|
||||
if (maxIndexTX < 1)
|
||||
{
|
||||
currentState = DataLinkState::Receiving;
|
||||
}
|
||||
// Check if the transmission is complete
|
||||
if (transmissionComplete)
|
||||
{
|
||||
transmissionComplete = false;
|
||||
isTransmitting = false;
|
||||
currentState = DataLinkState::WaitingForAck; // Move to WaitingForAck state
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
case DataLinkState::WaitingForAck:
|
||||
if (ackTimeout())
|
||||
{
|
||||
// Handle ACK timeout scenario
|
||||
transmissionError = true;
|
||||
isTransmitting = false;
|
||||
//handleAckTimeout();
|
||||
//--- if no ACK's etc received may as well move to Transmitting
|
||||
currentState = DataLinkState::Transmitting;
|
||||
}
|
||||
if (ackReceived())
|
||||
{
|
||||
// No data to send from the other device
|
||||
currentState = DataLinkState::Transmitting;
|
||||
}
|
||||
if (requestToSend)
|
||||
{
|
||||
// The other device has data to send (indicated by ACK+RTT)
|
||||
currentState = DataLinkState::Receiving;
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
case DataLinkState::Receiving:
|
||||
read();
|
||||
if (readComplete)
|
||||
{
|
||||
readComplete = false;
|
||||
// transition to transmit mode
|
||||
currentState = DataLinkState::Transmitting;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
unsigned long currentTime = millis();
|
||||
static DataLinkState oldstate;
|
||||
|
||||
|
||||
// Check if state has not changed for a prolonged period
|
||||
if (oldstate != currentState)
|
||||
{
|
||||
lastStateChangeTime = currentTime;
|
||||
oldstate = currentState;
|
||||
}
|
||||
if ((currentTime - lastStateChangeTime) > stateChangeTimeout) {
|
||||
// Reset the state to Idle and perform necessary cleanup
|
||||
currentState = DataLinkState::Idle;
|
||||
// Perform any additional cleanup or reinitialization here
|
||||
// ...
|
||||
|
||||
lastStateChangeTime = currentTime; // Reset the last state change time
|
||||
}
|
||||
|
||||
switch (currentState)
|
||||
{
|
||||
case DataLinkState::Idle:
|
||||
// Decide if the device should start transmitting
|
||||
currentState = DataLinkState::Receiving;
|
||||
if (shouldTransmit())
|
||||
{
|
||||
currentState = DataLinkState::WaitTobuildPacket;
|
||||
}
|
||||
break;
|
||||
|
||||
case DataLinkState::WaitTobuildPacket:
|
||||
constructPacket();
|
||||
if (isTransmitting)
|
||||
{
|
||||
currentState = DataLinkState::Transmitting;
|
||||
}
|
||||
break;
|
||||
|
||||
case DataLinkState::Transmitting:
|
||||
sendNextByte();
|
||||
|
||||
// Check if the transmission is complete
|
||||
if (transmissionComplete)
|
||||
{
|
||||
transmissionComplete = false;
|
||||
isTransmitting = false;
|
||||
currentState = DataLinkState::WaitingForAck; // Move to WaitingForAck state
|
||||
}
|
||||
break;
|
||||
|
||||
case DataLinkState::WaitingForAck:
|
||||
|
||||
if (ackTimeout())
|
||||
{
|
||||
// Handle ACK timeout scenario
|
||||
transmissionError = true;
|
||||
lastAcknowledgeStatus = -1;
|
||||
//--- if no ACK's etc received may as well move to Transmitting
|
||||
currentState = DataLinkState::Idle;
|
||||
}
|
||||
}
|
||||
if (ackReceived())
|
||||
{
|
||||
// No data to send from the other device
|
||||
currentState = DataLinkState::Idle;
|
||||
}
|
||||
if (requestToSend)
|
||||
{
|
||||
// The other device has data to send (indicated by ACK+RTT)
|
||||
currentState = DataLinkState::Receiving;
|
||||
requestToSend = false;
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
case DataLinkState::Receiving:
|
||||
read();
|
||||
if (readComplete)
|
||||
{
|
||||
readComplete = false;
|
||||
currentState = DataLinkState::SendingAck;
|
||||
}
|
||||
break;
|
||||
|
||||
case DataLinkState::SendingAck:
|
||||
|
||||
constructPacket();
|
||||
|
||||
if (muteAcknowledgement && (needToACK || needToNACK))
|
||||
{
|
||||
needToACK = false;
|
||||
needToNACK = false;
|
||||
}
|
||||
uint8_t ack;
|
||||
// now it is known which acknoledge need sending since last Reception
|
||||
if (needToACK)
|
||||
{
|
||||
needToACK = false;
|
||||
ack = (txBufferIndex > 5) ? ACK_RTT_CODE : ACK_CODE;
|
||||
serial.write(ack);
|
||||
}
|
||||
if (needToNACK)
|
||||
{
|
||||
needToNACK = false;
|
||||
ack = (txBufferIndex > 5) ? NACK_RTT_CODE : NACK_CODE;
|
||||
serial.write(ack);
|
||||
}
|
||||
|
||||
currentState = DataLinkState::Idle;
|
||||
if (isTransmitting)
|
||||
{
|
||||
currentState = DataLinkState::Wait;
|
||||
}
|
||||
break;
|
||||
|
||||
case DataLinkState::Wait:
|
||||
{
|
||||
static unsigned long waitTimer=0;
|
||||
if (waitTimer == 0) waitTimer = currentTime;
|
||||
if (currentTime - waitTimer > 20)
|
||||
{
|
||||
waitTimer=0;
|
||||
currentState = DataLinkState::Transmitting;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
currentState = DataLinkState::Idle;
|
||||
}
|
||||
}
|
||||
|
||||
void SerialDataLink::updateState(DataLinkState newState)
|
||||
|
|
@ -343,42 +373,48 @@ bool SerialDataLink::sendNextByte()
|
|||
|
||||
bool SerialDataLink::ackReceived()
|
||||
{
|
||||
|
||||
// Check if there is data available to read
|
||||
if (serial.available() > 0)
|
||||
int count = 0;
|
||||
if (serial.available() )
|
||||
{
|
||||
// Peek at the next byte without removing it from the buffer
|
||||
uint8_t nextByte = serial.peek();
|
||||
count++;
|
||||
// Peek at the next byte without removing it from the buffer
|
||||
uint8_t nextByte = serial.peek();
|
||||
|
||||
if (nextByte == headerChar)
|
||||
{
|
||||
requestToSend = true;
|
||||
transmissionError = true;
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t receivedByte = serial.read();
|
||||
|
||||
switch (receivedByte)
|
||||
{
|
||||
case ACK_CODE:
|
||||
// Handle standard ACK
|
||||
return true;
|
||||
|
||||
case ACK_RTT_CODE:
|
||||
// Handle ACK with request to transmit
|
||||
requestToSend = true;
|
||||
return true;
|
||||
|
||||
case NACK_RTT_CODE:
|
||||
requestToSend = true;
|
||||
case NACK_CODE:
|
||||
transmissionError = true;
|
||||
return true;
|
||||
|
||||
default:
|
||||
break;
|
||||
if (nextByte == headerChar)
|
||||
{
|
||||
requestToSend = true;
|
||||
transmissionError = true;
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t receivedByte = serial.read();
|
||||
|
||||
switch (receivedByte)
|
||||
{
|
||||
case ACK_CODE:
|
||||
// Handle standard ACK
|
||||
lastAcknowledgeStatus = 1;
|
||||
return true;
|
||||
|
||||
case ACK_RTT_CODE:
|
||||
// Handle ACK with request to transmit
|
||||
requestToSend = true;
|
||||
lastAcknowledgeStatus = 1;
|
||||
return true;
|
||||
|
||||
case NACK_RTT_CODE:
|
||||
requestToSend = true;
|
||||
case NACK_CODE:
|
||||
transmissionError = true;
|
||||
lastAcknowledgeStatus = -2;
|
||||
return true;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return false; // No ACK, NACK, or new packet received
|
||||
}
|
||||
|
|
|
|||
|
|
@ -68,6 +68,7 @@ public:
|
|||
|
||||
// Check for errors
|
||||
bool checkTransmissionError(bool resetFlag);
|
||||
int getLastAcknowledge(bool resetFlag);
|
||||
bool checkReadError(bool resetFlag);
|
||||
|
||||
// Setter methods for various parameters and special characters
|
||||
|
|
@ -84,9 +85,12 @@ private:
|
|||
enum class DataLinkState
|
||||
{
|
||||
Idle,
|
||||
WaitTobuildPacket,
|
||||
Transmitting,
|
||||
WaitingForAck,
|
||||
Receiving,
|
||||
SendingAck,
|
||||
Wait,
|
||||
Error
|
||||
};
|
||||
|
||||
|
|
@ -117,6 +121,7 @@ private:
|
|||
bool readComplete = false;
|
||||
bool retransmitEnabled;
|
||||
bool transmissionError = false;
|
||||
int lastAcknowledgeStatus = 0;
|
||||
bool readError = false;
|
||||
bool muteAcknowledgement = false;
|
||||
|
||||
|
|
@ -129,13 +134,15 @@ private:
|
|||
int16_t dataArrayRX[dataArraySizeRX];
|
||||
bool dataUpdated[dataArraySizeTX];
|
||||
unsigned long lastSent[dataArraySizeTX];
|
||||
|
||||
|
||||
// times in milliseconds
|
||||
unsigned long updateInterval = 1000;
|
||||
unsigned long ACK_TIMEOUT = 200;
|
||||
unsigned long PACKET_TIMEOUT = 200; // Timeout in milliseconds
|
||||
|
||||
unsigned long lastStateChangeTime = 0;
|
||||
unsigned long PACKET_TIMEOUT = 200;
|
||||
unsigned long stateChangeTimeout = 300;
|
||||
|
||||
unsigned long lastStateChangeTime = 0;
|
||||
|
||||
|
||||
// Special characters for packet framing
|
||||
char headerChar = '<';
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue