From 38275697d138be58af7cddff5c14ce9458c36fe5 Mon Sep 17 00:00:00 2001 From: tomdebree Date: Fri, 10 Jan 2025 16:01:03 +0000 Subject: [PATCH 1/2] InitialCommit Merge the CAN message into NissLeafMng.cpp for PDM and Inverter control. This should allow waking and sleeping of components via VCU CAN frames. --- Makefile | 2 +- include/NissLeafMng.h | 46 ++++ include/param_prj.h | 5 +- src/NissLeafMng.cpp | 494 ++++++++++++++++++++++++++++++++++++++++++ src/NissanPDM.cpp | 340 +---------------------------- src/leafinv.cpp | 414 +---------------------------------- src/stm32_vcu.cpp | 9 +- 7 files changed, 572 insertions(+), 738 deletions(-) create mode 100644 include/NissLeafMng.h create mode 100644 src/NissLeafMng.cpp diff --git a/Makefile b/Makefile index 6050748..c84b18e 100644 --- a/Makefile +++ b/Makefile @@ -44,7 +44,7 @@ OBJSL = $(BINARY).o hwinit.o stm32scheduler.o params.o terminal.o terminal_prj. chademo.o amperaheater.o amperacharger.o subaruvehicle.o iomatrix.o bmw_sbox.o NissanPDM.o teslaCharger.o extCharger.o vag_sbox.o \ daisychainbms.o simpbms.o outlanderCharger.o Can_OBD2.o cansdo.o TeslaDCDC.o BMW_E31.o F30_Lever.o \ CPC.o ElconCharger.o RearOutlanderinverter.o linbus.o VWheater.o JLR_G1.o JLR_G2.o Foccci.o digipot.o\ - OutlanderHeartBeat.o E65_Lever.o leafbms.o V_Classic.o kangoobms.o OutlanderCanHeater.o + OutlanderHeartBeat.o E65_Lever.o leafbms.o V_Classic.o kangoobms.o OutlanderCanHeater.o NissLeafMng.o OBJS = $(patsubst %.o,$(OUT_DIR)/%.o, $(OBJSL)) vpath %.c src/ libopeninv/src/ diff --git a/include/NissLeafMng.h b/include/NissLeafMng.h new file mode 100644 index 0000000..b50040a --- /dev/null +++ b/include/NissLeafMng.h @@ -0,0 +1,46 @@ +/* + * This file is part of the ZombieVerter project. + * + * Copyright (C) 2021-2023 Tom de Bree + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * + */ +#ifndef NISSLEAFMNG_H +#define NISSLEAFMNG_H + +#include +#include "params.h" +#include "canhardware.h" +#include "my_fp.h" +#include "my_math.h" +#include "utils.h" + +class NissLeafMng +{ + +public: +static void Task10Ms(int16_t final_torque_request); +static void Task100Ms(); +static void SetCanInterface(CanHardware* c); +static void SetPullInEVSE(bool pullInEVSE); +static void nissan_crc(uint8_t *data, uint8_t polynomial); + + +protected: + +}; + +#endif diff --git a/include/param_prj.h b/include/param_prj.h index 01c265d..add87ae 100644 --- a/include/param_prj.h +++ b/include/param_prj.h @@ -18,7 +18,7 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ -#define VER 2.21.A +#define VER 2.22.A /* Entries must be ordered as follows: @@ -153,6 +153,7 @@ VALUE_ENTRY(chgtyp, CHGTYPS, 2003 ) \ VALUE_ENTRY(lasterr, errorListString, 2004 ) \ VALUE_ENTRY(status, STATUS, 2005 ) \ + VALUE_ENTRY(CanAct, ONOFF, 2107 ) \ VALUE_ENTRY(TorqDerate, LIMITREASON, 2102 ) \ VALUE_ENTRY(udc, "V", 2006 ) \ VALUE_ENTRY(udc2, "V", 2007 ) \ @@ -242,7 +243,7 @@ VALUE_ENTRY(powerheater, "W", 2098 ) \ VALUE_ENTRY(VehLockSt, ONOFF, 2100 ) \ -//Next value Id: 2104 +//Next value Id: 2108 //Dead params /* diff --git a/src/NissLeafMng.cpp b/src/NissLeafMng.cpp new file mode 100644 index 0000000..c03559a --- /dev/null +++ b/src/NissLeafMng.cpp @@ -0,0 +1,494 @@ +/* + * This file is part of the ZombieVerter project. + * + * Copyright (C) 2021-2023 Tom de Bree + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * + */ + + +#include + +static bool BMSspoof = true; +static bool SendCan = false; +static uint8_t mprun10=0; +static uint8_t mprun100=0; +static uint8_t OBCpwr=0; +static uint16_t SleepCount =0; + +CanHardware* can; + + +void NissLeafMng::SetCanInterface(CanHardware* c) +{ + can = c; +} + +void NissLeafMng::Task10Ms(int16_t final_torque_request) +{ + uint8_t bytes[8]; + + int opmode = Param::GetInt(Param::opmode); + + if(SendCan == true)//only send CAN when needed + { + mprun10 = (mprun10 + 1) % 4; // mprun10 cycles between 0-1-2-3-0-1... + + ///////////////////////////////////////////////////////////////////////////////////////////////// + // CAN Messaage 0x11A + + // Data taken from a gen1 inFrame where the car is starting to + // move at about 10% throttle: 4E400055 0000017D + + // All possible gen1 values: 00 01 0D 11 1D 2D 2E 3D 3E 4D 4E + // MSB nibble: Selected gear (gen1/LeafLogs) + // 0: some kind of non-gear before driving + // 0: Park in Gen 2. byte 0 = 0x01 when in park and charging + // 1: some kind of non-gear after driving + // 2: R + // 3: N + // 4: D + // LSB nibble: ? (LeafLogs) + // 0: sometimes at startup, not always; never when the + // inverted is powered on (0.06%) + // 1: this is the usual value (55% of the time in LeafLogs) + // D: seems to occur for ~90ms when changing gears (0.2%) + // E: this also is a usual value, but never occurs with the + // non-gears 0 and 1 (44% of the time in LeafLogs) + + + //byte 0 determines motor rotation direction + if (opmode == MOD_CHARGE) bytes[0] = 0x01;//Car in park when charging + if (opmode != MOD_CHARGE) bytes[0] = 0x4E; + // 0x40 when car is ON, 0x80 when OFF, 0x50 when ECO. Car must be off when charing 0x80 + if (opmode == MOD_CHARGE) bytes[1] = 0x80; + if (opmode != MOD_CHARGE) bytes[1] = 0x40; + // Usually 0x00, sometimes 0x80 (LeafLogs), 0x04 seen by canmsgs + bytes[2] = 0x00; + + // Weird value at D3:4 that goes along with the counter + // NOTE: Not actually needed, you can just send constant AA C0 + const static uint8_t weird_d34_values[4][2] = + { + {0xaa, 0xc0}, + {0x55, 0x00}, + {0x55, 0x40}, + {0xaa, 0x80}, + }; + + bytes[3] = weird_d34_values[mprun10][0];//0xAA; + bytes[4] = weird_d34_values[mprun10][1];//0xC0; + bytes[5] = 0x00; // Always 0x00 (LeafLogs, canmsgs) + bytes[6] = mprun10; // A 2-bit counter + nissan_crc(bytes, 0x85); + + can->Send(0x11A, (uint32_t*)bytes, 8); + + ///////////////////////////////////////////////////////////////////////////////////////////////// + // CAN Message 0x1D4: Target Motor Torque + + // Data taken from a gen1 inFrame where the car is starting to + // move at about 10% throttle: F70700E0C74430D4 + + // Usually F7, but can have values between 9A...F7 (gen1) + bytes[0] = 0xF7; + // 2016: 6E + + // Usually 07, but can have values between 07...70 (gen1) + bytes[1] = 0x07; + // 2016: 6E + + // override any torque commands if not in run mode. + if (opmode != MOD_RUN) + { + final_torque_request = 0; + } + + // Requested torque (signed 12-bit value + always 0x0 in low nibble) + if(final_torque_request >= -2048 && final_torque_request <= 2047) + { + bytes[2] = ((final_torque_request < 0) ? 0x80 : 0) |((final_torque_request >> 4) & 0x7f); + bytes[3] = (final_torque_request << 4) & 0xf0; + } + else + { + bytes[2] = 0x00; + bytes[3] = 0x00; + } + + // MSB nibble: Runs through the sequence 0, 4, 8, C + // LSB nibble: Precharge report (precedes actual precharge + // control) + // 0: Discharging (5%) + // 2: Precharge not started (1.4%) + // 3: Precharging (0.4%) + // 5: Starting discharge (3x10ms) (2.0%) + // 7: Precharged (93%) + bytes[4] = 0x07 | (mprun10 << 6); + //bytes[4] = 0x02 | (mprun10 << 6); + //Bit 2 is HV status. 0x00 No HV, 0x01 HV On. + + // MSB nibble: + // 0: 35-40ms at startup when gear is 0, then at shutdown 40ms + // after the car has been shut off (6% total) + // 4: Otherwise (94%) + // LSB nibble: + // 0: ~100ms when changing gear, along with 11A D0 b3:0 value + // D (0.3%) + // 2: Reverse gear related (13%) + // 4: Forward gear related (21%) + // 6: Occurs always when gear 11A D0 is 01 or 11 (66%) + //outFrame.data.bytes[5] = 0x44; + //outFrame.data.bytes[5] = 0x46; + + // 2016 drive cycle: 06, 46, precharge, 44, drive, 46, discharge, 06 + // 0x46 requires ~25 torque to start + //outFrame.data.bytes[5] = 0x46; + // 0x44 requires ~8 torque to start + bytes[5] = 0x44; + //bit 6 is Main contactor status. 0x00 Not on, 0x01 on. + + // MSB nibble: + // In a drive cycle, this slowly changes between values (gen1): + // leaf_on_off.txt: + // 5 7 3 2 0 1 3 7 + // leaf_on_rev_off.txt: + // 5 7 3 2 0 6 + // leaf_on_Dx3.txt: + // 5 7 3 2 0 2 3 2 0 2 3 2 0 2 3 7 + // leaf_on_stat_DRDRDR.txt: + // 0 1 3 7 + // leaf_on_Driveincircle_off.txt: + // 5 3 2 0 8 B 3 2 0 8 A B 3 2 0 8 A B A 8 0 2 3 7 + // leaf_on_wotind_off.txt: + // 3 2 0 8 A B 3 7 + // leaf_on_wotinr_off.txt: + // 5 7 3 2 0 8 A B 3 7 + // leaf_ac_charge.txt: + // 4 6 E 6 + // Possibly some kind of control flags, try to figure out + // using: + // grep 000001D4 leaf_on_wotind_off.txt | cut -d' ' -f10 | uniq | ~/projects/leaf_tools/util/hex_to_ascii_binary.py + // 2016: + // Has different values! + // LSB nibble: + // 0: Always (gen1) + // 1: (2016) + + // 2016 drive cycle: + // E0: to 0.15s + // E1: 2 messages + // 61: to 2.06s (inverter is powered up and precharge + // starts and completes during this) + // 21: to 13.9s + // 01: to 17.9s + // 81: to 19.5s + // A1: to 26.8s + // 21: to 31.0s + // 01: to 33.9s + // 81: to 48.8s + // A1: to 53.0s + // 21: to 55.5s + // 61: 2 messages + // 60: to 55.9s + // E0: to end of capture (discharge starts during this) + + // This value has been chosen at the end of the hardest + // acceleration in the wide-open-throttle pull, with full-ish + // torque still being requested, in + // LeafLogs/leaf_on_wotind_off.txt + //outFrame.data.bytes[6] = 0x00; + + // This value has been chosen for being seen most of the time + // when, and before, applying throttle in the wide-open-throttle + // pull, in + // LeafLogs/leaf_on_wotind_off.txt + + if (opmode != MOD_CHARGE) bytes[6] = 0x30; //brake applied heavilly. + if (opmode == MOD_CHARGE) bytes[6] = 0xE0; //charging mode + //In Gen 2 byte 6 is Charge status. + //0x8C Charging interrupted + //0xE0 Charging + + // Value chosen from a 2016 log + //outFrame.data.bytes[6] = 0x61; + + // Value chosen from a 2016 log + // 2016-24kWh-ev-on-drive-park-off.pcap #12101 / 15.63s + // outFrame.data.bytes[6] = 0x01; + //byte 6 brake signal + nissan_crc(bytes, 0x85); + + can->Send(0x1D4, (uint32_t*)bytes, 8);//send on can1 + + ///////////////////////////////////////////////////////////////////////////////////////////////// + // CAN Message 0x1F2: Charge Power and DC/DC Converter Control + + // convert power setpoint to PDM format: + // 0x70 = 3 amps ish + // 0x6a = 1.4A + // 0x66 = 0.5A + // 0x65 = 0.3A + // 0x64 = no chg + // so 0x64=100. 0xA0=160. so 60 decimal steps. 1 step=100W??? + ///////////////////////////////////////////////////////////////////////////////////////////////////// + + + // get actual voltage and voltage setpoints + uint16_t Vbatt = Param::GetInt(Param::udc); + uint16_t VbattSP = Param::GetInt(Param::Voltspnt); + uint16_t calcBMSpwr=(Vbatt * Param::GetInt(Param::BMS_ChargeLim));//BMS charge current limit but needs to be power for most AC charger types. + + uint8_t OBCpwrSP = (MIN(Param::GetInt(Param::Pwrspnt),calcBMSpwr) / 100) + 0x64; + + if (opmode == MOD_CHARGE && Param::GetInt(Param::Chgctrl) == ChargeControl::Enable) + { + // clamp min and max values + if (OBCpwrSP > 0xA0) + OBCpwrSP = 0xA0; + else if (OBCpwrSP < 0x64) + OBCpwrSP = 0x64; + + // if measured vbatt is less than setpoint got to max power from web ui + if (Vbatt < VbattSP) + OBCpwr = OBCpwrSP; + + // decrement charger power if volt setpoint is reached + if (Vbatt >= VbattSP) + OBCpwr--; + } + else + { + // set power to 0 if charge control is set to off or not in charge mode + OBCpwr = 0x64; + } + + + // Commanded chg power in byte 1 and byte 0 bits 0-1. 10 bit number. + // byte 1=0x64 and byte 0=0x00 at 0 power. + // 0x00 chg 0ff dcdc on. + bytes[0] = 0x30; // msg is muxed but pdm doesn't seem to care. + bytes[1] = OBCpwr; + bytes[2] = 0x20;//0x20=Normal Charge + bytes[3] = 0xAC; + bytes[4] = 0x00; + bytes[5] = 0x3C; + bytes[6] = mprun10; + bytes[7] = 0x8F; //may not need checksum here? + + can->Send(0x1F2, (uint32_t*)bytes, 8); + + if(BMSspoof) + { + ///////////////////////////////////////////////////////////////////////////////////////////////// + // CAN Message 0x1DB + + //We need to send 0x1db here with voltage measured by inverter + //Zero seems to work also on my gen1 + //////////////////////////////////////////////////////////////// + //Byte 1 bits 8-10 LB Failsafe Status + //0x00 Normal start req. seems to stay on this value most of the time + //0x01 Normal stop req + //0x02 Charge stop req + //0x03 Charge and normal stop req. Other values call for a caution lamp which we don't need + //bits 11-12 LB relay cut req + //0x00 no req + //0x01,0x02,0x03 main relay off req + uint16_t Ibatt = Param::GetInt(Param::idc)*2; + Vbatt = Vbatt*4; + bytes[0] = Ibatt >> 8; //MSB current. 11 bit signed MSBit first + bytes[1] = Ibatt & 0xE0; //LSB current bits 7-5. Dont need to mess with bits 0-4 for now as 0 works. + bytes[2] = Vbatt >> 8; + bytes[3] = ((Vbatt & 0xC0) | (0x2b)); //0x2b should give no cut req, main rly on permission,normal p limit. + bytes[4] = 0x40; //SOC for dash in Leaf. fixed val. + bytes[5] = 0x00; + bytes[6] = mprun10; + + // Extra CRC in byte 7 + nissan_crc(bytes, 0x85); + + can->Send(0x1DB, (uint32_t*)bytes, 8); + } + + if(BMSspoof) + { + ///////////////////////////////////////////////////////////////////////////////////////////////// + // CAN Message 0x1DC: + + // 0x1dc from lbc. Contains chg power lims and disch power lims. + // Disch power lim in byte 0 and byte 1 bits 6-7. Just set to max for now. + // Max charging power in bits 13-20. 10 bit unsigned scale 0.25.Byte 1 limit in kw. + bytes[0]=0xA0;//0x6E - 110kw limit 0xA0-160Kw limit + bytes[1]=0x0A; + bytes[2]=0x05; + bytes[3]=0xD5; + bytes[4]=0x00;//may not need pairing code crap here...and we don't:) + bytes[5]=0x00; + bytes[6]=mprun10; + // Extra CRC in byte 7 + nissan_crc(bytes, 0x85); + + can->Send(0x1DC, (uint32_t*)bytes, 8); + } + } +} + +void NissLeafMng::Task100Ms() +{ + uint8_t bytes[8]; + int opmode = Param::GetInt(Param::opmode); + + if (Param::GetInt(Param::BMS_Mode) == 4) //Check ran here as this runs on Zombie power on. + { + BMSspoof = false; + } + else + { + BMSspoof = true; + } + + if(SendCan == false)//If not sending CAN check if we need to start + { + if(Param::GetInt(Param::CanAct) == 1)//External CAN wake trigger (currently only PDM) + { + SendCan = true; + SleepCount = 100;//10s shut down counter reset + } + if(opmode == MOD_CHARGE || opmode == MOD_RUN)//If we get put into RUN or Charge mode start CAN + { + SendCan = true; + SleepCount = 100;//10s shut down counter reset + } + } + + if(SendCan == true && opmode == MOD_OFF)//stop sending CAN if we are sending CAN and enter OFF mode + { + if(SleepCount == 0) + { + SendCan = false; + Param::SetInt(Param::CanAct,0);//Reset CAN wake trigger + //Send CAN sleep command and stop sending CAN + bytes[0] = 0x00; + bytes[1] = 0x00; + bytes[2] = 0x00; + bytes[3] = 0x00; + bytes[4] = 0x00; + bytes[5] = 0x00; + bytes[6] = 0x00; + + //possible problem here as 0x50B is DLC 7.... + can->Send(0x50B, (uint32_t*)bytes, 7); + } + else + { + SleepCount --; + } + } + + + if(SendCan == true)//only send CAN when needed + { + mprun100 = (mprun100 + 1) % 4; // mprun100 cycles between 0-1-2-3-0-1... + + ///////////////////////////////////////////////////////////////////////////////////////////////// + // CAN Message 0x50B + + // Statistics from 2016 capture: + // 10 00000000000000 + // 21 000002c0000000 + // 122 000000c0000000 + // 513 000006c0000000 + + // Let's just send the most common one all the time + // FIXME: This is a very sloppy implementation. Thanks. I try:) Dala: This message is not 10ms, it is 100ms + bytes[0] = 0x00; + bytes[1] = 0x00; + bytes[2] = 0x06; + bytes[3] = 0xc0; + bytes[4] = 0x00; + bytes[5] = 0x00; + bytes[6] = 0x00; + + //possible problem here as 0x50B is DLC 7.... + can->Send(0x50B, (uint32_t*)bytes, 7); + + if(BMSspoof) + { + ///////////////////////////////////////////////////////////////////////////////////////////////// + // CAN Message 0x55B: + + bytes[0] = 0xA4; + bytes[1] = 0x40; + bytes[2] = 0xAA; + bytes[3] = 0x00; + bytes[4] = 0xDF; + bytes[5] = 0xC0; + bytes[6] = ((0x1 << 4) | (mprun100)); + // Extra CRC in byte 7 + nissan_crc(bytes, 0x85); + + can->Send(0x55b, (uint32_t*)bytes, 8); + + ///////////////////////////////////////////////////////////////////////////////////////////////// + // CAN Message 0x59E: + + bytes[0] = 0x00;//Static msg works fine here + bytes[1] = 0x00;//Batt capacity for chg and qc. + bytes[2] = 0x0c; + bytes[3] = 0x76; + bytes[4] = 0x18; + bytes[5] = 0x00; + bytes[6] = 0x00; + bytes[7] = 0x00; + + can->Send(0x59e, (uint32_t*)bytes, 8); + + ///////////////////////////////////////////////////////////////////////////////////////////////// + // CAN Message 0x5BC: + + // muxed msg with info for gids etc. Will try static for a test. + bytes[0] = 0x3D;//Static msg works fine here + bytes[1] = 0x80; + bytes[2] = 0xF0; + bytes[3] = 0x64; + bytes[4] = 0xB0; + bytes[5] = 0x01; + bytes[6] = 0x00; + bytes[7] = 0x32; + + can->Send(0x5bc, (uint32_t*)bytes, 8); + } + } +} + +void NissLeafMng::nissan_crc(uint8_t *data, uint8_t polynomial) +{ + // We want to process 8 bytes with the 8th byte being zero + data[7] = 0; + uint8_t crc = 0; + for(int b=0; b<8; b++) + { + for(int i=7; i>=0; i--) + { + uint8_t bit = ((data[b] &(1 << i)) > 0) ? 1 : 0; + if(crc >= 0x80) + crc = (uint8_t)(((crc << 1) + bit) ^ polynomial); + else + crc = (uint8_t)((crc << 1) + bit); + } + } + data[7] = crc; +} diff --git a/src/NissanPDM.cpp b/src/NissanPDM.cpp index 414e1d8..2c102e2 100644 --- a/src/NissanPDM.cpp +++ b/src/NissanPDM.cpp @@ -3,6 +3,7 @@ * * Copyright (C) 2020 Johannes Huebner * 2021-2022 Damien Maguire + * 2024- Tom de Bree * Yes I'm really writing software now........run.....run away....... * * This program is free software: you can redistribute it and/or modify @@ -25,18 +26,11 @@ #include "stm32_can.h" #include "params.h" #include "utils.h" +#include "NissLeafMng.h" -static uint16_t Vbatt=0; -static uint16_t VbattSP=0; -static uint8_t mprun10=0; -static uint8_t mprun100=0; -static uint8_t OBCpwrSP=0; -static uint8_t OBCpwr=0; static bool PPStat = false; static uint8_t OBCVoltStat=0; static uint8_t PlugStat=0; -static uint16_t calcBMSpwr=0; -static bool BMSspoof = true; static uint8_t OBCAvailPwr = 0; static uint8_t OBCActPwr = 0; @@ -82,6 +76,7 @@ QC CAN----------------------PDM EV CAN void NissanPDM::SetCanInterface(CanHardware* c) { + NissLeafMng::SetCanInterface(c);//set Leaf VCM messages on same bus as PDM can = c; can->RegisterUserMessage(0x679);//Leaf obc msg can->RegisterUserMessage(0x390);//Leaf obc msg @@ -91,6 +86,11 @@ void NissanPDM::DecodeCAN(int id, uint32_t data[2]) { uint8_t* bytes = (uint8_t*)data;// arrgghhh this converts the two 32bit array into bytes. See comments are useful:) + if (id == 0x679)// WAKE UP MSG FROM PDM + { + Param::SetInt(Param::CanAct, 1);//PDM wants to talk so we need to talk back to it. + } + if (id == 0x390)// THIS MSG FROM PDM { OBCVoltStat = (bytes[3] >> 3) & 0x03; @@ -192,316 +192,21 @@ bool NissanPDM::ControlCharge(bool RunCh, bool ACReq) //Modeled off of Outlander void NissanPDM::Task10Ms() { int opmode = Param::GetInt(Param::opmode); - if (Param::GetInt(Param::BMS_Mode) == 4) - { - BMSspoof = false; - } - else - { - BMSspoof = true; - } - uint8_t bytes[8]; - mprun10 = (mprun10 + 1) % 4; // mprun10 cycles between 0-1-2-3-0-1... - - if (opmode == MOD_CHARGE) //ONLY send when charging else sent by leafinv.cpp + if (opmode != MOD_RUN) //Outside of Run PDM sends VCM 10ms task else sent by leafinv.cpp { - - ///////////////////////////////////////////////////////////////////////////////////////////////// - // CAN Messaage 0x11A - - // Data taken from a gen1 inFrame where the car is starting to - // move at about 10% throttle: 4E400055 0000017D - - // All possible gen1 values: 00 01 0D 11 1D 2D 2E 3D 3E 4D 4E - // MSB nibble: Selected gear (gen1/LeafLogs) - // 0: some kind of non-gear before driving - // 0: Park in Gen 2. byte 0 = 0x01 when in park and charging - // 1: some kind of non-gear after driving - // 2: R - // 3: N - // 4: D - // LSB nibble: ? (LeafLogs) - // 0: sometimes at startup, not always; never when the - // inverted is powered on (0.06%) - // 1: this is the usual value (55% of the time in LeafLogs) - // D: seems to occur for ~90ms when changing gears (0.2%) - // E: this also is a usual value, but never occurs with the - // non-gears 0 and 1 (44% of the time in LeafLogs) - - - //byte 0 determines motor rotation direction - bytes[0] = 0x01;//Car in park when charging - // 0x40 when car is ON, 0x80 when OFF, 0x50 when ECO. Car must be off when charing 0x80 - bytes[1] = 0x80; - // Usually 0x00, sometimes 0x80 (LeafLogs), 0x04 seen by canmsgs - bytes[2] = 0x00; - - // Weird value at D3:4 that goes along with the counter - // NOTE: Not actually needed, you can just send constant AA C0 - const static uint8_t weird_d34_values[4][2] = - { - {0xaa, 0xc0}, - {0x55, 0x00}, - {0x55, 0x40}, - {0xaa, 0x80}, - }; - - bytes[3] = weird_d34_values[mprun10][0];//0xAA; - bytes[4] = weird_d34_values[mprun10][1];//0xC0; - bytes[5] = 0x00; // Always 0x00 (LeafLogs, canmsgs) - bytes[6] = mprun10; // A 2-bit counter - nissan_crc(bytes, 0x85); - - can->Send(0x11A, (uint32_t*)bytes, 8); - - - ///////////////////////////////////////////////////////////////////////////////////////////////// - // CAN Message 0x1D4: Target Motor Torque - - // Data taken from a gen1 inFrame where the car is starting to - // move at about 10% throttle: F70700E0C74430D4 - - // Usually F7, but can have values between 9A...F7 (gen1) - bytes[0] = 0xF7; - - // Usually 07, but can have values between 07...70 (gen1) - bytes[1] = 0x07; - - bytes[2] = 0x00; - bytes[3] = 0x00; - - // MSB nibble: Runs through the sequence 0, 4, 8, C - // LSB nibble: Precharge report (precedes actual precharge - // control) - // 0: Discharging (5%) - // 2: Precharge not started (1.4%) - // 3: Precharging (0.4%) - // 5: Starting discharge (3x10ms) (2.0%) - // 7: Precharged (93%) - bytes[4] = 0x07 | (mprun10 << 6); - //bytes[4] = 0x02 | (mprun10 << 6); - //Bit 2 is HV status. 0x00 No HV, 0x01 HV On. - - // MSB nibble: - // 0: 35-40ms at startup when gear is 0, then at shutdown 40ms - // after the car has been shut off (6% total) - // 4: Otherwise (94%) - // LSB nibble: - // 0: ~100ms when changing gear, along with 11A D0 b3:0 value - // D (0.3%) - // 2: Reverse gear related (13%) - // 4: Forward gear related (21%) - // 6: Occurs always when gear 11A D0 is 01 or 11 (66%) - //outFrame.data.bytes[5] = 0x44; - //outFrame.data.bytes[5] = 0x46; - - // 2016 drive cycle: 06, 46, precharge, 44, drive, 46, discharge, 06 - // 0x46 requires ~25 torque to start - //outFrame.data.bytes[5] = 0x46; - // 0x44 requires ~8 torque to start - bytes[5] = 0x44; - bytes[6] = 0xE0; //charging mode - - // Extra CRC - nissan_crc(bytes, 0x85); - - can->Send(0x1D4, (uint32_t*)bytes, 8);//send on can1 + NissLeafMng::Task10Ms(0);//request no torque, MODE handling done inside } - - - if(BMSspoof) - { - ///////////////////////////////////////////////////////////////////////////////////////////////// - // CAN Message 0x1DB - - //We need to send 0x1db here with voltage measured by inverter - //Zero seems to work also on my gen1 - //////////////////////////////////////////////////////////////// - //Byte 1 bits 8-10 LB Failsafe Status - //0x00 Normal start req. seems to stay on this value most of the time - //0x01 Normal stop req - //0x02 Charge stop req - //0x03 Charge and normal stop req. Other values call for a caution lamp which we don't need - //bits 11-12 LB relay cut req - //0x00 no req - //0x01,0x02,0x03 main relay off req - s16fp TMP_battI = (Param::Get(Param::idc))*2; - s16fp TMP_battV = (Param::Get(Param::udc))*4; - bytes[0] = TMP_battI >> 8; //MSB current. 11 bit signed MSBit first - bytes[1] = TMP_battI & 0xE0; //LSB current bits 7-5. Dont need to mess with bits 0-4 for now as 0 works. - bytes[2] = TMP_battV >> 8; - bytes[3] = ((TMP_battV & 0xC0) | (0x2b)); //0x2b should give no cut req, main rly on permission,normal p limit. - bytes[4] = 0x40; //SOC for dash in Leaf. fixed val. - bytes[5] = 0x00; - bytes[6] = mprun10; - - // Extra CRC in byte 7 - nissan_crc(bytes, 0x85); - - can->Send(0x1DB, (uint32_t*)bytes, 8); - } - - - ///////////////////////////////////////////////////////////////////////////////////////////////// - // CAN Message 0x50B - - // Statistics from 2016 capture: - // 10 00000000000000 - // 21 000002c0000000 - // 122 000000c0000000 - // 513 000006c0000000 - - // Let's just send the most common one all the time - // FIXME: This is a very sloppy implementation. Thanks. I try:) Dala: This message is not 10ms, it is 100ms - bytes[0] = 0x00; - bytes[1] = 0x00; - bytes[2] = 0x06; - bytes[3] = 0xc0; - bytes[4] = 0x00; - bytes[5] = 0x00; - bytes[6] = 0x00; - - //possible problem here as 0x50B is DLC 7.... - can->Send(0x50B, (uint32_t*)bytes, 7); - - if(BMSspoof) - { - ///////////////////////////////////////////////////////////////////////////////////////////////// - // CAN Message 0x1DC: - - // 0x1dc from lbc. Contains chg power lims and disch power lims. - // Disch power lim in byte 0 and byte 1 bits 6-7. Just set to max for now. - // Max charging power in bits 13-20. 10 bit unsigned scale 0.25.Byte 1 limit in kw. - bytes[0]=0x6E; - bytes[1]=0x0A; - bytes[2]=0x05; - bytes[3]=0xD5; - bytes[4]=0x00;//may not need pairing code crap here...and we don't:) - bytes[5]=0x00; - bytes[6]=mprun10; - // Extra CRC in byte 7 - nissan_crc(bytes, 0x85); - - can->Send(0x1DC, (uint32_t*)bytes, 8); - } - - ///////////////////////////////////////////////////////////////////////////////////////////////// - // CAN Message 0x1F2: Charge Power and DC/DC Converter Control - - // convert power setpoint to PDM format: - // 0x70 = 3 amps ish - // 0x6a = 1.4A - // 0x66 = 0.5A - // 0x65 = 0.3A - // 0x64 = no chg - // so 0x64=100. 0xA0=160. so 60 decimal steps. 1 step=100W??? - ///////////////////////////////////////////////////////////////////////////////////////////////////// - - - // get actual voltage and voltage setpoints - Vbatt = Param::GetInt(Param::udc); - VbattSP = Param::GetInt(Param::Voltspnt); - calcBMSpwr=(Vbatt * Param::GetInt(Param::BMS_ChargeLim));//BMS charge current limit but needs to be power for most AC charger types. - - - OBCpwrSP = (MIN(Param::GetInt(Param::Pwrspnt),calcBMSpwr) / 100) + 0x64; - - if (opmode == MOD_CHARGE && Param::GetInt(Param::Chgctrl) == ChargeControl::Enable) - { - // clamp min and max values - if (OBCpwrSP > 0xA0) - OBCpwrSP = 0xA0; - else if (OBCpwrSP < 0x64) - OBCpwrSP = 0x64; - - // if measured vbatt is less than setpoint got to max power from web ui - if (Vbatt < VbattSP) - OBCpwr = OBCpwrSP; - - // decrement charger power if volt setpoint is reached - if (Vbatt >= VbattSP) - OBCpwr--; - } - else - { - // set power to 0 if charge control is set to off or not in charge mode - OBCpwr = 0x64; - } - - - // Commanded chg power in byte 1 and byte 0 bits 0-1. 10 bit number. - // byte 1=0x64 and byte 0=0x00 at 0 power. - // 0x00 chg 0ff dcdc on. - bytes[0] = 0x30; // msg is muxed but pdm doesn't seem to care. - bytes[1] = OBCpwr; - bytes[2] = 0x20;//0x20=Normal Charge - bytes[3] = 0xAC; - bytes[4] = 0x00; - bytes[5] = 0x3C; - bytes[6] = mprun10; - bytes[7] = 0x8F; //may not need checksum here? - - can->Send(0x1F2, (uint32_t*)bytes, 8); } void NissanPDM::Task100Ms() { - - - // MSGS for charging with pdm - uint8_t bytes[8]; - mprun100 = (mprun100 + 1) % 4; // mprun100 cycles between 0-1-2-3-0-1... - - if(BMSspoof) + if(Param::GetInt(Param::Inverter) != InvModes::Leaf_Gen1)//only run 100ms VCM task if leaf inverter not used { - ///////////////////////////////////////////////////////////////////////////////////////////////// - // CAN Message 0x55B: - - bytes[0] = 0xA4; - bytes[1] = 0x40; - bytes[2] = 0xAA; - bytes[3] = 0x00; - bytes[4] = 0xDF; - bytes[5] = 0xC0; - bytes[6] = ((0x1 << 4) | (mprun100)); - // Extra CRC in byte 7 - nissan_crc(bytes, 0x85); - - can->Send(0x55b, (uint32_t*)bytes, 8); - - ///////////////////////////////////////////////////////////////////////////////////////////////// - // CAN Message 0x59E: - - bytes[0] = 0x00;//Static msg works fine here - bytes[1] = 0x00;//Batt capacity for chg and qc. - bytes[2] = 0x0c; - bytes[3] = 0x76; - bytes[4] = 0x18; - bytes[5] = 0x00; - bytes[6] = 0x00; - bytes[7] = 0x00; - - can->Send(0x59e, (uint32_t*)bytes, 8); - - ///////////////////////////////////////////////////////////////////////////////////////////////// - // CAN Message 0x5BC: - - // muxed msg with info for gids etc. Will try static for a test. - bytes[0] = 0x3D;//Static msg works fine here - bytes[1] = 0x80; - bytes[2] = 0xF0; - bytes[3] = 0x64; - bytes[4] = 0xB0; - bytes[5] = 0x01; - bytes[6] = 0x00; - bytes[7] = 0x32; - - can->Send(0x5bc, (uint32_t*)bytes, 8); + NissLeafMng::Task100Ms(); } Param::SetInt(Param::PilotLim,float(OBCAvailPwr/2.25)); - } @@ -514,24 +219,3 @@ int8_t NissanPDM::fahrenheit_to_celsius(uint16_t fahrenheit) return 127; return result; } - - - -void NissanPDM::nissan_crc(uint8_t *data, uint8_t polynomial) -{ - // We want to process 8 bytes with the 8th byte being zero - data[7] = 0; - uint8_t crc = 0; - for(int b=0; b<8; b++) - { - for(int i=7; i>=0; i--) - { - uint8_t bit = ((data[b] &(1 << i)) > 0) ? 1 : 0; - if(crc >= 0x80) - crc = (uint8_t)(((crc << 1) + bit) ^ polynomial); - else - crc = (uint8_t)((crc << 1) + bit); - } - } - data[7] = crc; -} diff --git a/src/leafinv.cpp b/src/leafinv.cpp index c47f371..df3e7d5 100644 --- a/src/leafinv.cpp +++ b/src/leafinv.cpp @@ -4,6 +4,7 @@ * Copyright (C) 2020 Johannes Huebner * 2021-2022 Damien Maguire * Yes I'm really writing software now........run.....run away....... + * 2024- Tom de Bree * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -25,14 +26,7 @@ #include "stm32_can.h" #include "params.h" #include "utils.h" - -static uint16_t Vbatt=0; -static uint16_t VbattSP=0; -static uint8_t mprun10=0; -static uint8_t mprun100=0; -static uint8_t OBCpwrSP=0; -static uint8_t OBCpwr=0; -static bool BMSspoofI = true; +#include "NissLeafMng.h" /*Info on running Leaf Gen 2 PDM IDs required : @@ -53,6 +47,7 @@ PDM sends: void LeafINV::SetCanInterface(CanHardware* c) { + NissLeafMng::SetCanInterface(c);//set Leaf VCM messages on same bus as Inverter can = c; can->RegisterUserMessage(0x1DA);//Leaf inv msg @@ -99,389 +94,16 @@ void LeafINV::SetTorque(float torquePercent) Param::SetInt(Param::torque,final_torque_request);//post processed final torque value sent to inv to web interface } -void LeafINV::Task10Ms() +void LeafINV::Task10Ms()//ONLY RAN IN RUNMODE { - int opmode = Param::GetInt(Param::opmode); - - if (Param::GetInt(Param::BMS_Mode) == 4) - { - BMSspoofI = false; - } - else - { - BMSspoofI = true; - } - - uint8_t bytes[8]; - mprun10 = (mprun10 + 1) % 4; // mprun10 cycles between 0-1-2-3-0-1... - - ///////////////////////////////////////////////////////////////////////////////////////////////// - // CAN Messaage 0x11A - - // Data taken from a gen1 inFrame where the car is starting to - // move at about 10% throttle: 4E400055 0000017D - - // All possible gen1 values: 00 01 0D 11 1D 2D 2E 3D 3E 4D 4E - // MSB nibble: Selected gear (gen1/LeafLogs) - // 0: some kind of non-gear before driving - // 0: Park in Gen 2. byte 0 = 0x01 when in park and charging - // 1: some kind of non-gear after driving - // 2: R - // 3: N - // 4: D - // LSB nibble: ? (LeafLogs) - // 0: sometimes at startup, not always; never when the - // inverted is powered on (0.06%) - // 1: this is the usual value (55% of the time in LeafLogs) - // D: seems to occur for ~90ms when changing gears (0.2%) - // E: this also is a usual value, but never occurs with the - // non-gears 0 and 1 (44% of the time in LeafLogs) - - - //byte 0 determines motor rotation direction - if (opmode == MOD_CHARGE) bytes[0] = 0x01;//Car in park when charging - if (opmode != MOD_CHARGE) bytes[0] = 0x4E; - // 0x40 when car is ON, 0x80 when OFF, 0x50 when ECO. Car must be off when charing 0x80 - if (opmode == MOD_CHARGE) bytes[1] = 0x80; - if (opmode != MOD_CHARGE) bytes[1] = 0x40; - // Usually 0x00, sometimes 0x80 (LeafLogs), 0x04 seen by canmsgs - bytes[2] = 0x00; - - // Weird value at D3:4 that goes along with the counter - // NOTE: Not actually needed, you can just send constant AA C0 - const static uint8_t weird_d34_values[4][2] = - { - {0xaa, 0xc0}, - {0x55, 0x00}, - {0x55, 0x40}, - {0xaa, 0x80}, - }; - - bytes[3] = weird_d34_values[mprun10][0];//0xAA; - bytes[4] = weird_d34_values[mprun10][1];//0xC0; - bytes[5] = 0x00; // Always 0x00 (LeafLogs, canmsgs) - bytes[6] = mprun10; // A 2-bit counter - bytes[7] = nissan_crc(bytes); - - can->Send(0x11A, (uint32_t*)bytes, 8); - - - ///////////////////////////////////////////////////////////////////////////////////////////////// - // CAN Message 0x1D4: Target Motor Torque - - // Data taken from a gen1 inFrame where the car is starting to - // move at about 10% throttle: F70700E0C74430D4 - - // Usually F7, but can have values between 9A...F7 (gen1) - bytes[0] = 0xF7; - // 2016: 6E - // outFrame.data.bytes[0] = 0x6E; - - // Usually 07, but can have values between 07...70 (gen1) - bytes[1] = 0x07; - // 2016: 6E - //outFrame.data.bytes[1] = 0x6E; - - // override any torque commands if not in run mode. - if (opmode != MOD_RUN) - { - final_torque_request = 0; - } - - // Requested torque (signed 12-bit value + always 0x0 in low nibble) - if(final_torque_request >= -2048 && final_torque_request <= 2047) - { - bytes[2] = ((final_torque_request < 0) ? 0x80 : 0) |((final_torque_request >> 4) & 0x7f); - bytes[3] = (final_torque_request << 4) & 0xf0; - } - else - { - bytes[2] = 0x00; - bytes[3] = 0x00; - } - - // MSB nibble: Runs through the sequence 0, 4, 8, C - // LSB nibble: Precharge report (precedes actual precharge - // control) - // 0: Discharging (5%) - // 2: Precharge not started (1.4%) - // 3: Precharging (0.4%) - // 5: Starting discharge (3x10ms) (2.0%) - // 7: Precharged (93%) - bytes[4] = 0x07 | (mprun10 << 6); - //bytes[4] = 0x02 | (mprun10 << 6); - //Bit 2 is HV status. 0x00 No HV, 0x01 HV On. - - // MSB nibble: - // 0: 35-40ms at startup when gear is 0, then at shutdown 40ms - // after the car has been shut off (6% total) - // 4: Otherwise (94%) - // LSB nibble: - // 0: ~100ms when changing gear, along with 11A D0 b3:0 value - // D (0.3%) - // 2: Reverse gear related (13%) - // 4: Forward gear related (21%) - // 6: Occurs always when gear 11A D0 is 01 or 11 (66%) - //outFrame.data.bytes[5] = 0x44; - //outFrame.data.bytes[5] = 0x46; - - // 2016 drive cycle: 06, 46, precharge, 44, drive, 46, discharge, 06 - // 0x46 requires ~25 torque to start - //outFrame.data.bytes[5] = 0x46; - // 0x44 requires ~8 torque to start - bytes[5] = 0x44; - //bit 6 is Main contactor status. 0x00 Not on, 0x01 on. - - // MSB nibble: - // In a drive cycle, this slowly changes between values (gen1): - // leaf_on_off.txt: - // 5 7 3 2 0 1 3 7 - // leaf_on_rev_off.txt: - // 5 7 3 2 0 6 - // leaf_on_Dx3.txt: - // 5 7 3 2 0 2 3 2 0 2 3 2 0 2 3 7 - // leaf_on_stat_DRDRDR.txt: - // 0 1 3 7 - // leaf_on_Driveincircle_off.txt: - // 5 3 2 0 8 B 3 2 0 8 A B 3 2 0 8 A B A 8 0 2 3 7 - // leaf_on_wotind_off.txt: - // 3 2 0 8 A B 3 7 - // leaf_on_wotinr_off.txt: - // 5 7 3 2 0 8 A B 3 7 - // leaf_ac_charge.txt: - // 4 6 E 6 - // Possibly some kind of control flags, try to figure out - // using: - // grep 000001D4 leaf_on_wotind_off.txt | cut -d' ' -f10 | uniq | ~/projects/leaf_tools/util/hex_to_ascii_binary.py - // 2016: - // Has different values! - // LSB nibble: - // 0: Always (gen1) - // 1: (2016) - - // 2016 drive cycle: - // E0: to 0.15s - // E1: 2 messages - // 61: to 2.06s (inverter is powered up and precharge - // starts and completes during this) - // 21: to 13.9s - // 01: to 17.9s - // 81: to 19.5s - // A1: to 26.8s - // 21: to 31.0s - // 01: to 33.9s - // 81: to 48.8s - // A1: to 53.0s - // 21: to 55.5s - // 61: 2 messages - // 60: to 55.9s - // E0: to end of capture (discharge starts during this) - - // This value has been chosen at the end of the hardest - // acceleration in the wide-open-throttle pull, with full-ish - // torque still being requested, in - // LeafLogs/leaf_on_wotind_off.txt - //outFrame.data.bytes[6] = 0x00; - - // This value has been chosen for being seen most of the time - // when, and before, applying throttle in the wide-open-throttle - // pull, in - // LeafLogs/leaf_on_wotind_off.txt - - if (opmode != MOD_CHARGE) bytes[6] = 0x30; //brake applied heavilly. - if (opmode == MOD_CHARGE) bytes[6] = 0xE0; //charging mode - //In Gen 2 byte 6 is Charge status. - //0x8C Charging interrupted - //0xE0 Charging - - // Value chosen from a 2016 log - //outFrame.data.bytes[6] = 0x61; - - // Value chosen from a 2016 log - // 2016-24kWh-ev-on-drive-park-off.pcap #12101 / 15.63s - // outFrame.data.bytes[6] = 0x01; - //byte 6 brake signal - bytes[7] = nissan_crc(bytes); - - can->Send(0x1D4, (uint32_t*)bytes, 8);//send on can1 - - - if(BMSspoofI) - { - ///////////////////////////////////////////////////////////////////////////////////////////////// - // CAN Message 0x1DB - - //We need to send 0x1db here with voltage measured by inverter - //Zero seems to work also on my gen1 - //////////////////////////////////////////////////////////////// - //Byte 1 bits 8-10 LB Failsafe Status - //0x00 Normal start req. seems to stay on this value most of the time - //0x01 Normal stop req - //0x02 Charge stop req - //0x03 Charge and normal stop req. Other values call for a caution lamp which we don't need - //bits 11-12 LB relay cut req - //0x00 no req - //0x01,0x02,0x03 main relay off req - s16fp TMP_battI = (Param::Get(Param::idc))*2; - s16fp TMP_battV = (Param::Get(Param::udc))*4; - bytes[0] = TMP_battI >> 8; //MSB current. 11 bit signed MSBit first - bytes[1] = TMP_battI & 0xE0; //LSB current bits 7-5. Dont need to mess with bits 0-4 for now as 0 works. - bytes[2] = TMP_battV >> 8; - bytes[3] = ((TMP_battV & 0xC0) | (0x2b)); //0x2b should give no cut req, main rly on permission,normal p limit. - bytes[4] = 0x40; //SOC for dash in Leaf. fixed val. - bytes[5] = 0x00; - bytes[6] = mprun10; - bytes[7] = nissan_crc(bytes); - can->Send(0x1DB, (uint32_t*)bytes, 8); - } - - if(BMSspoofI) - { - ///////////////////////////////////////////////////////////////////////////////////////////////// - // CAN Message 0x1DC: - - // 0x1dc from lbc. Contains chg power lims and disch power lims. - // Disch power lim in byte 0 and byte 1 bits 6-7. Just set to max for now. - // Max charging power in bits 13-20. 10 bit unsigned scale 0.25.Byte 1 limit in kw. - bytes[0]=0x6E; - bytes[1]=0x0A; - bytes[2]=0x05; - bytes[3]=0xD5; - bytes[4]=0x00;//may not need pairing code crap here...and we don't:) - bytes[5]=0x00; - bytes[6]=mprun10; - bytes[7] = nissan_crc(bytes); - - can->Send(0x1DC, (uint32_t*)bytes, 8); - } - - ///////////////////////////////////////////////////////////////////////////////////////////////// - // CAN Message 0x1F2: Charge Power and DC/DC Converter Control - - // convert power setpoint to PDM format: - // 0x70 = 3 amps ish - // 0x6a = 1.4A - // 0x66 = 0.5A - // 0x65 = 0.3A - // 0x64 = no chg - // so 0x64=100. 0xA0=160. so 60 decimal steps. 1 step=100W??? - OBCpwrSP = (Param::GetInt(Param::Pwrspnt) / 100) + 0x64; - - // get actual voltage and voltage setpoints - Vbatt = Param::GetInt(Param::udc); - VbattSP = Param::GetInt(Param::Voltspnt); - - if (opmode == MOD_CHARGE && Param::GetInt(Param::Chgctrl) == ChargeControl::Enable) - { - // clamp min and max values - if (OBCpwrSP > 0xA0) - OBCpwrSP = 0xA0; - else if (OBCpwrSP < 0x64) - OBCpwrSP = 0x64; - - // if measured vbatt is less than setpoint got to max power from web ui - if (Vbatt < VbattSP) - OBCpwr = OBCpwrSP; - - // decrement charger power if volt setpoint is reached - if (Vbatt >= VbattSP) - OBCpwr--; - } - else - { - // set power to 0 if charge control is set to off or not in charge mode - OBCpwr = 0x64; - } - - - // Commanded chg power in byte 1 and byte 0 bits 0-1. 10 bit number. - // byte 1=0x64 and byte 0=0x00 at 0 power. - // 0x00 chg 0ff dcdc on. - bytes[0] = 0x30; // msg is muxed but pdm doesn't seem to care. - bytes[1] = OBCpwr; - bytes[2] = 0x20; - bytes[3] = 0xAC; - bytes[4] = 0x00; - bytes[5] = 0x3C; - bytes[6] = mprun10; - bytes[7] = 0x8F; //may not need checksum here? - - can->Send(0x1F2, (uint32_t*)bytes, 8); + NissLeafMng::Task10Ms(final_torque_request);//This is only called when in run mode, send torque request with it } -void LeafINV::Task100Ms() +void LeafINV::Task100Ms() //Always ran { - - // MSGS for charging with pdm - uint8_t bytes[8]; - mprun100 = (mprun100 + 1) % 4; // mprun100 cycles between 0-1-2-3-0-1... - - ///////////////////////////////////////////////////////////////////////////////////////////////// - // CAN Message 0x50B - Normally sent from VCM on a LEAF - - // Statistics from 2016 capture: - // 10 00000000000000 - // 21 000002c0000000 - // 122 000000c0000000 - // 513 000006c0000000 - - bytes[0] = 0x00; - bytes[1] = 0x00; - bytes[2] = 0x06; //HCM_WakeUpSleepCommand == 11b == WakeUp - bytes[3] = 0xc0; //CANMASK = 1 - bytes[4] = 0x00; - bytes[5] = 0x00; - bytes[6] = 0x00; - - //possible problem here as 0x50B is DLC 7.... - can->Send(0x50B, (uint32_t*)bytes, 7); - - if(BMSspoofI) - { - ///////////////////////////////////////////////////////////////////////////////////////////////// - // CAN Message 0x55B - Normally sent from BMS on a LEAF - - bytes[0] = 0xA4; - bytes[1] = 0x40; - bytes[2] = 0xAA; - bytes[3] = 0x00; - bytes[4] = 0xDF; - bytes[5] = 0xC0; - bytes[6] = ((0x1 << 4) | (mprun100)); - bytes[7] = nissan_crc(bytes); - - can->Send(0x55b, (uint32_t*)bytes, 8); - - ///////////////////////////////////////////////////////////////////////////////////////////////// - // CAN Message 0x59E - Normally sent from BMS on a LEAF - - bytes[0] = 0x00;//Static msg works fine here - bytes[1] = 0x00;//Batt capacity for chg and qc. - bytes[2] = 0x0c; - bytes[3] = 0x76; - bytes[4] = 0x18; - bytes[5] = 0x00; - bytes[6] = 0x00; - bytes[7] = 0x00; - - can->Send(0x59e, (uint32_t*)bytes, 8); - - ///////////////////////////////////////////////////////////////////////////////////////////////// - // CAN Message 0x5BC - Normally sent from BMS on a LEAF - - // muxed msg with info for gids etc. Will try static for a test. - bytes[0] = 0x3D;//Static msg works fine here - bytes[1] = 0x80; - bytes[2] = 0xF0; - bytes[3] = 0x64; - bytes[4] = 0xB0; - bytes[5] = 0x01; - bytes[6] = 0x00; - bytes[7] = 0x32; - - can->Send(0x5bc, (uint32_t*)bytes, 8); - } + //Inverter calling of 100ms VCM task takes precedences over PDM + NissLeafMng::Task100Ms(); } @@ -494,23 +116,3 @@ int8_t LeafINV::fahrenheit_to_celsius(uint16_t fahrenheit) return 127; return result; } - - - -uint8_t LeafINV::nissan_crc(uint8_t *data) -{ - uint8_t crc = 0; - uint8_t polynomial = 0x85; - - for (int b = 0; b < 8; b++) { - uint8_t byte = (b == 7) ? 0 : data[b]; // Treat 8th byte as 0 during calculation. - for (int i = 7; i >= 0; i--) { - uint8_t bit = ((byte & (1 << i)) > 0) ? 1 : 0; - if (crc >= 0x80) - crc = (uint8_t)(((crc << 1) + bit) ^ polynomial); - else - crc = (uint8_t)((crc << 1) + bit); - } - } - return crc; -} diff --git a/src/stm32_vcu.cpp b/src/stm32_vcu.cpp index 761f180..1d16cde 100644 --- a/src/stm32_vcu.cpp +++ b/src/stm32_vcu.cpp @@ -576,7 +576,14 @@ static void Ms10Task(void) selectedVehicle->Task10Ms(); selectedDCDC->Task10Ms(); selectedShifter->Task10Ms(); - if(opmode==MOD_CHARGE) selectedCharger->Task10Ms(); + if(opmode==MOD_CHARGE) + { + selectedCharger->Task10Ms(); + } + else if (Param::GetInt(Param::chargemodes) == ChargeModes::Leaf_PDM) + { + selectedCharger->Task10Ms(); + } if(opmode==MOD_RUN) Param::SetInt(Param::canctr, (Param::GetInt(Param::canctr) + 1) & 0xF);//Update the OI can counter in RUN mode only ////////////////////////////////////////////////// From 609c1dee01e13d62b4e321bd6336c0c0ad8cf83e Mon Sep 17 00:00:00 2001 From: tomdebree Date: Wed, 15 Jan 2025 10:55:11 +0000 Subject: [PATCH 2/2] Clean up mapping 0x1DB Fix issues in mapping of 0x1DB --- src/NissLeafMng.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/NissLeafMng.cpp b/src/NissLeafMng.cpp index c03559a..d161c2f 100644 --- a/src/NissLeafMng.cpp +++ b/src/NissLeafMng.cpp @@ -308,11 +308,11 @@ void NissLeafMng::Task10Ms(int16_t final_torque_request) //0x00 no req //0x01,0x02,0x03 main relay off req uint16_t Ibatt = Param::GetInt(Param::idc)*2; - Vbatt = Vbatt*4; - bytes[0] = Ibatt >> 8; //MSB current. 11 bit signed MSBit first - bytes[1] = Ibatt & 0xE0; //LSB current bits 7-5. Dont need to mess with bits 0-4 for now as 0 works. - bytes[2] = Vbatt >> 8; - bytes[3] = ((Vbatt & 0xC0) | (0x2b)); //0x2b should give no cut req, main rly on permission,normal p limit. + Vbatt = Vbatt*2; + bytes[0] = Ibatt >> 3; //MSB current. 11 bit signed MSBit first + bytes[1] = (Ibatt & 0x07) << 5; //LSB current bits 7-5. Dont need to mess with bits 0-4 for now as 0 works. + bytes[2] = Vbatt >> 2; + bytes[3] = (((Vbatt & 0x07) << 6)| (0x2b)); //0x2b should give no cut req, main rly on permission,normal p limit. bytes[4] = 0x40; //SOC for dash in Leaf. fixed val. bytes[5] = 0x00; bytes[6] = mprun10;