mirror of
https://github.com/dalathegreat/Battery-Emulator.git
synced 2025-10-03 17:59:27 +02:00
Merge pull request #621 from NJbubo/CHAdeMO
Fix CHAdeMO Vehicles, Battery
This commit is contained in:
commit
22c47f2d0c
5 changed files with 237 additions and 125 deletions
|
@ -1122,6 +1122,9 @@ void receive_can(CAN_frame* rx_frame, int interface) {
|
|||
|
||||
if (interface == can_config.battery) {
|
||||
receive_can_battery(*rx_frame);
|
||||
#ifdef CHADEMO_BATTERY
|
||||
ISA_handleFrame(rx_frame);
|
||||
#endif
|
||||
}
|
||||
if (interface == can_config.inverter) {
|
||||
#ifdef CAN_INVERTER_SELECTED
|
||||
|
|
|
@ -20,6 +20,7 @@
|
|||
|
||||
#ifdef CHADEMO_BATTERY
|
||||
#include "CHADEMO-BATTERY.h"
|
||||
#include "CHADEMO-SHUNTS.h"
|
||||
#endif
|
||||
|
||||
#ifdef IMIEV_CZERO_ION_BATTERY
|
||||
|
|
|
@ -120,7 +120,7 @@ void update_values_battery() {
|
|||
datalayer.battery.status.voltage_dV = get_measured_voltage() * 10;
|
||||
|
||||
datalayer.battery.info.total_capacity_Wh =
|
||||
((x101_chg_est.RatedBatteryCapacity / 0.11) *
|
||||
((x101_chg_est.RatedBatteryCapacity / 0.1) *
|
||||
1000); //(Added in CHAdeMO v1.0.1), maybe handle hardcoded on lower protocol version?
|
||||
|
||||
/* TODO max charging rate =
|
||||
|
@ -151,8 +151,8 @@ void update_values_battery() {
|
|||
|
||||
inline void process_vehicle_charging_minimums(CAN_frame rx_frame) {
|
||||
x100_chg_lim.MinimumChargeCurrent = rx_frame.data.u8[0];
|
||||
x100_chg_lim.MinimumBatteryVoltage = ((rx_frame.data.u8[2] << 8) | rx_frame.data.u8[3]);
|
||||
x100_chg_lim.MaximumBatteryVoltage = ((rx_frame.data.u8[4] << 8) | rx_frame.data.u8[5]);
|
||||
x100_chg_lim.MinimumBatteryVoltage = ((rx_frame.data.u8[3] << 8) | rx_frame.data.u8[2]);
|
||||
x100_chg_lim.MaximumBatteryVoltage = ((rx_frame.data.u8[5] << 8) | rx_frame.data.u8[4]);
|
||||
x100_chg_lim.ConstantOfChargingRateIndication = rx_frame.data.u8[6];
|
||||
}
|
||||
|
||||
|
@ -160,15 +160,14 @@ inline void process_vehicle_charging_maximums(CAN_frame rx_frame) {
|
|||
x101_chg_est.MaxChargingTime10sBit = rx_frame.data.u8[1];
|
||||
x101_chg_est.MaxChargingTime1minBit = rx_frame.data.u8[2];
|
||||
x101_chg_est.EstimatedChargingTime = rx_frame.data.u8[3];
|
||||
x101_chg_est.RatedBatteryCapacity = ((rx_frame.data.u8[5] << 8) | rx_frame.data.u8[6]);
|
||||
x101_chg_est.RatedBatteryCapacity = ((rx_frame.data.u8[6] << 8) | rx_frame.data.u8[5]);
|
||||
}
|
||||
|
||||
inline void process_vehicle_charging_session(CAN_frame rx_frame) {
|
||||
|
||||
uint16_t newTargetBatteryVoltage = ((rx_frame.data.u8[1] << 8) | rx_frame.data.u8[2]);
|
||||
uint16_t priorChargingCurrentRequest = x102_chg_session.ChargingCurrentRequest;
|
||||
uint8_t priorTargetBatteryVoltage = x102_chg_session.TargetBatteryVoltage;
|
||||
uint16_t newTargetBatteryVoltage = ((rx_frame.data.u8[2] << 8) | rx_frame.data.u8[1]);
|
||||
uint16_t priorTargetBatteryVoltage = x102_chg_session.TargetBatteryVoltage;
|
||||
uint8_t newChargingCurrentRequest = rx_frame.data.u8[3];
|
||||
uint8_t priorChargingCurrentRequest = x102_chg_session.ChargingCurrentRequest;
|
||||
|
||||
vehicle_can_initialized = true;
|
||||
|
||||
|
@ -187,6 +186,7 @@ inline void process_vehicle_charging_session(CAN_frame rx_frame) {
|
|||
x102_chg_session.s.status.StatusChargingError = bitRead(rx_frame.data.u8[5], 2);
|
||||
x102_chg_session.s.status.StatusVehicle = bitRead(rx_frame.data.u8[5], 3);
|
||||
x102_chg_session.s.status.StatusNormalStopRequest = bitRead(rx_frame.data.u8[5], 4);
|
||||
x102_chg_session.s.status.StatusVehicleDischargeCompatible = bitRead(rx_frame.data.u8[5], 7);
|
||||
|
||||
x102_chg_session.StateOfCharge = rx_frame.data.u8[6];
|
||||
|
||||
|
@ -202,7 +202,7 @@ inline void process_vehicle_charging_session(CAN_frame rx_frame) {
|
|||
uint8_t chargingrate = 0;
|
||||
if (x100_chg_lim.ConstantOfChargingRateIndication > 0) {
|
||||
chargingrate = x102_chg_session.StateOfCharge / x100_chg_lim.ConstantOfChargingRateIndication * 100;
|
||||
Serial.print("Charge Rate (kW):");
|
||||
Serial.print("Charge Rate (kW): ");
|
||||
Serial.println(chargingrate);
|
||||
}
|
||||
#endif
|
||||
|
@ -308,7 +308,7 @@ inline void process_vehicle_charging_session(CAN_frame rx_frame) {
|
|||
inline void process_vehicle_charging_limits(CAN_frame rx_frame) {
|
||||
|
||||
x200_discharge_limits.MaximumDischargeCurrent = rx_frame.data.u8[0];
|
||||
x200_discharge_limits.MinimumDischargeVoltage = ((rx_frame.data.u8[4] << 8) | rx_frame.data.u8[5]);
|
||||
x200_discharge_limits.MinimumDischargeVoltage = ((rx_frame.data.u8[5] << 8) | rx_frame.data.u8[4]);
|
||||
x200_discharge_limits.MinimumBatteryDischargeLevel = rx_frame.data.u8[6];
|
||||
x200_discharge_limits.MaxRemainingCapacityForCharging = rx_frame.data.u8[7];
|
||||
|
||||
|
@ -338,15 +338,15 @@ inline void process_vehicle_discharge_estimate(CAN_frame rx_frame) {
|
|||
unsigned long currentMillis = millis();
|
||||
|
||||
x201_discharge_estimate.V2HchargeDischargeSequenceNum = rx_frame.data.u8[0];
|
||||
x201_discharge_estimate.ApproxDischargeCompletionTime = ((rx_frame.data.u8[1] << 8) | rx_frame.data.u8[2]);
|
||||
x201_discharge_estimate.AvailableVehicleEnergy = ((rx_frame.data.u8[3] << 8) | rx_frame.data.u8[4]);
|
||||
x201_discharge_estimate.ApproxDischargeCompletionTime = ((rx_frame.data.u8[2] << 8) | rx_frame.data.u8[1]);
|
||||
x201_discharge_estimate.AvailableVehicleEnergy = ((rx_frame.data.u8[4] << 8) | rx_frame.data.u8[3]);
|
||||
|
||||
#ifdef DEBUG_VIA_USB
|
||||
if (currentMillis - previousMillis5000 >= INTERVAL_5_S) {
|
||||
previousMillis5000 = currentMillis;
|
||||
Serial.println("x201 availabile vehicle energy, completion time");
|
||||
Serial.print("x201 availabile vehicle energy, completion time: ");
|
||||
Serial.println(x201_discharge_estimate.AvailableVehicleEnergy);
|
||||
Serial.println("x201 approx vehicle completion time");
|
||||
Serial.print("x201 approx vehicle completion time: ");
|
||||
Serial.println(x201_discharge_estimate.ApproxDischargeCompletionTime);
|
||||
}
|
||||
#endif
|
||||
|
@ -364,7 +364,7 @@ inline void process_vehicle_dynamic_control(CAN_frame rx_frame) {
|
|||
inline void process_vehicle_vendor_ID(CAN_frame rx_frame) {
|
||||
x700_vendor_id.AutomakerCode = rx_frame.data.u8[0];
|
||||
x700_vendor_id.OptionalContent =
|
||||
((rx_frame.data.u8[1] << 8) | rx_frame.data.u8[2]); //Actually more bytes, but not needed for our purpose
|
||||
((rx_frame.data.u8[2] << 8) | rx_frame.data.u8[1]); //Actually more bytes, but not needed for our purpose
|
||||
}
|
||||
|
||||
void receive_can_battery(CAN_frame rx_frame) {
|
||||
|
@ -557,7 +557,7 @@ void update_evse_status(CAN_frame& f) {
|
|||
*
|
||||
*/
|
||||
if ((x102_chg_session.TargetBatteryVoltage > x108_evse_cap.available_output_voltage) ||
|
||||
(x100_chg_lim.MaximumBatteryVoltage > x108_evse_cap.threshold_voltage)) {
|
||||
(x100_chg_lim.MaximumBatteryVoltage < x108_evse_cap.threshold_voltage)) {
|
||||
//Toggle battery incompatibility flag 109.5.3
|
||||
x109_evse_state.s.status.EVSE_error = 1;
|
||||
x109_evse_state.s.status.battery_incompatible = 1;
|
||||
|
@ -602,7 +602,8 @@ void update_evse_discharge_estimate(CAN_frame& f) {
|
|||
*/
|
||||
|
||||
CHADEMO_209.data.u8[0] = x209_evse_dischg_est.sequence_control_number;
|
||||
CHADEMO_209.data.u8[1] = x209_evse_dischg_est.remaining_discharge_time;
|
||||
CHADEMO_209.data.u8[1] = lowByte(x209_evse_dischg_est.remaining_discharge_time);
|
||||
CHADEMO_209.data.u8[2] = highByte(x209_evse_dischg_est.remaining_discharge_time);
|
||||
}
|
||||
|
||||
/* x208 EVSE, peer to 0x200 Vehicle */
|
||||
|
@ -751,7 +752,7 @@ void handle_chademo_sequence() {
|
|||
|
||||
/* ------------------- State override conditions checks ------------------- */
|
||||
/* ------------------------------------------------------------------------------ */
|
||||
if (CHADEMO_Status >= CHADEMO_EV_ALLOWED && !x102_chg_session.s.status.StatusVehicleShifterPosition) {
|
||||
if (CHADEMO_Status >= CHADEMO_EV_ALLOWED && x102_chg_session.s.status.StatusVehicleShifterPosition) {
|
||||
#ifdef DEBUG_VIA_USB
|
||||
Serial.println("Vehicle is not parked, abort.");
|
||||
#endif
|
||||
|
@ -777,7 +778,6 @@ void handle_chademo_sequence() {
|
|||
#ifdef DEBUG_VIA_USB
|
||||
// Commented unless needed for debug
|
||||
// Serial.println("CHADEMO plug is not inserted.");
|
||||
//
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
|
@ -1032,6 +1032,15 @@ void handle_chademo_sequence() {
|
|||
|
||||
void setup_battery(void) { // Performs one time setup at startup
|
||||
|
||||
pinMode(CHADEMO_PIN_2, OUTPUT);
|
||||
digitalWrite(CHADEMO_PIN_2, LOW);
|
||||
pinMode(CHADEMO_PIN_10, OUTPUT);
|
||||
digitalWrite(CHADEMO_PIN_10, LOW);
|
||||
pinMode(CHADEMO_LOCK, OUTPUT);
|
||||
digitalWrite(CHADEMO_LOCK, LOW);
|
||||
pinMode(CHADEMO_PIN_4, INPUT);
|
||||
pinMode(CHADEMO_PIN_7, INPUT);
|
||||
|
||||
strncpy(datalayer.system.info.battery_protocol, "Chademo V2X mode", 63);
|
||||
datalayer.system.info.battery_protocol[63] = '\0';
|
||||
|
||||
|
@ -1075,6 +1084,9 @@ void setup_battery(void) { // Performs one time setup at startup
|
|||
x109_evse_state.s.status.ChgDischStopControl = 1;
|
||||
|
||||
handle_chademo_sequence();
|
||||
// ISA_deFAULT(); // ISA Setup - it is sufficient to set it once, because it is saved in SUNT
|
||||
// ISA_initialize(); // ISA Setup - it is sufficient to set it once, because it is saved in SUNT
|
||||
// ISA_RESTART();
|
||||
|
||||
setupMillis = millis();
|
||||
}
|
||||
|
|
|
@ -12,6 +12,11 @@
|
|||
*
|
||||
* 2024 - Modified to make use of ESP32-Arduino-CAN by miwagner
|
||||
*
|
||||
* 2024.11 - Modified byte sequence to Big Endian (this is the default for IVT) and the same as CHAdeMO
|
||||
* - Fixed and Added send functions
|
||||
* - Added some GET functions
|
||||
* by NJbubo
|
||||
*
|
||||
*/
|
||||
#include "../include.h"
|
||||
#ifdef CHADEMO_BATTERY
|
||||
|
@ -74,16 +79,29 @@ uint16_t get_measured_current() {
|
|||
}
|
||||
|
||||
//This is our CAN interrupt service routine to catch inbound frames
|
||||
inline void ISA_handleFrame(CAN_frame* frame) {
|
||||
void ISA_handleFrame(CAN_frame* frame) {
|
||||
|
||||
if (frame->ID < 0x521 || frame->ID > 0x528) {
|
||||
if (frame->ID < 0x510 || frame->ID > 0x528) {
|
||||
return;
|
||||
}
|
||||
|
||||
framecount++;
|
||||
|
||||
switch (frame->ID) {
|
||||
|
||||
case 0x510:
|
||||
case 0x511:
|
||||
Serial.print(millis()); // Example printout, time, ID, length, data: 7553 1DB 8 FF C0 B9 EA 0 0 2 5D
|
||||
Serial.print(" ");
|
||||
Serial.print(frame->ID, HEX);
|
||||
Serial.print(" ");
|
||||
Serial.print(frame->DLC);
|
||||
Serial.print(" ");
|
||||
for (int i = 0; i < frame->DLC; ++i) {
|
||||
Serial.print(frame->data.u8[i], HEX);
|
||||
Serial.print(" ");
|
||||
}
|
||||
Serial.println("");
|
||||
break;
|
||||
|
||||
case 0x521:
|
||||
|
@ -118,7 +136,6 @@ inline void ISA_handleFrame(CAN_frame* frame) {
|
|||
ISA_handle528(frame);
|
||||
break;
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -126,7 +143,7 @@ inline void ISA_handleFrame(CAN_frame* frame) {
|
|||
inline void ISA_handle521(CAN_frame* frame) {
|
||||
long current = 0;
|
||||
current =
|
||||
(long)((frame->data.u8[5] << 24) | (frame->data.u8[4] << 16) | (frame->data.u8[3] << 8) | (frame->data.u8[2]));
|
||||
(long)((frame->data.u8[2] << 24) | (frame->data.u8[3] << 16) | (frame->data.u8[4] << 8) | (frame->data.u8[5]));
|
||||
|
||||
milliamps = current;
|
||||
Amperes = current / 1000.0f;
|
||||
|
@ -135,7 +152,7 @@ inline void ISA_handle521(CAN_frame* frame) {
|
|||
//handle frame for Voltage
|
||||
inline void ISA_handle522(CAN_frame* frame) {
|
||||
long volt =
|
||||
(long)((frame->data.u8[5] << 24) | (frame->data.u8[4] << 16) | (frame->data.u8[3] << 8) | (frame->data.u8[2]));
|
||||
(long)((frame->data.u8[2] << 24) | (frame->data.u8[3] << 16) | (frame->data.u8[4] << 8) | (frame->data.u8[5]));
|
||||
|
||||
Voltage = volt / 1000.0f;
|
||||
Voltage1 = Voltage - (Voltage2 + Voltage3);
|
||||
|
@ -158,7 +175,7 @@ inline void ISA_handle522(CAN_frame* frame) {
|
|||
//handle frame for Voltage 2
|
||||
inline void ISA_handle523(CAN_frame* frame) {
|
||||
long volt =
|
||||
(long)((frame->data.u8[5] << 24) | (frame->data.u8[4] << 16) | (frame->data.u8[3] << 8) | (frame->data.u8[2]));
|
||||
(long)((frame->data.u8[2] << 24) | (frame->data.u8[3] << 16) | (frame->data.u8[4] << 8) | (frame->data.u8[5]));
|
||||
|
||||
Voltage2 = volt / 1000.0f;
|
||||
if (Voltage2 > 3)
|
||||
|
@ -177,7 +194,7 @@ inline void ISA_handle523(CAN_frame* frame) {
|
|||
//handle frame for Voltage3
|
||||
inline void ISA_handle524(CAN_frame* frame) {
|
||||
long volt =
|
||||
(long)((frame->data.u8[5] << 24) | (frame->data.u8[4] << 16) | (frame->data.u8[3] << 8) | (frame->data.u8[2]));
|
||||
(long)((frame->data.u8[2] << 24) | (frame->data.u8[3] << 16) | (frame->data.u8[4] << 8) | (frame->data.u8[5]));
|
||||
|
||||
Voltage3 = volt / 1000.0f;
|
||||
|
||||
|
@ -194,7 +211,7 @@ inline void ISA_handle524(CAN_frame* frame) {
|
|||
//handle frame for Temperature
|
||||
inline void ISA_handle525(CAN_frame* frame) {
|
||||
long temp = 0;
|
||||
temp = (long)((frame->data.u8[5] << 24) | (frame->data.u8[4] << 16) | (frame->data.u8[3] << 8) | (frame->data.u8[2]));
|
||||
temp = (long)((frame->data.u8[2] << 24) | (frame->data.u8[3] << 16) | (frame->data.u8[4] << 8) | (frame->data.u8[5]));
|
||||
|
||||
Temperature = temp / 10;
|
||||
}
|
||||
|
@ -202,14 +219,15 @@ inline void ISA_handle525(CAN_frame* frame) {
|
|||
//handle frame for Kilowatts
|
||||
inline void ISA_handle526(CAN_frame* frame) {
|
||||
watt = 0;
|
||||
watt = (long)((frame->data.u8[5] << 24) | (frame->data.u8[4] << 16) | (frame->data.u8[3] << 8) | (frame->data.u8[2]));
|
||||
watt = (long)((frame->data.u8[2] << 24) | (frame->data.u8[3] << 16) | (frame->data.u8[4] << 8) | (frame->data.u8[5]));
|
||||
|
||||
KW = watt / 1000.0f;
|
||||
}
|
||||
|
||||
//handle frame for Ampere-Hours
|
||||
inline void ISA_handle527(CAN_frame* frame) {
|
||||
As = 0;
|
||||
As = (frame->data.u8[5] << 24) | (frame->data.u8[4] << 16) | (frame->data.u8[3] << 8) | (frame->data.u8[2]);
|
||||
As = (long)(frame->data.u8[2] << 24) | (frame->data.u8[3] << 16) | (frame->data.u8[4] << 8) | (frame->data.u8[5]);
|
||||
|
||||
AH += (As - lastAs) / 3600.0f;
|
||||
lastAs = As;
|
||||
|
@ -217,133 +235,201 @@ inline void ISA_handle527(CAN_frame* frame) {
|
|||
|
||||
//handle frame for kiloWatt-hours
|
||||
inline void ISA_handle528(CAN_frame* frame) {
|
||||
wh = (long)((frame->data.u8[5] << 24) | (frame->data.u8[4] << 16) | (frame->data.u8[3] << 8) | (frame->data.u8[2]));
|
||||
wh = (long)((frame->data.u8[2] << 24) | (frame->data.u8[3] << 16) | (frame->data.u8[4] << 8) | (frame->data.u8[5]));
|
||||
KWH += (wh - lastWh) / 1000.0f;
|
||||
lastWh = wh;
|
||||
}
|
||||
|
||||
/*
|
||||
void ISA_initialize() {
|
||||
firstframe=false;
|
||||
STOP();
|
||||
delay(700);
|
||||
for(int i=0;i<9;i++) {
|
||||
Serial.println("initialization \n");
|
||||
firstframe = false;
|
||||
ISA_STOP();
|
||||
delay(500);
|
||||
for (int i = 0; i < 8; i++) {
|
||||
Serial.print("ISA Initialization ");
|
||||
Serial.println(i);
|
||||
|
||||
outframe.data.u8[0]=(0x20+i);
|
||||
outframe.data.u8[1]=0x42;
|
||||
outframe.data.u8[2]=0x02;
|
||||
outframe.data.u8[3]=(0x60+(i*18));
|
||||
outframe.data.u8[4]=0x00;
|
||||
outframe.data.u8[5]=0x00;
|
||||
outframe.data.u8[6]=0x00;
|
||||
outframe.data.u8[7]=0x00;
|
||||
outframe.data.u8[0] = (0x20 + i);
|
||||
outframe.data.u8[1] = 0x02;
|
||||
outframe.data.u8[2] = 0x02;
|
||||
outframe.data.u8[3] = (0x60 + (i * 18));
|
||||
outframe.data.u8[4] = 0x00;
|
||||
outframe.data.u8[5] = 0x00;
|
||||
outframe.data.u8[6] = 0x00;
|
||||
outframe.data.u8[7] = 0x00;
|
||||
|
||||
transmit_can((&outframe, can_config.battery);
|
||||
|
||||
delay(500);
|
||||
|
||||
sendSTORE();
|
||||
delay(500);
|
||||
}
|
||||
|
||||
START();
|
||||
transmit_can(&outframe, can_config.battery);
|
||||
delay(500);
|
||||
lastAs=As;
|
||||
lastWh=wh;
|
||||
}
|
||||
|
||||
ISA_sendSTORE();
|
||||
delay(500);
|
||||
|
||||
ISA_START();
|
||||
delay(500);
|
||||
lastAs = As;
|
||||
lastWh = wh;
|
||||
}
|
||||
|
||||
void ISA_STOP() {
|
||||
outframe.data.u8[0]=0x34;
|
||||
outframe.data.u8[1]=0x00;
|
||||
outframe.data.u8[2]=0x01;
|
||||
outframe.data.u8[3]=0x00;
|
||||
outframe.data.u8[4]=0x00;
|
||||
outframe.data.u8[5]=0x00;
|
||||
outframe.data.u8[6]=0x00;
|
||||
outframe.data.u8[7]=0x00;
|
||||
transmit_can((&outframe, can_config.battery);
|
||||
Serial.println("ISA STOP");
|
||||
|
||||
outframe.data.u8[0] = 0x34;
|
||||
outframe.data.u8[1] = 0x00;
|
||||
outframe.data.u8[2] = 0x01;
|
||||
outframe.data.u8[3] = 0x00;
|
||||
outframe.data.u8[4] = 0x00;
|
||||
outframe.data.u8[5] = 0x00;
|
||||
outframe.data.u8[6] = 0x00;
|
||||
outframe.data.u8[7] = 0x00;
|
||||
|
||||
transmit_can(&outframe, can_config.battery);
|
||||
}
|
||||
|
||||
void ISA_sendSTORE() {
|
||||
outframe.data.u8[0]=0x32;
|
||||
outframe.data.u8[1]=0x00;
|
||||
outframe.data.u8[2]=0x00;
|
||||
outframe.data.u8[3]=0x00;
|
||||
outframe.data.u8[4]=0x00;
|
||||
outframe.data.u8[5]=0x00;
|
||||
outframe.data.u8[6]=0x00;
|
||||
outframe.data.u8[7]=0x00;
|
||||
transmit_can((&outframe, can_config.battery);
|
||||
Serial.println("ISA send STORE");
|
||||
|
||||
outframe.data.u8[0] = 0x32;
|
||||
outframe.data.u8[1] = 0x00;
|
||||
outframe.data.u8[2] = 0x00;
|
||||
outframe.data.u8[3] = 0x00;
|
||||
outframe.data.u8[4] = 0x00;
|
||||
outframe.data.u8[5] = 0x00;
|
||||
outframe.data.u8[6] = 0x00;
|
||||
outframe.data.u8[7] = 0x00;
|
||||
|
||||
transmit_can(&outframe, can_config.battery);
|
||||
}
|
||||
|
||||
void ISA_START() {
|
||||
outframe.data.u8[0]=0x34;
|
||||
outframe.data.u8[1]=0x01;
|
||||
outframe.data.u8[2]=0x01;
|
||||
outframe.data.u8[3]=0x00;
|
||||
outframe.data.u8[4]=0x00;
|
||||
outframe.data.u8[5]=0x00;
|
||||
outframe.data.u8[6]=0x00;
|
||||
outframe.data.u8[7]=0x00;
|
||||
transmit_can((&outframe, can_config.battery);
|
||||
Serial.println("ISA START");
|
||||
|
||||
outframe.data.u8[0] = 0x34;
|
||||
outframe.data.u8[1] = 0x01;
|
||||
outframe.data.u8[2] = 0x01;
|
||||
outframe.data.u8[3] = 0x00;
|
||||
outframe.data.u8[4] = 0x00;
|
||||
outframe.data.u8[5] = 0x00;
|
||||
outframe.data.u8[6] = 0x00;
|
||||
outframe.data.u8[7] = 0x00;
|
||||
|
||||
transmit_can(&outframe, can_config.battery);
|
||||
}
|
||||
|
||||
void ISA_RESTART() {
|
||||
//Has the effect of zeroing AH and KWH
|
||||
outframe.data.u8[0]=0x3F;
|
||||
outframe.data.u8[1]=0x00;
|
||||
outframe.data.u8[2]=0x00;
|
||||
outframe.data.u8[3]=0x00;
|
||||
outframe.data.u8[4]=0x00;
|
||||
outframe.data.u8[5]=0x00;
|
||||
outframe.data.u8[6]=0x00;
|
||||
outframe.data.u8[7]=0x00;
|
||||
transmit_can((&outframe, can_config.battery);
|
||||
//Has the effect of zeroing AH and KWH
|
||||
Serial.println("ISA RESTART");
|
||||
|
||||
outframe.data.u8[0] = 0x3F;
|
||||
outframe.data.u8[1] = 0x00;
|
||||
outframe.data.u8[2] = 0x00;
|
||||
outframe.data.u8[3] = 0x00;
|
||||
outframe.data.u8[4] = 0x00;
|
||||
outframe.data.u8[5] = 0x00;
|
||||
outframe.data.u8[6] = 0x00;
|
||||
outframe.data.u8[7] = 0x00;
|
||||
|
||||
transmit_can(&outframe, can_config.battery);
|
||||
}
|
||||
|
||||
void ISA_deFAULT() {
|
||||
//Returns module to original defaults
|
||||
outframe.data.u8[0]=0x3D;
|
||||
outframe.data.u8[1]=0x00;
|
||||
outframe.data.u8[2]=0x00;
|
||||
outframe.data.u8[3]=0x00;
|
||||
outframe.data.u8[4]=0x00;
|
||||
outframe.data.u8[5]=0x00;
|
||||
outframe.data.u8[6]=0x00;
|
||||
outframe.data.u8[7]=0x00;
|
||||
transmit_can((&outframe, can_config.battery);
|
||||
//Returns module to original defaults
|
||||
ISA_STOP();
|
||||
delay(500);
|
||||
|
||||
Serial.println("ISA RESTART to default");
|
||||
|
||||
outframe.data.u8[0] = 0x3D;
|
||||
outframe.data.u8[1] = 0x00;
|
||||
outframe.data.u8[2] = 0x00;
|
||||
outframe.data.u8[3] = 0x00;
|
||||
outframe.data.u8[4] = 0x00;
|
||||
outframe.data.u8[5] = 0x00;
|
||||
outframe.data.u8[6] = 0x00;
|
||||
outframe.data.u8[7] = 0x00;
|
||||
|
||||
transmit_can(&outframe, can_config.battery);
|
||||
delay(500);
|
||||
|
||||
ISA_START();
|
||||
delay(500);
|
||||
}
|
||||
|
||||
void ISA_initCurrent() {
|
||||
STOP();
|
||||
delay(500);
|
||||
|
||||
Serial.println("initialization \n");
|
||||
|
||||
outframe.data.u8[0]=0x21;
|
||||
outframe.data.u8[1]=0x42;
|
||||
outframe.data.u8[2]=0x01;
|
||||
outframe.data.u8[3]=0x61;
|
||||
outframe.data.u8[4]=0x00;
|
||||
outframe.data.u8[5]=0x00;
|
||||
outframe.data.u8[6]=0x00;
|
||||
outframe.data.u8[7]=0x00;
|
||||
ISA_STOP();
|
||||
delay(500);
|
||||
|
||||
transmit_can((&outframe, can_config.battery);
|
||||
Serial.println("ISA Initialization Current");
|
||||
|
||||
delay(500);
|
||||
outframe.data.u8[0] = 0x21;
|
||||
outframe.data.u8[1] = 0x02;
|
||||
outframe.data.u8[2] = 0x01;
|
||||
outframe.data.u8[3] = 0x61;
|
||||
outframe.data.u8[4] = 0x00;
|
||||
outframe.data.u8[5] = 0x00;
|
||||
outframe.data.u8[6] = 0x00;
|
||||
outframe.data.u8[7] = 0x00;
|
||||
|
||||
sendSTORE();
|
||||
delay(500);
|
||||
transmit_can(&outframe, can_config.battery);
|
||||
delay(500);
|
||||
|
||||
START();
|
||||
delay(500);
|
||||
lastAs=As;
|
||||
lastWh=wh;
|
||||
ISA_sendSTORE();
|
||||
delay(500);
|
||||
|
||||
ISA_START();
|
||||
delay(500);
|
||||
lastAs = As;
|
||||
lastWh = wh;
|
||||
}
|
||||
*/
|
||||
|
||||
void ISA_getCONFIG(uint8_t i) {
|
||||
Serial.print("ISA Get Config ");
|
||||
Serial.println(i);
|
||||
|
||||
outframe.data.u8[0] = (0x60 + i);
|
||||
outframe.data.u8[1] = 0x00;
|
||||
outframe.data.u8[2] = 0x00;
|
||||
outframe.data.u8[3] = 0x00;
|
||||
outframe.data.u8[4] = 0x00;
|
||||
outframe.data.u8[5] = 0x00;
|
||||
outframe.data.u8[6] = 0x00;
|
||||
outframe.data.u8[7] = 0x00;
|
||||
|
||||
transmit_can(&outframe, can_config.battery);
|
||||
}
|
||||
|
||||
void ISA_getCAN_ID(uint8_t i) {
|
||||
Serial.print("ISA Get CAN ID ");
|
||||
Serial.println(i);
|
||||
|
||||
outframe.data.u8[0] = (0x50 + i);
|
||||
if (i == 8)
|
||||
outframe.data.u8[0] = 0x5D;
|
||||
if (i == 9)
|
||||
outframe.data.u8[0] = 0x5F;
|
||||
outframe.data.u8[1] = 0x00;
|
||||
outframe.data.u8[2] = 0x00;
|
||||
outframe.data.u8[3] = 0x00;
|
||||
outframe.data.u8[4] = 0x00;
|
||||
outframe.data.u8[5] = 0x00;
|
||||
outframe.data.u8[6] = 0x00;
|
||||
outframe.data.u8[7] = 0x00;
|
||||
|
||||
transmit_can(&outframe, can_config.battery);
|
||||
}
|
||||
|
||||
void ISA_getINFO(uint8_t i) {
|
||||
Serial.print("ISA Get INFO ");
|
||||
Serial.println(i, HEX);
|
||||
|
||||
outframe.data.u8[0] = (0x70 + i);
|
||||
outframe.data.u8[1] = 0x00;
|
||||
outframe.data.u8[2] = 0x00;
|
||||
outframe.data.u8[3] = 0x00;
|
||||
outframe.data.u8[4] = 0x00;
|
||||
outframe.data.u8[5] = 0x00;
|
||||
outframe.data.u8[6] = 0x00;
|
||||
outframe.data.u8[7] = 0x00;
|
||||
|
||||
transmit_can(&outframe, can_config.battery);
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -5,7 +5,7 @@
|
|||
|
||||
uint16_t get_measured_voltage();
|
||||
uint16_t get_measured_current();
|
||||
inline void ISA_handler(CAN_frame* frame);
|
||||
void ISA_handleFrame(CAN_frame* frame);
|
||||
inline void ISA_handle521(CAN_frame* frame);
|
||||
inline void ISA_handle522(CAN_frame* frame);
|
||||
inline void ISA_handle523(CAN_frame* frame);
|
||||
|
@ -14,6 +14,16 @@ inline void ISA_handle525(CAN_frame* frame);
|
|||
inline void ISA_handle526(CAN_frame* frame);
|
||||
inline void ISA_handle527(CAN_frame* frame);
|
||||
inline void ISA_handle528(CAN_frame* frame);
|
||||
void ISA_initialize();
|
||||
void ISA_STOP();
|
||||
void ISA_sendSTORE();
|
||||
void ISA_START();
|
||||
void ISA_RESTART();
|
||||
void ISA_deFAULT();
|
||||
void ISA_initCurrent();
|
||||
void ISA_getCONFIG(uint8_t i);
|
||||
void ISA_getCAN_ID(uint8_t i);
|
||||
void ISA_getINFO(uint8_t i);
|
||||
|
||||
void transmit_can(CAN_frame* tx_frame, int interface);
|
||||
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue