rusefi
/
ultimate-nag52-fw
mirror of https://github.com/rusefi/ultimate-nag52-fw.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #49 from rnd-ash/dev 

Latest DEV changes
This commit is contained in:
Ashcon Mohseninia (RAND_ASH) 2023-01-03 08:30:07 +00:00 committed by GitHub
parent 52613c4800 c1fa4998d0
commit 064e292a6e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
110 changed files with 6123 additions and 22959 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

120
.github/workflows/fw_build_upload.yml vendored
View File

@ -3,11 +3,9 @@ name: PlatformIO Build
on:
push:
branches: [ "dev" ]
pull_request:
branches: [ "main" ]
jobs:
EGS51:
UNIFIED_FW:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v3
@ -31,112 +29,20 @@ jobs:
run: |
python -m pip install --upgrade pip
pip install --upgrade platformio
- name: Build EGS52 FW
- name: Build Prod PCB FW
run: |
pio run -e egs51
pio run -e unified
- uses: actions/upload-artifact@v3
with:
name: EGS51_FW
name: UNIFIED_FW
path: |
.pio/build/egs51/firmware.elf
.pio/build/egs51/firmware.bin
EGS52:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v3
.pio/build/unified/firmware.elf
.pio/build/unified/firmware.bin
- uses: pyTooling/Actions/releaser@r0
with:
submodules: "recursive"
- name: Cache pip
uses: actions/cache@v2
with:
path: ~/.cache/pip
key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }}
restore-keys: |
${{ runner.os }}-pip-
- name: Cache PlatformIO
uses: actions/cache@v2
with:
path: ~/.platformio
key: ${{ runner.os }}-${{ hashFiles('**/lockfiles') }}
- name: Set up Python
uses: actions/setup-python@v2
- name: Install PlatformIO
run: |
python -m pip install --upgrade pip
pip install --upgrade platformio
- name: Build EGS52 FW
run: |
pio run -e egs52
- uses: actions/upload-artifact@v3
with:
name: EGS52_FW
path: |
.pio/build/egs52/firmware.elf
.pio/build/egs52/firmware.bin
EGS52-RED-PCB:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v3
with:
submodules: "recursive"
- name: Cache pip
uses: actions/cache@v2
with:
path: ~/.cache/pip
key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }}
restore-keys: |
${{ runner.os }}-pip-
- name: Cache PlatformIO
uses: actions/cache@v2
with:
path: ~/.platformio
key: ${{ runner.os }}-${{ hashFiles('**/lockfiles') }}
- name: Set up Python
uses: actions/setup-python@v2
- name: Install PlatformIO
run: |
python -m pip install --upgrade pip
pip install --upgrade platformio
- name: Build EGS52 FW
run: |
pio run -e egs52-red-pcb
- uses: actions/upload-artifact@v3
with:
name: EGS52_FW-RED-PCB
path: |
.pio/build/egs52-red-pcb/firmware.elf
.pio/build/egs52-red-pcb/firmware.bin
EGS53:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v3
with:
submodules: "recursive"
- name: Cache pip
uses: actions/cache@v2
with:
path: ~/.cache/pip
key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }}
restore-keys: |
${{ runner.os }}-pip-
- name: Cache PlatformIO
uses: actions/cache@v2
with:
path: ~/.platformio
key: ${{ runner.os }}-${{ hashFiles('**/lockfiles') }}
- name: Set up Python
uses: actions/setup-python@v2
- name: Install PlatformIO
run: |
python -m pip install --upgrade pip
pip install --upgrade platformio
- name: Build EGS53 FW
run: |
pio run -e egs53
- uses: actions/upload-artifact@v3
with:
name: EGS53_FW
path: |
.pio/build/egs53/firmware.elf
.pio/build/egs53/firmware.bin
tag: "dev"
rm: true
token: ${{ secrets.GITHUB_TOKEN }}
files: |
.pio/build/unified/firmware.elf
.pio/build/unified/firmware.bin

2
.gitignore vendored
View File

@ -44,3 +44,5 @@ cmake-build-debug/
.idea/**
html/**
latex/**
__pycache__
__pycache__/*

8
Doxyfile
View File

@ -1181,6 +1181,10 @@ HTML_FILE_EXTENSION = .html
# of the possible markers and block names see the documentation.
# This tag requires that the tag GENERATE_HTML is set to YES.
GENERATE_TREEVIEW = YES
DISABLE_INDEX = NO
FULL_SIDEBAR = N
HTML_EXTRA_STYLESHEET = doxygen-awesome-css/doxygen-awesome.css
HTML_HEADER =
# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
@ -1226,7 +1230,7 @@ HTML_EXTRA_STYLESHEET =
# files will be copied as-is; there are no commands or markers available.
# This tag requires that the tag GENERATE_HTML is set to YES.
HTML_EXTRA_FILES =
HTML_EXTRA_FILES = doxygen-awesome-darkmode-toggle.js
# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
# will adjust the colors in the style sheet and background images according to
@ -1526,7 +1530,7 @@ DISABLE_INDEX = NO
# The default value is: NO.
# This tag requires that the tag GENERATE_HTML is set to YES.
GENERATE_TREEVIEW = NO
GENERATE_TREEVIEW = YES
# When both GENERATE_TREEVIEW and DISABLE_INDEX are set to YES, then the
# FULL_SIDEBAR option determines if the side bar is limited to only the treeview

8
README.md
View File

@ -10,6 +10,14 @@ I am in no way responsible if your gearbox or car dies as a result of using this
Although this firmware is being tested actively, there are loads of unknowns which may occur rarely during operation that
are not tested yet.
## IMPORTANT
If you have just built your board, or are updating to firmware after 30/11/22, please look at [this guide](https://youtu.be/ov3pYcKIA70)!
Your TCU will be bricked until you follow it!
## NOTICE ABOUT COPIED CODE (For PRs or change requests)
This project contains code built from the ground up, or code based on black-box style reverse engineering of the behaviour of the standard EGS52 module. Submitions of code pull requests based on unethically sourced documentation or code will be immediatly deleted and closed.
## Generating the code documentation
1. Install [doxygen](https://www.doxygen.nl/manual/install.html)

BIN
__pycache__/progname.cpython-310.pyc Normal file
View File

Binary file not shown.

153
include/dtcs.h
View File

@ -1,153 +0,0 @@
#ifndef DTCS_H_
#define DTCS_H_
#include <stdint.h>
/**
* DTCs (Diagnostic trouble codes) for stock EGS52.
*
* Not all DTCs will be utilized by ultimate-nag52
*
* Diagversion 51
*/
enum class DtcCode {
/**
* TCM internal errors
*/
P2000 = 0x2000, // Control unit watchdog error
P2004 = 0x2004, // Control unit clock error
P2005 = 0x2005, // Control unit RAM error
P2008 = 0x2008, // Control unit ROM error
P200A = 0x200A, // EEPROM non functional
P200C = 0x200C, // Control flow error (Kernel)
P2010 = 0x2010, // Uncoded EGS module (EEPROM blank - no SCN coding present)
P2011 = 0x2011, // Invalid variant identifier
P2012 = 0x2012, // SCN Checksum invalid
/**
* Gearbox solenoid errors
*/
P2100 = 0x2100, // Y3 solenoid error
P2101 = 0x2101, // Y3 solenoid short circuit
P2102 = 0x2102, // Y4 solenoid error
P2103 = 0x2103, // Y4 solenoid short circuit
P2104 = 0x2104, // Y5 solenoid error
P2105 = 0x2105, // Y5 solenoid short circuit
P2106 = 0x2106, // TCC solenoid error
P2107 = 0x2107, // MPC solenoid error
P2108 = 0x2108, // SPC solenoid error
P2109 = 0x2109, // R-P Lock solenoid error
P210A = 0x210A, // Starter lock relay error
/**
* Gearbox sensor errors
*/
P2200 = 0x2200, // N2 speed sensor no signal
P2203 = 0x2203, // N3 speed sensor no signal
P2206 = 0x2206, // Downforce speed sensor no signal
P2207 = 0x2207, // Downforce speed sensor consistency error
P220A = 0x220A, // N2 and N3 speed sensors disagree
P220B = 0x220B, // N2 or N3 speed sensors overspeed
P2210 = 0x2210, // Selector lever SCN coding invalid
P2211 = 0x2211, // Selector lever position implausible
P2220 = 0x2220, // ATF Temp sensor / Starter lockout contact short circuit
P2221 = 0x2221, // ATF Temp sensor / Starter lockout contact implausible
P2222 = 0x2222, // ATF Temp sensor / Starter lockout contact inconsistent
P2226 = 0x2226, // Gearbox overheated
/**
* CAN errors - module communication
*
* ECU Names
* * BS - ABS and ESP module
* * MS - Engine ECU
* * EWM - Shift selector lever
* * KOMBI - Instrument cluster
* * AAC - Climate control
* * VG - Transfer case
* * EZS - Ignition switch and CANB<->CANC gateway
* * MRM - Steering wheel and steering wheel columns/buttons
*/
P2300 = 0x2300, // CAN Controller error
P2310 = 0x2310, // CAN communication with BS disturbed
P2311 = 0x2311, // CAN communication with MS disturbed (short term)
P2312 = 0x2312, // CAN communication with MS disturbed (long term)
P2313 = 0x2313, // CAN communication with EWM disturbed
P2314 = 0x2314, // CAN communication with EZS disturbed
P2315 = 0x2315, // CAN communication with KOMBI disturbed
P2316 = 0x2316, // CAN communication with AAC disturbed
P2317 = 0x2317, // CAN communication with VG disturbed
P2322 = 0x2322, // CAN: Variant data from MS missing
P2330 = 0x2330, // CAN message length from BS inconsistent
P2331 = 0x2331, // CAN message length from MS inconsistent (short term)
P2332 = 0x2332, // CAN message length from MS inconsistent (long term)
P2333 = 0x2333, // CAN message length from EWM inconsistent
P2334 = 0x2334, // CAN message length from EZS inconsistent
P2335 = 0x2335, // CAN message length from KOMBI inconsistent
P2336 = 0x2336, // CAN message length from AAC inconsistent
P2337 = 0x2337, // CAN message length from VG inconsistent
P233B = 0x233B, // CAN message length for variant type from MS inconsistent
/**
* CAN errors - missing or invalid data
*/
P2400 = 0x2400, // CAN Wheel speed RR (From BS) not avaliable
P2401 = 0x2401, // CAN Wheel speed RL (From BS) not avaliable
P2402 = 0x2402, // CAN Wheel speed FR (From BS) not avaliable
P2403 = 0x2403, // CAN Wheel speed FL (From BS) not avaliable
P2404 = 0x2404, // CAN Brake light switch (From BS) not avaliable
P2405 = 0x2405, // CAN Accelerator pedal position (From MS) not avaliable
P2406 = 0x2406, // CAN engine static torque (From MS) not avaliable
P2407 = 0x2407, // CAN default torque (From BS) not avaliable
P2408 = 0x2408, // CAN engine minimal torque (From MS) not avaliable
P2409 = 0x2409, // CAN engine maximum torque (From MS) not avaliable
P240A = 0x240A, // CAN engine RPM (From MS) not avaliable
P240B = 0x240B, // CAN engine coolant temperature (From MS) not avaliable
P240C = 0x240C, // CAN Selector lever position (From EWM) not avaliable
P240D = 0x240D, // CAN Transfer case position (From VG) not avaliable
P2415 = 0x2415, // CAN Steering wheel paddle positions (From MRM) not avaliable
P2418 = 0x2418, // CAN Steering wheel control element (From MRM) not avaliable
P2430 = 0x2430, // CAN Multiple wheel speeds (from BS) not avaliable
P2450 = 0x2450, // CAN Variant ID (From MS) is invalid
P2451 = 0x2451, // Variant coding - EGS and MS mismatch!
/**
* Gearbox related errors (Gear ratios)
*/
P2500 = 0x2500, // Inadmissible gear ratio
P2501 = 0x2501, // Engine overspeed
P2502 = 0x2502, // Gear implausible or transmission is slipping
P2503 = 0x2503, // Gear comparison implausible multiple times!
P2507 = 0x2507, // Overspeed N2 RPM sensor
P2508 = 0x2508, // Overspeed N3 RPM sensor
P2510 = 0x2510, // Torque converter uncommanded lockup!
P2511 = 0x2511, // TCC solenoid - Excessive power consumption
P2512 = 0x2512, // Torque converter control not possible
P2520 = 0x2520, // Gear protection (multiple times) was not received
P2560 = 0x2560, // Gear 1 implausible or transmission slipping
P2561 = 0x2561, // Gear 2 implausible or transmission slipping
P2562 = 0x2562, // Gear 3 implausible or transmission slipping
P2563 = 0x2563, // Gear 4 implausible or transmission slipping
P2564 = 0x2564, // Gear 5 implausible or transmission slipping
/**
* Power supply related errors
*/
P2600 = 0x2600, // Undervoltage to entire module
P2601 = 0x2601, // Overvoltage to entire power supply
P2602 = 0x2602, // Supply voltage for module outside tolerance
P2603 = 0x2603, // Supply voltage for sensors outside tolerance
P260E = 0x260E, // Supply voltage for sensors undervoltage
P260F = 0x260F, // Supply voltage for sensors overvoltage
// ------------------------------------------------------------------------------------- //
// Codes Below are ultimate-nag52 specific and are not recognised by Daimler's software //
// ------------------------------------------------------------------------------------- //
// 404 - Gearbox not found
P0404 = 0x0404, // Where gearbox? (No solenoid control or sensor inputs)
};
#endif

6
include/gearbox_config.h
View File

@ -1,5 +1,5 @@
#ifndef __GEARBOX_CONFIG_H_
#define __GEARBOX_CONFIG_H_
#ifndef GEARBOX_CONFIG_H
#define GEARBOX_CONFIG_H
#ifndef RED_BCB
#define BOARD_V2 // Comment this line for the red beta boards!!!
@ -28,4 +28,4 @@
// The old EGS51, EGS52 and EGS53 MODE FLAGS ARE NOW MOVED TO PLATFORMIO.INI!
#endif // __GEARBOX_CONFIG_H_
#endif // GEARBOX_CONFIG_H

18
include/macros.h
View File

@ -1,18 +0,0 @@
#include "esp_log.h"
#define LOG_TAG "MISC"
#define CHECK_ESP_FUNC(x, msg, ...) \
res = x; \
if (res != ESP_OK) { \
ESP_LOG_LEVEL(ESP_LOG_ERROR, LOG_TAG, msg, ##__VA_ARGS__); \
return false; \
} \
#define CLAMP(value, min, max) \
if (value < min) { \
value = min; \
} else if (value >= max) { \
value = max-1; \
}

78
include/pins.h
View File

@ -1,78 +0,0 @@
#ifndef __PINS_H_
#define __PINS_H_
#include "gearbox_config.h"
/**
* Pin configuration
*
* Current board. V1.0
*/
#ifdef BOARD_V2
#define PIN_CAN_TX gpio_num_t::GPIO_NUM_5 // CAN TWAI Tx
#define PIN_CAN_RX gpio_num_t::GPIO_NUM_18 // CAN TWAI Rx
#define PIN_SPKR gpio_num_t::GPIO_NUM_4 // Piezo speaker
#define PIN_VBATT gpio_num_t::GPIO_NUM_25 // Battery voltage feedback
#define PIN_ATF gpio_num_t::GPIO_NUM_27 // ATF temp sensor and lockout
#define PIN_N3 gpio_num_t::GPIO_NUM_14 // N3 speed sensor
#define PIN_N2 gpio_num_t::GPIO_NUM_26 // N2 speed sensor
#define PIN_Y3_SENSE gpio_num_t::GPIO_NUM_36 // Y3 (1-2/4-5) shift solenoid (Current feedback)
#define PIN_Y3_PWM gpio_num_t::GPIO_NUM_23 // Y3 (1-2/4-5) shift solenoid (PWM output)
#define PIN_Y4_SENSE gpio_num_t::GPIO_NUM_39 // Y4 (3-4) shift solenoid (Current feedback)
#define PIN_Y4_PWM gpio_num_t::GPIO_NUM_22 // Y4 (3-4) shift solenoid (PWM output)
#define PIN_Y5_SENSE gpio_num_t::GPIO_NUM_35 // Y5 (2-3) shift solenoid (Current feedback)
#define PIN_Y5_PWM gpio_num_t::GPIO_NUM_19 // Y5 (2-3) shift solenoid (PWM output)
#define PIN_MPC_SENSE gpio_num_t::GPIO_NUM_34 // Modulating pressure solenoid (Current feedback)
#define PIN_MPC_PWM gpio_num_t::GPIO_NUM_21 // Modulating pressure solenoid (PWM output)
#define PIN_SPC_SENSE gpio_num_t::GPIO_NUM_32 // Shift pressure solenoid (Current feedback)
#define PIN_SPC_PWM gpio_num_t::GPIO_NUM_12 // Shift pressure solenoid (PWM output)
#define PIN_TCC_SENSE gpio_num_t::GPIO_NUM_33 // Torque converter solenoid(Current feedback)
#define PIN_TCC_PWM gpio_num_t::GPIO_NUM_13 // Torque converter solenoid (PWM output)
#define PIN_I2C_SDA gpio_num_t::GPIO_NUM_15 // I2C clock
#define PIN_I2C_SCL gpio_num_t::GPIO_NUM_2 // I2C data
#else // Legacy red board
#define PIN_CAN_TX gpio_num_t::GPIO_NUM_5 // CAN TWAI Tx
#define PIN_CAN_RX gpio_num_t::GPIO_NUM_4 // CAN TWAI Rx
#define PIN_SPKR gpio_num_t::GPIO_NUM_15 // Piezo speaker
#define PIN_5V_EN gpio_num_t::GPIO_NUM_2 // 5V enable circuit for sensors
#define PIN_VBATT gpio_num_t::GPIO_NUM_25 // Battery voltage feedback
#define PIN_ATF gpio_num_t::GPIO_NUM_26 // ATF temp sensor and lockout
#define PIN_N3 gpio_num_t::GPIO_NUM_27 // N3 speed sensor
#define PIN_N2 gpio_num_t::GPIO_NUM_14 // N2 speed sensor
#define PIN_Y3_SENSE gpio_num_t::GPIO_NUM_36 // Y3 (1-2/4-5) shift solenoid (Current feedback)
#define PIN_Y3_PWM gpio_num_t::GPIO_NUM_23 // Y3 (1-2/4-5) shift solenoid (PWM output)
#define PIN_Y4_SENSE gpio_num_t::GPIO_NUM_39 // Y4 (3-4) shift solenoid (Current feedback)
#define PIN_Y4_PWM gpio_num_t::GPIO_NUM_22 // Y4 (3-4) shift solenoid (PWM output)
#define PIN_Y5_SENSE gpio_num_t::GPIO_NUM_35 // Y5 (2-3) shift solenoid (Current feedback)
#define PIN_Y5_PWM gpio_num_t::GPIO_NUM_19 // Y5 (2-3) shift solenoid (PWM output)
#define PIN_MPC_SENSE gpio_num_t::GPIO_NUM_34 // Modulating pressure solenoid (Current feedback)
#define PIN_MPC_PWM gpio_num_t::GPIO_NUM_21 // Modulating pressure solenoid (PWM output)
#define PIN_SPC_SENSE gpio_num_t::GPIO_NUM_32 // Shift pressure solenoid (Current feedback)
#define PIN_SPC_PWM gpio_num_t::GPIO_NUM_12 // Shift pressure solenoid (PWM output)
#define PIN_TCC_SENSE gpio_num_t::GPIO_NUM_33 // Torque converter solenoid(Current feedback)
#define PIN_TCC_PWM gpio_num_t::GPIO_NUM_13 // Torque converter solenoid (PWM output)
#endif
#endif // __PINS_H_

10601
include/scn.h
View File

File diff suppressed because it is too large Load Diff

1837
include/scn_definition.h
View File

File diff suppressed because it is too large Load Diff

1450
include/scn_internal.h
View File

File diff suppressed because it is too large Load Diff

30
lib/convert.py
View File

@ -59,7 +59,7 @@ class Signal:
elif self.is_bool: # Boolean data type
return "bool"
elif self.is_enum: # Enum data type
return "{}_{}".format(frame_name, self.name)
return "{}_{}{}".format(frame_name, self.name, global_guard)
elif self.is_char:
return "char"
else:
@ -146,10 +146,6 @@ class ECU:
def make_output_str(self) -> str:
self.filter_frames()
# Create output header string
guard = ""
if output_guard:
guard = "#ifdef {0}".format(global_guard)
tmp = """
/**
* AUTOGENERATED BY convert.py
@ -160,16 +156,14 @@ class ECU:
* CAN Defintiion for ECU '{0}'
*/
{1}
#ifndef __ECU_{0}_H_
#define __ECU_{0}_H_
#include <stdint.h>
""".format(self.name, guard)
""".format(self.name)
for f in self.frames:
tmp += "\n#define {}_CAN_ID 0x{:04X}".format(f.name.strip().removesuffix("h"), f.can_id)
tmp += "\n#define {}{}_CAN_ID 0x{:04X}".format(f.name.strip().removesuffix("h"), global_guard, f.can_id)
tmp += "\n\n"
# Now iterate over all enums of the ECU
@ -177,7 +171,7 @@ class ECU:
for s in x.signals:
if s.is_enum:
tmp += "/** {} */".format(s.desc)
tmp += "\nenum class {}_{} {{".format(x.name, s.name)
tmp += "\nenum class {}_{}{} {{".format(x.name, s.name, global_guard)
for e in s.enum_table:
tmp += "\n\t{} = {}, // {}".format(e.name, e.raw, e.desc)
tmp += "\n};\n\n"
@ -188,14 +182,14 @@ class ECU:
tmp += "\n\ntypedef union {" # Struct name
tmp += "\n\tuint64_t raw;" # Store raw value
tmp += "\n\tuint8_t bytes[8];"
tmp += "\n\n\t/** Gets CAN ID of {} */".format(struct_name)
tmp += "\n\tuint32_t get_canid(){{ return {}_CAN_ID; }}".format(struct_name)
tmp += "\n\n\t/** Gets CAN ID of {}{} */".format(struct_name, global_guard)
tmp += "\n\tuint32_t get_canid(){{ return {}{}_CAN_ID; }}".format(struct_name, global_guard)
# Setters and getters!
for s in x.signals:
tmp += s.get_setter_and_getter(x.name)
tmp += "\n}} {};\n\n".format(struct_name)
tmp += "\n}} {}{};\n\n".format(struct_name, global_guard)
# Now magic to create the class ;)
@ -217,10 +211,10 @@ class ECU:
switch(can_id) {"""
for idx, frame in enumerate(self.frames):
tmp += """
case {0}_CAN_ID:
case {0}{2}_CAN_ID:
LAST_FRAME_TIMES[{1}] = timestamp_now;
FRAME_DATA[{1}] = value;
return true;""".format(frame.name.strip().removesuffix("h"), idx)
return true;""".format(frame.name.strip().removesuffix("h"), idx, global_guard)
tmp += """
default:
return false;
@ -237,7 +231,7 @@ class ECU:
*
* If the function returns true, then the pointer to 'dest' has been updated with the new CAN data
*/
bool get_{0}(uint64_t now, uint64_t max_expire_time, {0}* dest) const {{
bool get_{0}(uint64_t now, uint64_t max_expire_time, {0}{2}* dest) const {{
if (LAST_FRAME_TIMES[{1}] == 0 || dest == nullptr) {{ // CAN Frame has not been seen on bus yet / NULL pointer
return false;
}} else if (now > LAST_FRAME_TIMES[{1}] && now - LAST_FRAME_TIMES[{1}] > max_expire_time) {{ // CAN Frame has not refreshed in valid interval
@ -247,7 +241,7 @@ class ECU:
return true;
}}
}}
""".format(frame.name.strip().removesuffix("h"), idx)
""".format(frame.name.strip().removesuffix("h"), idx, global_guard)
tmp += "\n\tprivate:"
tmp += "\n\t\tuint64_t FRAME_DATA[{0}];".format(num_frames)
tmp += "\n\t\tuint64_t LAST_FRAME_TIMES[{0}];".format(num_frames)
@ -255,8 +249,6 @@ class ECU:
# Lastly append endif guard
tmp += "\n#endif // __ECU_{}_H_".format(self.name)
if output_guard:
tmp += "\n\n#endif // {}".format(global_guard)
return tmp

165
lib/core/src/tcm_maths.cpp
View File

@ -1,165 +0,0 @@
#include "tcm_maths.h"
#include "string.h"
#include "esp_heap_caps.h"
float scale_number(int16_t raw, int16_t new_min, int16_t new_max, int16_t raw_min, int16_t raw_max) {
raw = MAX(raw_min, MIN(raw, raw_max));
return ((float)((new_max-new_min) * (raw - raw_min)) / ((float)(raw_max-raw_min))) + (float)new_min;
}
TcmMap::TcmMap(uint16_t X_Size, uint16_t Y_size, const int16_t* x_ids, const int16_t* y_ids) {
this->x_size = X_Size;
this->y_size = Y_size;
this->alloc_ok = false;
this->x_headers = (int16_t*)heap_caps_malloc(x_size*sizeof(int16_t), MALLOC_CAP_SPIRAM);
this->y_headers = (int16_t*)heap_caps_malloc(y_size*sizeof(int16_t), MALLOC_CAP_SPIRAM);
this->data = (int16_t*)heap_caps_malloc(y_size*x_size*sizeof(int16_t), MALLOC_CAP_SPIRAM);
if (this->x_headers == nullptr || this->y_headers == nullptr || this->data == nullptr) {
// Allocation failed!
return;
}
memcpy(this->x_headers, x_ids, sizeof(int16_t)*x_size);
memcpy(this->y_headers, y_ids, sizeof(int16_t)*y_size);
memset(this->data, 0, y_size*x_size*sizeof(int16_t));
this->alloc_ok = true;
}
bool TcmMap::add_data(int16_t* map, int size) {
if (map == nullptr) {
return false;
}
if (size != this->x_size*this->y_size) {
return false;
}
memcpy(this->data, map, size*sizeof(int16_t));
return true;
}
bool TcmMap::allocate_ok() {
return this->alloc_ok;
}
int16_t* TcmMap::get_row(uint16_t id) {
if (id >= this->y_size) {
return nullptr; // Row out of range
}
return &this->data[id*this->x_size];
}
float TcmMap::get_value(float x_value, float y_value) {
uint16_t x_idx_min = 0;
uint16_t x_idx_max = 0;
uint16_t y_idx_min = 0;
uint16_t y_idx_max = 0;
if (this->x_headers[0] >= x_value) {
x_idx_min = 0;
x_idx_max = 0;
} else if (this->x_headers[this->x_size-1] <= x_value) {
x_idx_min = this->x_size-1;
x_idx_max = this->x_size-1;
} else {
// Lookup X value
for (uint16_t i = 0; i < this->x_size-1; i++) {
if (x_value == this->x_headers[i]) {
x_idx_min = i;
x_idx_max = i;
break;
}
if (this->x_headers[i] <= x_value && this->x_headers[i+1] > x_value) {
x_idx_min = i;
x_idx_max = i+1;
break;
}
}
}
// Lookup Y value
if (this->y_headers[0] >= y_value) { // Check 0th value and below
y_idx_min = 0;
y_idx_max = 0;
} else if (this->y_headers[this->y_size-1] <= y_value) { // Check nth value and above
y_idx_min = this->y_size-1;
y_idx_max = this->y_size-1;
} else {
// Lookup X value
for (uint16_t i = 0; i < this->y_size-1; i++) {
if (y_value == this->y_headers[i]) {
y_idx_min = i;
y_idx_max = i;
break;
}
if (this->y_headers[i] <= y_value && this->y_headers[i+1] > y_value) {
y_idx_min = i;
y_idx_max = i+1;
break;
}
}
}
if (x_idx_max == x_idx_min && y_idx_max == y_idx_min) {
return data[(y_idx_min*this->x_size)+x_idx_min];
} else {
float dx;
float dy;
float rx_min;
float rx_max;
float ry_min;
float ry_max;
if (x_idx_max == x_idx_min) {
dx = 1.0;
rx_min = 0.5;
rx_max = 0.5;
} else {
dx = this->x_headers[x_idx_max] - this->x_headers[x_idx_min];
rx_min = 1.0 - ((x_value-this->x_headers[x_idx_min])/dx);
rx_max = 1.0-rx_min;
}
if (y_idx_max == y_idx_min) {
dy = 1.0;
ry_min = 0.5;
ry_max = 0.5;
} else {
dy = this->y_headers[y_idx_max] - this->y_headers[y_idx_min];
ry_min = 1.0 - ((y_value-this->y_headers[y_idx_min])/dy);
ry_max = 1.0-ry_min;
}
// Now to combine the data
// First do linear interpolation on X axis for both Y elements
// For min y
float x_ymin = (rx_min*data[(y_idx_min*this->x_size)+x_idx_min]) + (rx_max*data[(y_idx_min*this->x_size)+x_idx_max]);
// For max y
float x_ymax = (rx_min*data[(y_idx_max*this->x_size)+x_idx_min]) + (rx_max*data[(y_idx_max*this->x_size)+x_idx_max]);
return (x_ymin*ry_min) + (x_ymax*ry_max);
}
return 0;
}

41
lib/core/src/tcm_maths.h
View File

@ -1,41 +0,0 @@
#ifndef __TCM_MATHS_H__
#define __TCM_MATHS_H__
#include <stdint.h>
// Core maths and calculation stuff this the TCM uses
#ifndef MAX
#define MAX(a, b) (((a) > (b)) ? (a) : (b))
#endif
#ifndef MIN
#define MIN(a, b) (((a) < (b)) ? (a) : (b))
#endif
float scale_number(int16_t raw, int16_t new_min, int16_t new_max, int16_t raw_min, int16_t raw_max);
class TcmMap {
public:
/// Creates a 0'ed map
TcmMap(uint16_t X_Size, uint16_t Y_size, const int16_t* x_ids, const int16_t* y_ids);
/// MUST call after constructor to ensure that memory was allocated
/// OK for the map!
bool allocate_ok();
/// Adds a new row to the map
bool add_data(int16_t* map, int size);
int16_t* get_row(uint16_t id);
float get_value(float x_value, float y_value);
private:
int16_t* data;
int16_t* x_headers;
int16_t* y_headers;
uint16_t x_size;
uint16_t y_size;
bool alloc_ok;
};
#endif // __TCM_MATHS_H__

6
lib/core/tcu_maths.cpp Normal file
View File

@ -0,0 +1,6 @@
#include "tcu_maths.h"
float scale_number(float raw, float new_min, float new_max, float raw_min, float raw_max) {
float raw_limited = MAX(raw_min, MIN(raw, raw_max));
return (((new_max - new_min) * (raw_limited - raw_min)) / (raw_max - raw_min)) + new_min;
}

16
lib/core/tcu_maths.h Normal file
View File

@ -0,0 +1,16 @@
#ifndef TCM_MATHS_H
#define TCM_MATHS_H
// Core maths and calculation stuff this the TCM uses
#ifndef MAX
#define MAX(a, b) (((a) > (b)) ? (a) : (b))
#endif
#ifndef MIN
#define MIN(a, b) (((a) < (b)) ? (a) : (b))
#endif
float scale_number(float raw, float new_min, float new_max, float raw_min, float raw_max);
#endif // TCM_MATHS_H

134
lib/core/tcumap.cpp Normal file
View File

@ -0,0 +1,134 @@
#include "tcumap.h"
#include "string.h"
#include "esp_heap_caps.h"
TcuMap::TcuMap(uint16_t X_Size, uint16_t Y_size, const int16_t* x_ids, const int16_t* y_ids) {
this->x_size = X_Size;
this->y_size = Y_size;
this->alloc_ok = false;
this->x_headers = static_cast<int16_t*>(heap_caps_malloc(x_size * sizeof(int16_t), MALLOC_CAP_SPIRAM));
this->y_headers = static_cast<int16_t*>(heap_caps_malloc(y_size * sizeof(int16_t), MALLOC_CAP_SPIRAM));
this->data = static_cast<int16_t*>(heap_caps_malloc(y_size * x_size * sizeof(int16_t), MALLOC_CAP_SPIRAM));
if (!((nullptr == this->x_headers) || (nullptr == this->y_headers) || (nullptr == this->data))) {
// Allocation succeeded!
(void)memcpy(this->x_headers, x_ids, (sizeof(int16_t) * x_size));
(void)memcpy(this->y_headers, y_ids, (sizeof(int16_t) * y_size));
(void)memset(this->data, 0, y_size * (x_size * sizeof(int16_t)));
this->alloc_ok = true;
}
}
bool TcuMap::add_data(const int16_t* map, const uint16_t size) {
bool result = false;
if (nullptr != map)
{
if (((uint16_t)size) == ((this->x_size) * (this->y_size)))
{
(void)memcpy(this->data, map, size * sizeof(int16_t));
result = true;
}
}
return result;
}
bool TcuMap::allocate_ok(void) const {
return this->alloc_ok;
}
inline void TcuMap::set_indices(const float value, uint16_t* idx_min, uint16_t* idx_max, const int16_t* headers, const uint16_t size) {
// Set minimum index to the first element of the field.
*idx_min = 0u;
// Set maximum index to the last element of the field.
*idx_max = size - 1u;
// Check, if search value is smaller than smallest element of the field.
if (value > (float)headers[0]) {
if (value < (float)headers[*idx_max]) {
// Search value is in between the limits of the smallest and the biggest element of the field.
do {
// Calculate the middle of the remaining list. If the size is odd, it is rounded down.
uint16_t idx_mid = (*idx_min + *idx_max) >> 1;
if (value < (float)headers[idx_mid]) {
// Search value is smaller than the element in the middle of the remaining list.
*idx_max = idx_mid;
}
else if (value > (float)headers[idx_mid]) {
// Search value is bigger than the element in the middle of the remaining list.
*idx_min = idx_mid;
}
else {
// Search value is also an element of the field.
*idx_min = idx_mid;
*idx_max = idx_mid;
}
// Reduce the remaining search area until it is narrowed down to two consecutive elements.
} while (1u < ((*idx_max) - (*idx_min)));
}
else {
// Search value is as big as or bigger then the biggest element in the field.
*idx_min = *idx_max;
}
}
else {
// Search value is as small as or smaller than smallest element of the field.
*idx_max = *idx_min;
}
}
inline float TcuMap::interpolate(const float f_1, const float f_2, const int16_t x_1, const int16_t x_2, const float x) {
// cast values from signed integer values to floating values in order to avoid casting the same value twice
const float x_1_f = (float)x_1;
const float x_2_f = (float)x_2;
// See https://en.wikipedia.org/wiki/Linear_interpolation for details. Return f_1, if x_1 and x_2 are identical.
return (x_1 != x_2) ? f_1 + ((f_2 - f_1) / (x_2_f - x_1_f)) * (x - x_1_f) : f_1;
}
float TcuMap::get_value(float x_value, float y_value) {
uint16_t x_idx_min;
uint16_t x_idx_max;
uint16_t y_idx_min;
uint16_t y_idx_max;
// part 1a - identification of the indices for x-value
set_indices(x_value, &x_idx_min, &x_idx_max, this->x_headers, this->x_size);
// part 1b - identification of the indices for y-value
set_indices(y_value, &y_idx_min, &y_idx_max, this->y_headers, this->y_size);
// part 2: do the interpolation
int16_t x1 = this->x_headers[x_idx_min];
int16_t x2 = this->x_headers[x_idx_max];
int16_t y1 = this->y_headers[y_idx_min];
int16_t y2 = this->y_headers[y_idx_max];
// some precalculations for making the code more readable, although somewhat inefficient
float f_11 = (float)data[(y_idx_min * this->x_size) + x_idx_min];
float f_12 = (float)data[(y_idx_min * this->x_size) + x_idx_max];
float f_21 = (float)data[(y_idx_max * this->x_size) + x_idx_min];
float f_22 = (float)data[(y_idx_max * this->x_size) + x_idx_max];
// interpolation on x-axis for smaller y-index
float f_11f_12_interpolated = interpolate(f_11, f_12, x1, x2, x_value);
// interpolation on x-axis for greater y-index
float f_21f_22_interpolated = interpolate(f_21, f_22, x1, x2, x_value);
// bilinear interpolation, not always efficient, but with more or less constant runtime
// also see https://en.wikipedia.org/wiki/Bilinear_interpolation, https://helloacm.com/cc-function-to-compute-the-bilinear-interpolation/ for mathematical background
return interpolate(f_11f_12_interpolated, f_21f_22_interpolated, y1, y2, y_value);
}
int16_t* TcuMap::get_current_data(void) {
return this->data;
}
void TcuMap::get_x_headers(uint16_t *size, int16_t **headers){
*size = this->x_size;
*headers = this->x_headers;
}
void TcuMap::get_y_headers(uint16_t *size, int16_t **headers){
*size = this->y_size;
*headers = this->y_headers;
}

52
lib/core/tcumap.h Normal file
View File

@ -0,0 +1,52 @@
#ifndef TCUMAP_H
#define TCUMAP_H
#include <stdint.h>
class TcuMap {
public:
/// Creates a 0'ed map
TcuMap(uint16_t X_Size, uint16_t Y_size, const int16_t* x_ids, const int16_t* y_ids);
/// MUST call after constructor to ensure that memory was allocated
/// OK for the map!
bool allocate_ok(void) const;
/// Adds a new row to the map
bool add_data(const int16_t* map, const uint16_t size);
/**
* @brief Search for the value at a given x- and y- value of the underlying 3D-map
*
* @return The interpolated value of the 3D-map.
*/
float get_value(float x_value, float y_value);
/**
* @brief Returns pointer to the current stored map data
*
* @return int16_t*
*/
int16_t* get_current_data(void);
void get_x_headers(uint16_t *size, int16_t **headers);
void get_y_headers(uint16_t *size, int16_t **headers);
private:
int16_t* data;
int16_t* x_headers;
int16_t* y_headers;
uint16_t x_size;
uint16_t y_size;
bool alloc_ok;
/**
* @brief Sets the indices idx_min and idx_max in between the value is found in headers.
*/
inline static void set_indices(const float value, uint16_t* idx_min, uint16_t* idx_max, const int16_t* headers, const uint16_t size);
/**
* @brief Calulates interpolated value between given values of function f_1 and f_2 for given value x.
*/
inline static float interpolate(const float f_1, const float f_2, const int16_t x_1, const int16_t x_2, const float x);
};
#endif // TCUMAP_H

0
lib/core/src/core_structures.h → lib/dtcs/dtc_manager.cpp
View File

41
lib/dtcs/dtc_manager.h Normal file
View File

@ -0,0 +1,41 @@
#ifndef __DTC_MANAGER_H_
#define __DTC_MANAGER_H_
#include <stdint.h>
#include "dtcs.h"
struct EgsSnapShotData {
};
/**
* @brief DTC Stored state on the TCU
*
* ## About the `counter`
*
* When the TCU start up, the counter is set to 0. Meaning the error has not been checked. Upon every successful conformation, or error, the counter
* is either decreased or increased. When reaching its negative limit (-128), the DTC is marked as OK. If the counter reaches 127, then the failure
* is marked as current, the `env_data_first_occurrence` is written to, and the `drive_cycle_count` is increased by 1. If the `drive_cycle_count` is
* now more than 1, indicating this is the 2nd or more time this fault has been triggered, then `env_data_most_recent_occurrence` is also written to,
* the TCU is now considered stored and current. If on the next startup, the counter hits -127 (Error is OK now), then the DTC is just marked as `stored`
*
*/
struct DtcState {
/// @brief DTC Code
DtcCode code;
/// @brief DTC Counter for the current drive. This allows for debouncing. -128 = Passed (OK), +127 = Failed (Error), 0 = Not ready.
int8_t counter;
/// @brief Drive cycle count of error
uint8_t drive_cycle_count;
/// @brief Snapshot data of most recent occurrence
EgsSnapShotData* env_data_most_recent_occurrence;
/// @brief Snapshot data of the first occurrence
EgsSnapShotData* env_data_first_occurrence;
};
class DtcManager {
public:
DtcManager();
};
#endif

223
lib/dtcs/dtcs.h Normal file
View File

@ -0,0 +1,223 @@
#ifndef DTCS_H_
#define DTCS_H_
#include <stdint.h>
/**
* DTCs (Diagnostic trouble codes) for stock EGS52.
*
* Not all DTCs will be utilized by ultimate-nag52
*
* Diagversion 51
*
* ## DTC Ranges
* * P2000 - P2012 - TCU Internal errors
* * P2100 - P210A - Gearbox solenoid errors
* * P2200 - P2226 - Gearbox sensor errors
* * P2300 - P233B - CAN errors (module communication)
* * P2400 - P2451 - CAN errors (invalid or missing data)
* * P2500 - P2564 - Gear errors
* * P2600 - P260F - Power supply errors
* * P2700 - P2900 - Ultimate NAG52 specific errors (Will not be recognised with Daimler's own tools)
*/
enum class DtcCode {
/// Control unit watchdog error
P2000 = 0x2000,
/// Control unit clock error
P2004 = 0x2004,
/// Control unit RAM error
P2005 = 0x2005,
/// Control unit ROM error
P2008 = 0x2008,
/// EEPROM non functional
P200A = 0x200A,
/// Control flow error (Kernel)
P200C = 0x200C,
/// Uncoded EGS module (EEPROM blank - no SCN coding present)
P2010 = 0x2010,
/// Invalid variant identifier
P2011 = 0x2011,
/// SCN Checksum invalid
P2012 = 0x2012,
/// Y3 solenoid error
P2100 = 0x2100,
/// Y3 solenoid short circuit
P2101 = 0x2101,
/// Y4 solenoid error
P2102 = 0x2102,
/// Y4 solenoid short circuit
P2103 = 0x2103,
/// Y5 solenoid error
P2104 = 0x2104,
/// Y5 solenoid short circuit
P2105 = 0x2105,
/// TCC solenoid error
P2106 = 0x2106,
/// MPC solenoid error
P2107 = 0x2107,
/// SPC solenoid error
P2108 = 0x2108,
/// R-P Lock solenoid error
P2109 = 0x2109,
/// Starter lock relay error
P210A = 0x210A,
/// N2 speed sensor no signal
P2200 = 0x2200,
/// N3 speed sensor no signal
P2203 = 0x2203,
/// Output speed sensor no signal
P2206 = 0x2206,
/// Output speed sensor consistency error
P2207 = 0x2207,
/// N2 and N3 speed sensors disagree
P220A = 0x220A,
/// N2 or N3 speed sensors overspeed
P220B = 0x220B,
/// Selector lever SCN coding invalid
P2210 = 0x2210,
/// Selector lever position implausible
P2211 = 0x2211,
/// ATF Temp sensor / Starter lockout contact short circuit
P2220 = 0x2220,
/// ATF Temp sensor / Starter lockout contact implausible
P2221 = 0x2221,
/// ATF Temp sensor / Starter lockout contact inconsistent
P2222 = 0x2222,
/// Gearbox overheated
P2226 = 0x2226,
/// CAN Controller error
P2300 = 0x2300,
/// CAN communication with BS disturbed
P2310 = 0x2310,
/// CAN communication with MS disturbed (short term)
P2311 = 0x2311,
/// CAN communication with MS disturbed (long term)
P2312 = 0x2312,
/// CAN communication with EWM disturbed
P2313 = 0x2313,
/// CAN communication with EZS disturbed
P2314 = 0x2314,
/// CAN communication with KOMBI disturbed
P2315 = 0x2315,
/// CAN communication with AAC disturbed
P2316 = 0x2316,
/// CAN communication with VG disturbed
P2317 = 0x2317,
/// CAN Variant data from MS missing
P2322 = 0x2322,
/// CAN message length from BS inconsistent
P2330 = 0x2330,
/// CAN message length from MS inconsistent (short term)
P2331 = 0x2331,
/// CAN message length from MS inconsistent (long term)
P2332 = 0x2332,
/// CAN message length from EWM inconsistent
P2333 = 0x2333,
/// CAN message length from EZS inconsistent
P2334 = 0x2334,
/// CAN message length from KOMBI inconsistent
P2335 = 0x2335,
/// CAN message length from AAC inconsistent
P2336 = 0x2336,
/// CAN message length from VG inconsistent
P2337 = 0x2337,
/// CAN message length for variant type from MS inconsistent
P233B = 0x233B,
/// CAN Wheel speed RR (From BS) not available
P2400 = 0x2400,
/// CAN Wheel speed RL (From BS) not available
P2401 = 0x2401,
/// CAN Wheel speed FR (From BS) not available
P2402 = 0x2402,
/// CAN Wheel speed FL (From BS) not available
P2403 = 0x2403,
/// CAN Brake light switch (From BS) not available
P2404 = 0x2404,
/// CAN Accelerator pedal position (From MS) not available
P2405 = 0x2405,
/// CAN engine static torque (From MS) not available
P2406 = 0x2406,
/// CAN default torque (From BS) not available
P2407 = 0x2407,
/// CAN engine minimal torque (From MS) not available
P2408 = 0x2408,
/// CAN engine maximum torque (From MS) not available
P2409 = 0x2409,
/// CAN engine RPM (From MS) not available
P240A = 0x240A,
/// CAN engine coolant temperature (From MS) not available
P240B = 0x240B,
/// CAN Selector lever position (From EWM) not available
P240C = 0x240C,
/// CAN Transfer case position (From VG) not available
P240D = 0x240D,
/// CAN Steering wheel paddle positions (From MRM) not available
P2415 = 0x2415,
/// CAN Steering wheel control element (From MRM) not available
P2418 = 0x2418,
/// CAN Multiple wheel speeds (from BS) not available
P2430 = 0x2430,
/// CAN Variant ID (From MS) is invalid
P2450 = 0x2450,
/// Variant coding - EGS and MS mismatch!
P2451 = 0x2451,
/// Inadmissible gear ratio
P2500 = 0x2500,
/// Engine overspeed
P2501 = 0x2501,
/// Gear implausible or transmission is slipping
P2502 = 0x2502,
/// Gear comparison implausible multiple times!
P2503 = 0x2503,
/// Overspeed N2 RPM sensor
P2507 = 0x2507,
/// Overspeed N3 RPM sensor
P2508 = 0x2508,
/// Torque converter un-commanded lockup!
P2510 = 0x2510,
/// TCC solenoid - Excessive power consumption
P2511 = 0x2511,
/// Torque converter control not possible
P2512 = 0x2512,
/// Gear protection (multiple times) was not received
P2520 = 0x2520,
/// Gear 1 implausible or transmission slipping
P2560 = 0x2560,
/// Gear 2 implausible or transmission slipping
P2561 = 0x2561,
/// Gear 3 implausible or transmission slipping
P2562 = 0x2562,
/// Gear 4 implausible or transmission slipping
P2563 = 0x2563,
/// Gear 5 implausible or transmission slipping
P2564 = 0x2564,
/// Undervoltage to entire module
P2600 = 0x2600,
/// Overvoltage to entire power supply
P2601 = 0x2601,
/// Supply voltage for module outside tolerance
P2602 = 0x2602,
/// Supply voltage for sensors outside tolerance
P2603 = 0x2603,
/// Supply voltage for sensors undervoltage
P260E = 0x260E,
/// Supply voltage for sensors overvoltage
P260F = 0x260F,
/// Memory allocation failure in PSRAM in one or more function calls
P2700 = 0x2700,
/// Crash detected (Rebooted, coredump saved)
P2701 = 0x2701,
/// Engine failing to acknowledge torque requests (Only when ESP not intervening)
P2702 = 0x2702,
P2703 = 0x2703,
};
#endif

18
lib/egs51_ecus/can_data.txt
View File

@ -240,7 +240,23 @@ ECU GS51
# 4th byte 01001010 <- ON
SIGNAL KICKDOWN, OFFSET: 25, LEN: 1, DESC: Kickdown pressed, DATA TYPE BOOL
SIGNAL FEHLER, OFFSET: 44, LEN: 4, DESC: error number or counter for calid / CVN transmission, DATA TYPE NUMBER(_MULTIPLIER_: 1.0, _OFFSET_: 0.0)
ECU EWM51
FRAME EWM_230h (0x00000230)
SIGNAL W_S, OFFSET: 0, LEN: 1, DESC: Driving program, DATA TYPE BOOL
SIGNAL FPT, OFFSET: 1, LEN: 1, DESC: Driving program button actuated, DATA TYPE BOOL
SIGNAL KD, OFFSET: 2, LEN: 1, DESC: Kickdown, DATA TYPE BOOL
SIGNAL SPERR, OFFSET: 3, LEN: 1, DESC: barrier magnet energized, DATA TYPE BOOL
SIGNAL WHC, OFFSET: 4, LEN: 4, DESC: gear selector lever position (NAG only), DATA TYPE ENUM
ENUM D, RAW: 5, DESC: selector lever in position "D"
ENUM N, RAW: 6, DESC: selector lever in position "N"
ENUM R, RAW: 7, DESC: selector lever in position "R"
ENUM P, RAW: 8, DESC: selector lever in position "P"
ENUM PLUS, RAW: 9, DESC: selector lever in position "+"
ENUM MINUS, RAW: 10, DESC: selector lever in position "-"
ENUM N_ZW_D, RAW: 11, DESC: selector lever in intermediate position "N-D"
ENUM R_ZW_N, RAW: 12, DESC: selector lever in intermediate position "R-N"
ENUM P_ZW_R, RAW: 13, DESC: selector lever in intermediate position "P-R"
ENUM SNV, RAW: 15, DESC: selector lever position unplausible
ECU EZS51
FRAME EZS_240h (0x00000240)
SIGNAL WH_UP, OFFSET: 2, LEN: 1, DESC: cruise control lever implausible, DATA TYPE BOOL

96
lib/egs51_ecus/src/ESP51.h
View File

@ -8,18 +8,16 @@
* CAN Defintiion for ECU 'ESP51'
*/
#ifdef EGS51_MODE
#ifndef __ECU_ESP51_H_
#define __ECU_ESP51_H_
#include <stdint.h>
#define BS_200_CAN_ID 0x0200
#define BS_208_CAN_ID 0x0208
#define BS_200EGS51_CAN_ID 0x0200
#define BS_208EGS51_CAN_ID 0x0208
/** brake light switch */
enum class BS_200h_BLS {
enum class BS_200h_BLSEGS51 {
BREMSE_NBET = 0, // Brake not actuated
BREMSE_BET = 1, // brake actuated
UNKNOWN = 2, // not defined
@ -27,7 +25,7 @@ enum class BS_200h_BLS {
};
/** rotary direction wheel front left */
enum class BS_200h_DRTGVL {
enum class BS_200h_DRTGVLEGS51 {
PASSIVE = 0, // No rotation detection
FWD = 1, // direction of rotation forward
REV = 2, // direction of rotation backwards
@ -35,7 +33,7 @@ enum class BS_200h_DRTGVL {
};
/** direction of rotation wheel front right */
enum class BS_200h_DRTGVR {
enum class BS_200h_DRTGVREGS51 {
PASSIVE = 0, // No rotation detection
FWD = 1, // direction of rotation forward
REV = 2, // direction of rotation backwards
@ -43,7 +41,7 @@ enum class BS_200h_DRTGVR {
};
/** Rad Left for Cruise */
enum class BS_200h_DRTGTM {
enum class BS_200h_DRTGTMEGS51 {
PASSIVE = 0, // No rotation detection
FWD = 1, // direction of rotation forward
REV = 2, // direction of rotation backwards
@ -51,7 +49,7 @@ enum class BS_200h_DRTGTM {
};
/** Gear, upper limit */
enum class BS_208h_GMAX_ESP {
enum class BS_208h_GMAX_ESPEGS51 {
PASSIVE = 0, // passive value
G1 = 1, // Gear, upper limit = 1
G2 = 2, // Gear, upper limit = 2
@ -63,7 +61,7 @@ enum class BS_208h_GMAX_ESP {
};
/** Gear, lower limit */
enum class BS_208h_GMIN_ESP {
enum class BS_208h_GMIN_ESPEGS51 {
PASSIVE = 0, // passive value
G1 = 1, // Gear, lower limit = 1
G2 = 2, // Gear, lower limit = 2
@ -75,7 +73,7 @@ enum class BS_208h_GMIN_ESP {
};
/** system condition */
enum class BS_208h_SZS {
enum class BS_208h_SZSEGS51 {
ERR = 0, // system error
NORM = 1, // normal operation
DIAG = 2, // Diagnosis
@ -83,7 +81,7 @@ enum class BS_208h_SZS {
};
/** Switching Difference ESP */
enum class BS_208h_SLV_ESP {
enum class BS_208h_SLV_ESPEGS51 {
SKL0 = 0, // Shift characteristic "0"
SKL1 = 1, // Shift characteristic "1"
SKL2 = 2, // Shift characteristic "2"
@ -98,7 +96,7 @@ enum class BS_208h_SLV_ESP {
};
/** ESP request: "N" Insert */
enum class BS_208h_ANFN {
enum class BS_208h_ANFNEGS51 {
UNKNOWN = 0, // not defined
ANF_N = 1, // requirement "neutral"
IDLE = 2, // No requirement
@ -106,7 +104,7 @@ enum class BS_208h_ANFN {
};
/** rotary direction wheel rear right */
enum class BS_208h_DRTGHR {
enum class BS_208h_DRTGHREGS51 {
PASSIVE = 0, // No rotation detection
FWD = 1, // direction of rotation forward
REV = 2, // direction of rotation backwards
@ -114,7 +112,7 @@ enum class BS_208h_DRTGHR {
};
/** rotary direction wheel rear left */
enum class BS_208h_DRTGHL {
enum class BS_208h_DRTGHLEGS51 {
PASSIVE = 0, // No rotation detection
FWD = 1, // direction of rotation forward
REV = 2, // direction of rotation backwards
@ -127,8 +125,8 @@ typedef union {
uint64_t raw;
uint8_t bytes[8];
/** Gets CAN ID of BS_200 */
uint32_t get_canid(){ return BS_200_CAN_ID; }
/** Gets CAN ID of BS_200EGS51 */
uint32_t get_canid(){ return BS_200EGS51_CAN_ID; }
/** Sets Brake defective control lamp (EBV_KL at 463/461 / NCV2) */
void set_BRE_KL(bool value){ raw = (raw & 0x7fffffffffffffff) | ((uint64_t)value & 0x1) << 63; }
@ -190,16 +188,16 @@ typedef union {
uint8_t get_BZ200h() const { return (uint8_t)(raw >> 50 & 0xf); }
/** Sets brake light switch */
void set_BLS(BS_200h_BLS value){ raw = (raw & 0xfffcffffffffffff) | ((uint64_t)value & 0x3) << 48; }
void set_BLS(BS_200h_BLSEGS51 value){ raw = (raw & 0xfffcffffffffffff) | ((uint64_t)value & 0x3) << 48; }
/** Gets brake light switch */
BS_200h_BLS get_BLS() const { return (BS_200h_BLS)(raw >> 48 & 0x3); }
BS_200h_BLSEGS51 get_BLS() const { return (BS_200h_BLSEGS51)(raw >> 48 & 0x3); }
/** Sets rotary direction wheel front left */
void set_DRTGVL(BS_200h_DRTGVL value){ raw = (raw & 0xffff3fffffffffff) | ((uint64_t)value & 0x3) << 46; }
void set_DRTGVL(BS_200h_DRTGVLEGS51 value){ raw = (raw & 0xffff3fffffffffff) | ((uint64_t)value & 0x3) << 46; }
/** Gets rotary direction wheel front left */
BS_200h_DRTGVL get_DRTGVL() const { return (BS_200h_DRTGVL)(raw >> 46 & 0x3); }
BS_200h_DRTGVLEGS51 get_DRTGVL() const { return (BS_200h_DRTGVLEGS51)(raw >> 46 & 0x3); }
/** Sets wheel speed front left. Conversion formula (To raw from real): y=(x-0.0)/1.00 */
void set_DVL(uint16_t value){ raw = (raw & 0xffffc000ffffffff) | ((uint64_t)value & 0x3fff) << 32; }
@ -208,10 +206,10 @@ typedef union {
uint16_t get_DVL() const { return (uint16_t)(raw >> 32 & 0x3fff); }
/** Sets direction of rotation wheel front right */
void set_DRTGVR(BS_200h_DRTGVR value){ raw = (raw & 0xffffffff3fffffff) | ((uint64_t)value & 0x3) << 30; }
void set_DRTGVR(BS_200h_DRTGVREGS51 value){ raw = (raw & 0xffffffff3fffffff) | ((uint64_t)value & 0x3) << 30; }
/** Gets direction of rotation wheel front right */
BS_200h_DRTGVR get_DRTGVR() const { return (BS_200h_DRTGVR)(raw >> 30 & 0x3); }
BS_200h_DRTGVREGS51 get_DRTGVR() const { return (BS_200h_DRTGVREGS51)(raw >> 30 & 0x3); }
/** Sets Right speed front right. Conversion formula (To raw from real): y=(x-0.0)/1.00 */
void set_DVR(uint16_t value){ raw = (raw & 0xffffffffc000ffff) | ((uint64_t)value & 0x3fff) << 16; }
@ -220,10 +218,10 @@ typedef union {
uint16_t get_DVR() const { return (uint16_t)(raw >> 16 & 0x3fff); }
/** Sets Rad Left for Cruise */
void set_DRTGTM(BS_200h_DRTGTM value){ raw = (raw & 0xffffffffffff3fff) | ((uint64_t)value & 0x3) << 14; }
void set_DRTGTM(BS_200h_DRTGTMEGS51 value){ raw = (raw & 0xffffffffffff3fff) | ((uint64_t)value & 0x3) << 14; }
/** Gets Rad Left for Cruise */
BS_200h_DRTGTM get_DRTGTM() const { return (BS_200h_DRTGTM)(raw >> 14 & 0x3); }
BS_200h_DRTGTMEGS51 get_DRTGTM() const { return (BS_200h_DRTGTMEGS51)(raw >> 14 & 0x3); }
/** Sets wheel speed links for cruise control. Conversion formula (To raw from real): y=(x-0.0)/1.00 */
void set_TM_DL(uint16_t value){ raw = (raw & 0xffffffffffffc000) | ((uint64_t)value & 0x3fff) << 0; }
@ -231,7 +229,7 @@ typedef union {
/** Gets wheel speed links for cruise control. Conversion formula (To real from raw): y=(1.00x)+0.0 */
uint16_t get_TM_DL() const { return (uint16_t)(raw >> 0 & 0x3fff); }
} BS_200;
} BS_200EGS51;
@ -239,8 +237,8 @@ typedef union {
uint64_t raw;
uint8_t bytes[8];
/** Gets CAN ID of BS_208 */
uint32_t get_canid(){ return BS_208_CAN_ID; }
/** Gets CAN ID of BS_208EGS51 */
uint32_t get_canid(){ return BS_208EGS51_CAN_ID; }
/** Sets ESP / Art-Wish: "Active Retract" */
void set_AKT_R_ESP(bool value){ raw = (raw & 0x7fffffffffffffff) | ((uint64_t)value & 0x1) << 63; }
@ -254,16 +252,16 @@ typedef union {
bool get_MINMAX_ART() const { return (bool)(raw >> 62 & 0x1); }
/** Sets Gear, upper limit */
void set_GMAX_ESP(BS_208h_GMAX_ESP value){ raw = (raw & 0xc7ffffffffffffff) | ((uint64_t)value & 0x7) << 59; }
void set_GMAX_ESP(BS_208h_GMAX_ESPEGS51 value){ raw = (raw & 0xc7ffffffffffffff) | ((uint64_t)value & 0x7) << 59; }
/** Gets Gear, upper limit */
BS_208h_GMAX_ESP get_GMAX_ESP() const { return (BS_208h_GMAX_ESP)(raw >> 59 & 0x7); }
BS_208h_GMAX_ESPEGS51 get_GMAX_ESP() const { return (BS_208h_GMAX_ESPEGS51)(raw >> 59 & 0x7); }
/** Sets Gear, lower limit */
void set_GMIN_ESP(BS_208h_GMIN_ESP value){ raw = (raw & 0xf8ffffffffffffff) | ((uint64_t)value & 0x7) << 56; }
void set_GMIN_ESP(BS_208h_GMIN_ESPEGS51 value){ raw = (raw & 0xf8ffffffffffffff) | ((uint64_t)value & 0x7) << 56; }
/** Gets Gear, lower limit */
BS_208h_GMIN_ESP get_GMIN_ESP() const { return (BS_208h_GMIN_ESP)(raw >> 56 & 0x7); }
BS_208h_GMIN_ESPEGS51 get_GMIN_ESP() const { return (BS_208h_GMIN_ESPEGS51)(raw >> 56 & 0x7); }
/** Sets Suppression Dynamic fully detection */
void set_DDYN_UNT(bool value){ raw = (raw & 0xff7fffffffffffff) | ((uint64_t)value & 0x1) << 55; }
@ -272,10 +270,10 @@ typedef union {
bool get_DDYN_UNT() const { return (bool)(raw >> 55 & 0x1); }
/** Sets system condition */
void set_SZS(BS_208h_SZS value){ raw = (raw & 0xff9fffffffffffff) | ((uint64_t)value & 0x3) << 53; }
void set_SZS(BS_208h_SZSEGS51 value){ raw = (raw & 0xff9fffffffffffff) | ((uint64_t)value & 0x3) << 53; }
/** Gets system condition */
BS_208h_SZS get_SZS() const { return (BS_208h_SZS)(raw >> 53 & 0x3); }
BS_208h_SZSEGS51 get_SZS() const { return (BS_208h_SZSEGS51)(raw >> 53 & 0x3); }
/** Sets Tempomat operation */
void set_TM_AUS(bool value){ raw = (raw & 0xffefffffffffffff) | ((uint64_t)value & 0x1) << 52; }
@ -284,10 +282,10 @@ typedef union {
bool get_TM_AUS() const { return (bool)(raw >> 52 & 0x1); }
/** Sets Switching Difference ESP */
void set_SLV_ESP(BS_208h_SLV_ESP value){ raw = (raw & 0xfff0ffffffffffff) | ((uint64_t)value & 0xf) << 48; }
void set_SLV_ESP(BS_208h_SLV_ESPEGS51 value){ raw = (raw & 0xfff0ffffffffffff) | ((uint64_t)value & 0xf) << 48; }
/** Gets Switching Difference ESP */
BS_208h_SLV_ESP get_SLV_ESP() const { return (BS_208h_SLV_ESP)(raw >> 48 & 0xf); }
BS_208h_SLV_ESPEGS51 get_SLV_ESP() const { return (BS_208h_SLV_ESPEGS51)(raw >> 48 & 0xf); }
/** Sets ESP brake engagement active */
void set_BRE_AKT_ESP(bool value){ raw = (raw & 0xffff7fffffffffff) | ((uint64_t)value & 0x1) << 47; }
@ -296,10 +294,10 @@ typedef union {
bool get_BRE_AKT_ESP() const { return (bool)(raw >> 47 & 0x1); }
/** Sets ESP request: "N" Insert */
void set_ANFN(BS_208h_ANFN value){ raw = (raw & 0xffff9fffffffffff) | ((uint64_t)value & 0x3) << 45; }
void set_ANFN(BS_208h_ANFNEGS51 value){ raw = (raw & 0xffff9fffffffffff) | ((uint64_t)value & 0x3) << 45; }
/** Gets ESP request: "N" Insert */
BS_208h_ANFN get_ANFN() const { return (BS_208h_ANFN)(raw >> 45 & 0x3); }
BS_208h_ANFNEGS51 get_ANFN() const { return (BS_208h_ANFNEGS51)(raw >> 45 & 0x3); }
/** Sets ART brake intervention active */
void set_BRE_AKT_ART(bool value){ raw = (raw & 0xffffefffffffffff) | ((uint64_t)value & 0x1) << 44; }
@ -314,10 +312,10 @@ typedef union {
uint16_t get_MBRE_ESP() const { return (uint16_t)(raw >> 32 & 0xfff); }
/** Sets rotary direction wheel rear right */
void set_DRTGHR(BS_208h_DRTGHR value){ raw = (raw & 0xffffffff3fffffff) | ((uint64_t)value & 0x3) << 30; }
void set_DRTGHR(BS_208h_DRTGHREGS51 value){ raw = (raw & 0xffffffff3fffffff) | ((uint64_t)value & 0x3) << 30; }
/** Gets rotary direction wheel rear right */
BS_208h_DRTGHR get_DRTGHR() const { return (BS_208h_DRTGHR)(raw >> 30 & 0x3); }
BS_208h_DRTGHREGS51 get_DRTGHR() const { return (BS_208h_DRTGHREGS51)(raw >> 30 & 0x3); }
/** Sets Rear wheel speed. Conversion formula (To raw from real): y=(x-0.0)/1.00 */
void set_DHR(uint16_t value){ raw = (raw & 0xffffffffc000ffff) | ((uint64_t)value & 0x3fff) << 16; }
@ -326,10 +324,10 @@ typedef union {
uint16_t get_DHR() const { return (uint16_t)(raw >> 16 & 0x3fff); }
/** Sets rotary direction wheel rear left */
void set_DRTGHL(BS_208h_DRTGHL value){ raw = (raw & 0xffffffffffff3fff) | ((uint64_t)value & 0x3) << 14; }
void set_DRTGHL(BS_208h_DRTGHLEGS51 value){ raw = (raw & 0xffffffffffff3fff) | ((uint64_t)value & 0x3) << 14; }
/** Gets rotary direction wheel rear left */
BS_208h_DRTGHL get_DRTGHL() const { return (BS_208h_DRTGHL)(raw >> 14 & 0x3); }
BS_208h_DRTGHLEGS51 get_DRTGHL() const { return (BS_208h_DRTGHLEGS51)(raw >> 14 & 0x3); }
/** Sets Rear wheel speed. Conversion formula (To raw from real): y=(x-0.0)/1.00 */
void set_DHL(uint16_t value){ raw = (raw & 0xffffffffffffc000) | ((uint64_t)value & 0x3fff) << 0; }
@ -337,7 +335,7 @@ typedef union {
/** Gets Rear wheel speed. Conversion formula (To real from raw): y=(1.00x)+0.0 */
uint16_t get_DHL() const { return (uint16_t)(raw >> 0 & 0x3fff); }
} BS_208;
} BS_208EGS51;
@ -352,11 +350,11 @@ class ECU_ESP51 {
*/
bool import_frames(uint64_t value, uint32_t can_id, uint64_t timestamp_now) {
switch(can_id) {
case BS_200_CAN_ID:
case BS_200EGS51_CAN_ID:
LAST_FRAME_TIMES[0] = timestamp_now;
FRAME_DATA[0] = value;
return true;
case BS_208_CAN_ID:
case BS_208EGS51_CAN_ID:
LAST_FRAME_TIMES[1] = timestamp_now;
FRAME_DATA[1] = value;
return true;
@ -372,7 +370,7 @@ class ECU_ESP51 {
*
* If the function returns true, then the pointer to 'dest' has been updated with the new CAN data
*/
bool get_BS_200(uint64_t now, uint64_t max_expire_time, BS_200* dest) const {
bool get_BS_200(uint64_t now, uint64_t max_expire_time, BS_200EGS51* dest) const {
if (LAST_FRAME_TIMES[0] == 0 || dest == nullptr) { // CAN Frame has not been seen on bus yet / NULL pointer
return false;
} else if (now > LAST_FRAME_TIMES[0] && now - LAST_FRAME_TIMES[0] > max_expire_time) { // CAN Frame has not refreshed in valid interval
@ -390,7 +388,7 @@ class ECU_ESP51 {
*
* If the function returns true, then the pointer to 'dest' has been updated with the new CAN data
*/
bool get_BS_208(uint64_t now, uint64_t max_expire_time, BS_208* dest) const {
bool get_BS_208(uint64_t now, uint64_t max_expire_time, BS_208EGS51* dest) const {
if (LAST_FRAME_TIMES[1] == 0 || dest == nullptr) { // CAN Frame has not been seen on bus yet / NULL pointer
return false;
} else if (now > LAST_FRAME_TIMES[1] && now - LAST_FRAME_TIMES[1] > max_expire_time) { // CAN Frame has not refreshed in valid interval
@ -405,6 +403,4 @@ class ECU_ESP51 {
uint64_t FRAME_DATA[2];
uint64_t LAST_FRAME_TIMES[2];
};
#endif // __ECU_ESP51_H_
#endif // EGS51_MODE
#endif // __ECU_ESP51_H_

36
lib/egs51_ecus/src/EZS51.h
View File

@ -8,18 +8,16 @@
* CAN Defintiion for ECU 'EZS51'
*/
#ifdef EGS51_MODE
#ifndef __ECU_EZS51_H_
#define __ECU_EZS51_H_
#include <stdint.h>
#define EZS_240_CAN_ID 0x0240
#define KLA_410_CAN_ID 0x0410
#define EZS_240EGS51_CAN_ID 0x0240
#define KLA_410EGS51_CAN_ID 0x0410
/** LHD / RHD */
enum class EZS_240h_LL_RLC {
enum class EZS_240h_LL_RLCEGS51 {
UNKNOWN = 0, // not defined
LL = 1, // Left
RL = 2, // RHD
@ -32,8 +30,8 @@ typedef union {
uint64_t raw;
uint8_t bytes[8];
/** Gets CAN ID of EZS_240 */
uint32_t get_canid(){ return EZS_240_CAN_ID; }
/** Gets CAN ID of EZS_240EGS51 */
uint32_t get_canid(){ return EZS_240EGS51_CAN_ID; }
/** Sets cruise control lever implausible */
void set_WH_UP(bool value){ raw = (raw & 0xdfffffffffffffff) | ((uint64_t)value & 0x1) << 61; }
@ -83,10 +81,10 @@ typedef union {
bool get_KG_ALB_OK() const { return (bool)(raw >> 54 & 0x1); }
/** Sets LHD / RHD */
void set_LL_RLC(EZS_240h_LL_RLC value){ raw = (raw & 0xffcfffffffffffff) | ((uint64_t)value & 0x3) << 52; }
void set_LL_RLC(EZS_240h_LL_RLCEGS51 value){ raw = (raw & 0xffcfffffffffffff) | ((uint64_t)value & 0x3) << 52; }
/** Gets LHD / RHD */
EZS_240h_LL_RLC get_LL_RLC() const { return (EZS_240h_LL_RLC)(raw >> 52 & 0x3); }
EZS_240h_LL_RLCEGS51 get_LL_RLC() const { return (EZS_240h_LL_RLCEGS51)(raw >> 52 & 0x3); }
/** Sets Reverse gear engaged (manual transmission only) */
void set_RG_SCHALT(bool value){ raw = (raw & 0xfff7ffffffffffff) | ((uint64_t)value & 0x1) << 51; }
@ -124,7 +122,7 @@ typedef union {
/** Gets Message counter. Conversion formula (To real from raw): y=(1.00x)+0.0 */
uint8_t get_BZ240h() const { return (uint8_t)(raw >> 40 & 0xf); }
} EZS_240;
} EZS_240EGS51;
@ -132,8 +130,8 @@ typedef union {
uint64_t raw;
uint8_t bytes[8];
/** Gets CAN ID of KLA_410 */
uint32_t get_canid(){ return KLA_410_CAN_ID; }
/** Gets CAN ID of KLA_410EGS51 */
uint32_t get_canid(){ return KLA_410EGS51_CAN_ID; }
/** Sets Turn on a heater */
void set_ZH_EIN_OK(bool value){ raw = (raw & 0x7fffffffffffffff) | ((uint64_t)value & 0x1) << 63; }
@ -194,7 +192,7 @@ typedef union {
/** Gets Motor fan setpoint speed. Conversion formula (To real from raw): y=(1.00x)+0.0 */
uint8_t get_NLFTS() const { return (uint8_t)(raw >> 32 & 0xff); }
} KLA_410;
} KLA_410EGS51;
@ -209,11 +207,11 @@ class ECU_EZS51 {
*/
bool import_frames(uint64_t value, uint32_t can_id, uint64_t timestamp_now) {
switch(can_id) {
case EZS_240_CAN_ID:
case EZS_240EGS51_CAN_ID:
LAST_FRAME_TIMES[0] = timestamp_now;
FRAME_DATA[0] = value;
return true;
case KLA_410_CAN_ID:
case KLA_410EGS51_CAN_ID:
LAST_FRAME_TIMES[1] = timestamp_now;
FRAME_DATA[1] = value;
return true;
@ -229,7 +227,7 @@ class ECU_EZS51 {
*
* If the function returns true, then the pointer to 'dest' has been updated with the new CAN data
*/
bool get_EZS_240(uint64_t now, uint64_t max_expire_time, EZS_240* dest) const {
bool get_EZS_240(uint64_t now, uint64_t max_expire_time, EZS_240EGS51* dest) const {
if (LAST_FRAME_TIMES[0] == 0 || dest == nullptr) { // CAN Frame has not been seen on bus yet / NULL pointer
return false;
} else if (now > LAST_FRAME_TIMES[0] && now - LAST_FRAME_TIMES[0] > max_expire_time) { // CAN Frame has not refreshed in valid interval
@ -247,7 +245,7 @@ class ECU_EZS51 {
*
* If the function returns true, then the pointer to 'dest' has been updated with the new CAN data
*/
bool get_KLA_410(uint64_t now, uint64_t max_expire_time, KLA_410* dest) const {
bool get_KLA_410(uint64_t now, uint64_t max_expire_time, KLA_410EGS51* dest) const {
if (LAST_FRAME_TIMES[1] == 0 || dest == nullptr) { // CAN Frame has not been seen on bus yet / NULL pointer
return false;
} else if (now > LAST_FRAME_TIMES[1] && now - LAST_FRAME_TIMES[1] > max_expire_time) { // CAN Frame has not refreshed in valid interval
@ -262,6 +260,4 @@ class ECU_EZS51 {
uint64_t FRAME_DATA[2];
uint64_t LAST_FRAME_TIMES[2];
};
#endif // __ECU_EZS51_H_
#endif // EGS51_MODE
#endif // __ECU_EZS51_H_

34
lib/egs51_ecus/src/GS51.h
View File

@ -8,17 +8,15 @@
* CAN Defintiion for ECU 'GS51'
*/
#ifdef EGS51_MODE
#ifndef __ECU_GS51_H_
#define __ECU_GS51_H_
#include <stdint.h>
#define GS_218_CAN_ID 0x0218
#define GS_218EGS51_CAN_ID 0x0218
/** Target gear */
enum class GS_218h_GZC {
enum class GS_218h_GZCEGS51 {
G_N = 0, // Destination "N"
G_D1 = 1, // Destination "1"
G_D2 = 2, // Destination "2"
@ -32,7 +30,7 @@ enum class GS_218h_GZC {
};
/** actual gear */
enum class GS_218h_GIC {
enum class GS_218h_GICEGS51 {
G_N = 0, // Destination "N"
G_D1 = 1, // Destination "1"
G_D2 = 2, // Destination "2"
@ -51,12 +49,12 @@ typedef union {
uint64_t raw;
uint8_t bytes[8];
/** Gets CAN ID of GS_218 */
uint32_t get_canid(){ return GS_218_CAN_ID; }
/** Sets Torque request value. 0xFE when inactive. Conversion formula (To raw from real): y=(x-0.0)/2.00 */
/** Gets CAN ID of GS_218EGS51 */
uint32_t get_canid(){ return GS_218EGS51_CAN_ID; }
/** Sets Torque request value. 0xFE when inactive. Conversion formula (To raw from real): y=(x-0.0)/0.50 */
void set_TORQUE_REQ(uint8_t value){ raw = (raw & 0x00ffffffffffffff) | ((uint64_t)value & 0xff) << 56; }
/** Gets Torque request value. 0xFE when inactive. Conversion formula (To real from raw): y=(2.00x)+0.0 */
/** Gets Torque request value. 0xFE when inactive. Conversion formula (To real from raw): y=(0.50x)+0.0 */
uint8_t get_TORQUE_REQ() const { return (uint8_t)(raw >> 56 & 0xff); }
/** Sets Enable torque request */
@ -66,16 +64,16 @@ typedef union {
bool get_TORQUE_REQ_EN() const { return (bool)(raw >> 54 & 0x1); }
/** Sets Target gear */
void set_GZC(GS_218h_GZC value){ raw = (raw & 0xffff0fffffffffff) | ((uint64_t)value & 0xf) << 44; }
void set_GZC(GS_218h_GZCEGS51 value){ raw = (raw & 0xffff0fffffffffff) | ((uint64_t)value & 0xf) << 44; }
/** Gets Target gear */
GS_218h_GZC get_GZC() const { return (GS_218h_GZC)(raw >> 44 & 0xf); }
GS_218h_GZCEGS51 get_GZC() const { return (GS_218h_GZCEGS51)(raw >> 44 & 0xf); }
/** Sets actual gear */
void set_GIC(GS_218h_GIC value){ raw = (raw & 0xfffff0ffffffffff) | ((uint64_t)value & 0xf) << 40; }
void set_GIC(GS_218h_GICEGS51 value){ raw = (raw & 0xfffff0ffffffffff) | ((uint64_t)value & 0xf) << 40; }
/** Gets actual gear */
GS_218h_GIC get_GIC() const { return (GS_218h_GIC)(raw >> 40 & 0xf); }
GS_218h_GICEGS51 get_GIC() const { return (GS_218h_GICEGS51)(raw >> 40 & 0xf); }
/** Sets Kickdown pressed */
void set_KICKDOWN(bool value){ raw = (raw & 0xffffffbfffffffff) | ((uint64_t)value & 0x1) << 38; }
@ -89,7 +87,7 @@ typedef union {
/** Gets error number or counter for calid / CVN transmission. Conversion formula (To real from raw): y=(1.00x)+0.0 */
uint8_t get_FEHLER() const { return (uint8_t)(raw >> 16 & 0xf); }
} GS_218;
} GS_218EGS51;
@ -104,7 +102,7 @@ class ECU_GS51 {
*/
bool import_frames(uint64_t value, uint32_t can_id, uint64_t timestamp_now) {
switch(can_id) {
case GS_218_CAN_ID:
case GS_218EGS51_CAN_ID:
LAST_FRAME_TIMES[0] = timestamp_now;
FRAME_DATA[0] = value;
return true;
@ -120,7 +118,7 @@ class ECU_GS51 {
*
* If the function returns true, then the pointer to 'dest' has been updated with the new CAN data
*/
bool get_GS_218(uint64_t now, uint64_t max_expire_time, GS_218* dest) const {
bool get_GS_218(uint64_t now, uint64_t max_expire_time, GS_218EGS51* dest) const {
if (LAST_FRAME_TIMES[0] == 0 || dest == nullptr) { // CAN Frame has not been seen on bus yet / NULL pointer
return false;
} else if (now > LAST_FRAME_TIMES[0] && now - LAST_FRAME_TIMES[0] > max_expire_time) { // CAN Frame has not refreshed in valid interval
@ -135,6 +133,4 @@ class ECU_GS51 {
uint64_t FRAME_DATA[1];
uint64_t LAST_FRAME_TIMES[1];
};
#endif // __ECU_GS51_H_
#endif // EGS51_MODE
#endif // __ECU_GS51_H_

24
lib/egs51_ecus/src/KOMBI51.h
View File

@ -8,17 +8,15 @@
* CAN Defintiion for ECU 'KOMBI51'
*/
#ifdef EGS51_MODE
#ifndef __ECU_KOMBI51_H_
#define __ECU_KOMBI51_H_
#include <stdint.h>
#define KOMBI_408_CAN_ID 0x0408
#define KOMBI_408EGS51_CAN_ID 0x0408
/** Winter tire maximum speed with 4 bits */
enum class KOMBI_408h_WRC {
enum class KOMBI_408h_WRCEGS51 {
UBG = 0, // Unlimited
BG210 = 1, // 210 km / h
BG190 = 2, // 190 km / h
@ -43,8 +41,8 @@ typedef union {
uint64_t raw;
uint8_t bytes[8];
/** Gets CAN ID of KOMBI_408 */
uint32_t get_canid(){ return KOMBI_408_CAN_ID; }
/** Gets CAN ID of KOMBI_408EGS51 */
uint32_t get_canid(){ return KOMBI_408EGS51_CAN_ID; }
/** Sets Tank level. Conversion formula (To raw from real): y=(x-0.0)/1.00 */
void set_TANK_FS(uint8_t value){ raw = (raw & 0x00ffffffffffffff) | ((uint64_t)value & 0xff) << 56; }
@ -154,12 +152,12 @@ typedef union {
bool get_RT_EIN() const { return (bool)(raw >> 3 & 0x1); }
/** Sets Winter tire maximum speed with 4 bits */
void set_WRC(KOMBI_408h_WRC value){ raw = (raw & 0xfffffffffffffff8) | ((uint64_t)value & 0x7) << 0; }
void set_WRC(KOMBI_408h_WRCEGS51 value){ raw = (raw & 0xfffffffffffffff8) | ((uint64_t)value & 0x7) << 0; }
/** Gets Winter tire maximum speed with 4 bits */
KOMBI_408h_WRC get_WRC() const { return (KOMBI_408h_WRC)(raw >> 0 & 0x7); }
KOMBI_408h_WRCEGS51 get_WRC() const { return (KOMBI_408h_WRCEGS51)(raw >> 0 & 0x7); }
} KOMBI_408;
} KOMBI_408EGS51;
@ -174,7 +172,7 @@ class ECU_KOMBI51 {
*/
bool import_frames(uint64_t value, uint32_t can_id, uint64_t timestamp_now) {
switch(can_id) {
case KOMBI_408_CAN_ID:
case KOMBI_408EGS51_CAN_ID:
LAST_FRAME_TIMES[0] = timestamp_now;
FRAME_DATA[0] = value;
return true;
@ -190,7 +188,7 @@ class ECU_KOMBI51 {
*
* If the function returns true, then the pointer to 'dest' has been updated with the new CAN data
*/
bool get_KOMBI_408(uint64_t now, uint64_t max_expire_time, KOMBI_408* dest) const {
bool get_KOMBI_408(uint64_t now, uint64_t max_expire_time, KOMBI_408EGS51* dest) const {
if (LAST_FRAME_TIMES[0] == 0 || dest == nullptr) { // CAN Frame has not been seen on bus yet / NULL pointer
return false;
} else if (now > LAST_FRAME_TIMES[0] && now - LAST_FRAME_TIMES[0] > max_expire_time) { // CAN Frame has not refreshed in valid interval
@ -205,6 +203,4 @@ class ECU_KOMBI51 {
uint64_t FRAME_DATA[1];
uint64_t LAST_FRAME_TIMES[1];
};
#endif // __ECU_KOMBI51_H_
#endif // EGS51_MODE
#endif // __ECU_KOMBI51_H_

80
lib/egs51_ecus/src/MS51.h
View File

@ -8,20 +8,18 @@
* CAN Defintiion for ECU 'MS51'
*/
#ifdef EGS51_MODE
#ifndef __ECU_MS51_H_
#define __ECU_MS51_H_
#include <stdint.h>
#define MS_308_CAN_ID 0x0308
#define MS_210_CAN_ID 0x0210
#define MS_310_CAN_ID 0x0310
#define MS_608_CAN_ID 0x0608
#define MS_308EGS51_CAN_ID 0x0308
#define MS_210EGS51_CAN_ID 0x0210
#define MS_310EGS51_CAN_ID 0x0310
#define MS_608EGS51_CAN_ID 0x0608
/** switching line shift MS */
enum class MS_210h_SLV_MS {
enum class MS_210h_SLV_MSEGS51 {
SKL0 = 0, // Shift characteristic "0"
SKL1 = 1, // Shift characteristic "1"
SKL2 = 2, // Shift characteristic "2"
@ -36,7 +34,7 @@ enum class MS_210h_SLV_MS {
};
/** Gear, upper limit */
enum class MS_210h_GMAX_MS {
enum class MS_210h_GMAX_MSEGS51 {
PASSIVE = 0, // passive value
G1 = 1, // Gear, upper limit = 1
G2 = 2, // Gear, upper limit = 2
@ -48,7 +46,7 @@ enum class MS_210h_GMAX_MS {
};
/** Gear, lower limit */
enum class MS_210h_GMIN_MS {
enum class MS_210h_GMIN_MSEGS51 {
PASSIVE = 0, // passive value
G1 = 1, // Gear, lower limit = 1
G2 = 2, // Gear, lower limit = 2
@ -65,8 +63,8 @@ typedef union {
uint64_t raw;
uint8_t bytes[8];
/** Gets CAN ID of MS_308 */
uint32_t get_canid(){ return MS_308_CAN_ID; }
/** Gets CAN ID of MS_308EGS51 */
uint32_t get_canid(){ return MS_308EGS51_CAN_ID; }
/** Sets clutch kicked */
void set_KPL(bool value){ raw = (raw & 0x7fffffffffffffff) | ((uint64_t)value & 0x1) << 63; }
@ -229,7 +227,7 @@ typedef union {
/** Gets oil quality. Conversion formula (To real from raw): y=(1.00x)+0.0 */
uint8_t get_OEL_QUAL() const { return (uint8_t)(raw >> 0 & 0xff); }
} MS_308;
} MS_308EGS51;
@ -237,8 +235,8 @@ typedef union {
uint64_t raw;
uint8_t bytes[8];
/** Gets CAN ID of MS_210 */
uint32_t get_canid(){ return MS_210_CAN_ID; }
/** Gets CAN ID of MS_210EGS51 */
uint32_t get_canid(){ return MS_210EGS51_CAN_ID; }
/** Sets Air compressor Emergency Shutdown */
void set_KOMP_NOTAUS(bool value){ raw = (raw & 0x7fffffffffffffff) | ((uint64_t)value & 0x1) << 63; }
@ -246,10 +244,10 @@ typedef union {
bool get_KOMP_NOTAUS() const { return (bool)(raw >> 63 & 0x1); }
/** Sets switching line shift MS */
void set_SLV_MS(MS_210h_SLV_MS value){ raw = (raw & 0x87ffffffffffffff) | ((uint64_t)value & 0xf) << 59; }
void set_SLV_MS(MS_210h_SLV_MSEGS51 value){ raw = (raw & 0x87ffffffffffffff) | ((uint64_t)value & 0xf) << 59; }
/** Gets switching line shift MS */
MS_210h_SLV_MS get_SLV_MS() const { return (MS_210h_SLV_MS)(raw >> 59 & 0xf); }
MS_210h_SLV_MSEGS51 get_SLV_MS() const { return (MS_210h_SLV_MSEGS51)(raw >> 59 & 0xf); }
/** Sets Switch KSG-creep */
void set_KRIECH_AUS(bool value){ raw = (raw & 0xfdffffffffffffff) | ((uint64_t)value & 0x1) << 57; }
@ -276,16 +274,16 @@ typedef union {
bool get_ZH_AUS_MS() const { return (bool)(raw >> 54 & 0x1); }
/** Sets Gear, upper limit */
void set_GMAX_MS(MS_210h_GMAX_MS value){ raw = (raw & 0xffc7ffffffffffff) | ((uint64_t)value & 0x7) << 51; }
void set_GMAX_MS(MS_210h_GMAX_MSEGS51 value){ raw = (raw & 0xffc7ffffffffffff) | ((uint64_t)value & 0x7) << 51; }
/** Gets Gear, upper limit */
MS_210h_GMAX_MS get_GMAX_MS() const { return (MS_210h_GMAX_MS)(raw >> 51 & 0x7); }
MS_210h_GMAX_MSEGS51 get_GMAX_MS() const { return (MS_210h_GMAX_MSEGS51)(raw >> 51 & 0x7); }
/** Sets Gear, lower limit */
void set_GMIN_MS(MS_210h_GMIN_MS value){ raw = (raw & 0xfff8ffffffffffff) | ((uint64_t)value & 0x7) << 48; }
void set_GMIN_MS(MS_210h_GMIN_MSEGS51 value){ raw = (raw & 0xfff8ffffffffffff) | ((uint64_t)value & 0x7) << 48; }
/** Gets Gear, lower limit */
MS_210h_GMIN_MS get_GMIN_MS() const { return (MS_210h_GMIN_MS)(raw >> 48 & 0x7); }
MS_210h_GMIN_MSEGS51 get_GMIN_MS() const { return (MS_210h_GMIN_MSEGS51)(raw >> 48 & 0x7); }
/** Sets pedal. Conversion formula (To raw from real): y=(x-0.0)/1.00 */
void set_PW(uint8_t value){ raw = (raw & 0xffff00ffffffffff) | ((uint64_t)value & 0xff) << 40; }
@ -449,7 +447,7 @@ typedef union {
/** Gets Set maximum or cruise control speed. Conversion formula (To real from raw): y=(1.00x)+0.0 */
uint8_t get_V_MAX_TM() const { return (uint8_t)(raw >> 0 & 0xff); }
} MS_210;
} MS_210EGS51;
@ -457,21 +455,21 @@ typedef union {
uint64_t raw;
uint8_t bytes[8];
/** Gets CAN ID of MS_310 */
uint32_t get_canid(){ return MS_310_CAN_ID; }
/** Sets engine coolant temperature. Conversion formula (To raw from real): y=(x-0.0)/2.00 */
/** Gets CAN ID of MS_310EGS51 */
uint32_t get_canid(){ return MS_310EGS51_CAN_ID; }
/** Sets engine coolant temperature. Conversion formula (To raw from real): y=(x-0.0)/0.50 */
void set_STA_TORQUE(uint8_t value){ raw = (raw & 0xffffffff00ffffff) | ((uint64_t)value & 0xff) << 24; }
/** Gets engine coolant temperature. Conversion formula (To real from raw): y=(2.00x)+0.0 */
/** Gets engine coolant temperature. Conversion formula (To real from raw): y=(0.50x)+0.0 */
uint8_t get_STA_TORQUE() const { return (uint8_t)(raw >> 24 & 0xff); }
/** Sets engine coolant temperature. Conversion formula (To raw from real): y=(x-0.0)/2.00 */
/** Sets engine coolant temperature. Conversion formula (To raw from real): y=(x-0.0)/0.50 */
void set_MAX_TORQUE(uint8_t value){ raw = (raw & 0xffffffffffff00ff) | ((uint64_t)value & 0xff) << 8; }
/** Gets engine coolant temperature. Conversion formula (To real from raw): y=(2.00x)+0.0 */
/** Gets engine coolant temperature. Conversion formula (To real from raw): y=(0.50x)+0.0 */
uint8_t get_MAX_TORQUE() const { return (uint8_t)(raw >> 8 & 0xff); }
} MS_310;
} MS_310EGS51;
@ -479,8 +477,8 @@ typedef union {
uint64_t raw;
uint8_t bytes[8];
/** Gets CAN ID of MS_608 */
uint32_t get_canid(){ return MS_608_CAN_ID; }
/** Gets CAN ID of MS_608EGS51 */
uint32_t get_canid(){ return MS_608EGS51_CAN_ID; }
/** Sets engine coolant temperature. Conversion formula (To raw from real): y=(x-0.0)/1.00 */
void set_T_MOT(uint8_t value){ raw = (raw & 0x00ffffffffffffff) | ((uint64_t)value & 0xff) << 56; }
@ -535,7 +533,7 @@ typedef union {
/** Gets consumption. Conversion formula (To real from raw): y=(0.50x)+0.0 */
uint16_t get_VB() const { return (uint16_t)(raw >> 8 & 0xffff); }
} MS_608;
} MS_608EGS51;
@ -550,19 +548,19 @@ class ECU_MS51 {
*/
bool import_frames(uint64_t value, uint32_t can_id, uint64_t timestamp_now) {
switch(can_id) {
case MS_308_CAN_ID:
case MS_308EGS51_CAN_ID:
LAST_FRAME_TIMES[0] = timestamp_now;
FRAME_DATA[0] = value;
return true;
case MS_210_CAN_ID:
case MS_210EGS51_CAN_ID:
LAST_FRAME_TIMES[1] = timestamp_now;
FRAME_DATA[1] = value;
return true;
case MS_310_CAN_ID:
case MS_310EGS51_CAN_ID:
LAST_FRAME_TIMES[2] = timestamp_now;
FRAME_DATA[2] = value;
return true;
case MS_608_CAN_ID:
case MS_608EGS51_CAN_ID:
LAST_FRAME_TIMES[3] = timestamp_now;
FRAME_DATA[3] = value;
return true;
@ -578,7 +576,7 @@ class ECU_MS51 {
*
* If the function returns true, then the pointer to 'dest' has been updated with the new CAN data
*/
bool get_MS_308(uint64_t now, uint64_t max_expire_time, MS_308* dest) const {
bool get_MS_308(uint64_t now, uint64_t max_expire_time, MS_308EGS51* dest) const {
if (LAST_FRAME_TIMES[0] == 0 || dest == nullptr) { // CAN Frame has not been seen on bus yet / NULL pointer
return false;
} else if (now > LAST_FRAME_TIMES[0] && now - LAST_FRAME_TIMES[0] > max_expire_time) { // CAN Frame has not refreshed in valid interval
@ -596,7 +594,7 @@ class ECU_MS51 {
*
* If the function returns true, then the pointer to 'dest' has been updated with the new CAN data
*/
bool get_MS_210(uint64_t now, uint64_t max_expire_time, MS_210* dest) const {
bool get_MS_210(uint64_t now, uint64_t max_expire_time, MS_210EGS51* dest) const {
if (LAST_FRAME_TIMES[1] == 0 || dest == nullptr) { // CAN Frame has not been seen on bus yet / NULL pointer
return false;
} else if (now > LAST_FRAME_TIMES[1] && now - LAST_FRAME_TIMES[1] > max_expire_time) { // CAN Frame has not refreshed in valid interval
@ -614,7 +612,7 @@ class ECU_MS51 {
*
* If the function returns true, then the pointer to 'dest' has been updated with the new CAN data
*/
bool get_MS_310(uint64_t now, uint64_t max_expire_time, MS_310* dest) const {
bool get_MS_310(uint64_t now, uint64_t max_expire_time, MS_310EGS51* dest) const {
if (LAST_FRAME_TIMES[2] == 0 || dest == nullptr) { // CAN Frame has not been seen on bus yet / NULL pointer
return false;
} else if (now > LAST_FRAME_TIMES[2] && now - LAST_FRAME_TIMES[2] > max_expire_time) { // CAN Frame has not refreshed in valid interval
@ -632,7 +630,7 @@ class ECU_MS51 {
*
* If the function returns true, then the pointer to 'dest' has been updated with the new CAN data
*/
bool get_MS_608(uint64_t now, uint64_t max_expire_time, MS_608* dest) const {
bool get_MS_608(uint64_t now, uint64_t max_expire_time, MS_608EGS51* dest) const {
if (LAST_FRAME_TIMES[3] == 0 || dest == nullptr) { // CAN Frame has not been seen on bus yet / NULL pointer
return false;
} else if (now > LAST_FRAME_TIMES[3] && now - LAST_FRAME_TIMES[3] > max_expire_time) { // CAN Frame has not refreshed in valid interval
@ -647,6 +645,4 @@ class ECU_MS51 {
uint64_t FRAME_DATA[4];
uint64_t LAST_FRAME_TIMES[4];
};
#endif // __ECU_MS51_H_
#endif // EGS51_MODE
#endif // __ECU_MS51_H_

6
lib/egs52_ecus/src/ANY_ECU.h → lib/egs52_ecus/src/ANY_ECU52.h
View File

@ -8,8 +8,6 @@
* CAN Defintiion for ECU 'ANY_ECU'
*/
#ifdef EGS52_MODE
#ifndef __ECU_ANY_ECU_H_
#define __ECU_ANY_ECU_H_
@ -1133,6 +1131,4 @@ class ECU_ANY_ECU {
uint64_t FRAME_DATA[10];
uint64_t LAST_FRAME_TIMES[10];
};
#endif // __ECU_ANY_ECU_H_
#endif // EGS52_MODE
#endif // __ECU_ANY_ECU_H_

6
lib/egs52_ecus/src/ESP_SBC.h
View File

@ -8,8 +8,6 @@
* CAN Defintiion for ECU 'ESP_SBC'
*/
#ifdef EGS52_MODE
#ifndef __ECU_ESP_SBC_H_
#define __ECU_ESP_SBC_H_
@ -796,6 +794,4 @@ class ECU_ESP_SBC {
uint64_t FRAME_DATA[5];
uint64_t LAST_FRAME_TIMES[5];
};
#endif // __ECU_ESP_SBC_H_
#endif // EGS52_MODE
#endif // __ECU_ESP_SBC_H_

6
lib/egs52_ecus/src/EWM.h
View File

@ -8,8 +8,6 @@
* CAN Defintiion for ECU 'EWM'
*/
#ifdef EGS52_MODE
#ifndef __ECU_EWM_H_
#define __ECU_EWM_H_
@ -115,6 +113,4 @@ class ECU_EWM {
uint64_t FRAME_DATA[1];
uint64_t LAST_FRAME_TIMES[1];
};
#endif // __ECU_EWM_H_
#endif // EGS52_MODE
#endif // __ECU_EWM_H_

6
lib/egs52_ecus/src/EZS.h
View File

@ -8,8 +8,6 @@
* CAN Defintiion for ECU 'EZS'
*/
#ifdef EGS52_MODE
#ifndef __ECU_EZS_H_
#define __ECU_EZS_H_
@ -556,6 +554,4 @@ class ECU_EZS {
uint64_t FRAME_DATA[4];
uint64_t LAST_FRAME_TIMES[4];
};
#endif // __ECU_EZS_H_
#endif // EGS52_MODE
#endif // __ECU_EZS_H_

6
lib/egs52_ecus/src/GS.h
View File

@ -8,8 +8,6 @@
* CAN Defintiion for ECU 'GS'
*/
#ifdef EGS52_MODE
#ifndef __ECU_GS_H_
#define __ECU_GS_H_
@ -733,6 +731,4 @@ class ECU_GS {
uint64_t FRAME_DATA[5];
uint64_t LAST_FRAME_TIMES[5];
};
#endif // __ECU_GS_H_
#endif // EGS52_MODE
#endif // __ECU_GS_H_

6
lib/egs52_ecus/src/KOMBI.h
View File

@ -8,8 +8,6 @@
* CAN Defintiion for ECU 'KOMBI'
*/
#ifdef EGS52_MODE
#ifndef __ECU_KOMBI_H_
#define __ECU_KOMBI_H_
@ -332,6 +330,4 @@ class ECU_KOMBI {
uint64_t FRAME_DATA[2];
uint64_t LAST_FRAME_TIMES[2];
};
#endif // __ECU_KOMBI_H_
#endif // EGS52_MODE
#endif // __ECU_KOMBI_H_

6
lib/egs52_ecus/src/MRM.h
View File

@ -8,8 +8,6 @@
* CAN Defintiion for ECU 'MRM'
*/
#ifdef EGS52_MODE
#ifndef __ECU_MRM_H_
#define __ECU_MRM_H_
@ -252,6 +250,4 @@ class ECU_MRM {
uint64_t FRAME_DATA[2];
uint64_t LAST_FRAME_TIMES[2];
};
#endif // __ECU_MRM_H_
#endif // EGS52_MODE
#endif // __ECU_MRM_H_

6
lib/egs52_ecus/src/MS.h
View File

@ -8,8 +8,6 @@
* CAN Defintiion for ECU 'MS'
*/
#ifdef EGS52_MODE
#ifndef __ECU_MS_H_
#define __ECU_MS_H_
@ -1159,6 +1157,4 @@ class ECU_MS {
uint64_t FRAME_DATA[8];
uint64_t LAST_FRAME_TIMES[8];
};
#endif // __ECU_MS_H_
#endif // EGS52_MODE
#endif // __ECU_MS_H_

6
lib/egs53_ecus/src/ANY_ECU.h
View File

@ -8,8 +8,6 @@
* CAN Defintiion for ECU 'ANY_ECU'
*/
#ifdef EGS53_MODE
#ifndef __ECU_ANY_ECU_H_
#define __ECU_ANY_ECU_H_
@ -764,6 +762,4 @@ class ECU_ANY_ECU {
uint64_t FRAME_DATA[6];
uint64_t LAST_FRAME_TIMES[6];
};
#endif // __ECU_ANY_ECU_H_
#endif // EGS53_MODE
#endif // __ECU_ANY_ECU_H_

6
lib/egs53_ecus/src/ECM.h
View File

@ -8,8 +8,6 @@
* CAN Defintiion for ECU 'ECM'
*/
#ifdef EGS53_MODE
#ifndef __ECU_ECM_H_
#define __ECU_ECM_H_
@ -4674,6 +4672,4 @@ class ECU_ECM {
uint64_t FRAME_DATA[30];
uint64_t LAST_FRAME_TIMES[30];
};
#endif // __ECU_ECM_H_
#endif // EGS53_MODE
#endif // __ECU_ECM_H_

6
lib/egs53_ecus/src/FSCM.h
View File

@ -8,8 +8,6 @@
* CAN Defintiion for ECU 'FSCM'
*/
#ifdef EGS53_MODE
#ifndef __ECU_FSCM_H_
#define __ECU_FSCM_H_
@ -299,6 +297,4 @@ class ECU_FSCM {
uint64_t FRAME_DATA[2];
uint64_t LAST_FRAME_TIMES[2];
};
#endif // __ECU_FSCM_H_
#endif // EGS53_MODE
#endif // __ECU_FSCM_H_

6
lib/egs53_ecus/src/TCM.h
View File

@ -8,8 +8,6 @@
* CAN Defintiion for ECU 'TCM'
*/
#ifdef EGS53_MODE
#ifndef __ECU_TCM_H_
#define __ECU_TCM_H_
@ -1105,6 +1103,4 @@ class ECU_TCM {
uint64_t FRAME_DATA[8];
uint64_t LAST_FRAME_TIMES[8];
};
#endif // __ECU_TCM_H_
#endif // EGS53_MODE
#endif // __ECU_TCM_H_

6
lib/egs53_ecus/src/TSLM.h
View File

@ -8,8 +8,6 @@
* CAN Defintiion for ECU 'TSLM'
*/
#ifdef EGS53_MODE
#ifndef __ECU_TSLM_H_
#define __ECU_TSLM_H_
@ -246,6 +244,4 @@ class ECU_TSLM {
uint64_t FRAME_DATA[2];
uint64_t LAST_FRAME_TIMES[2];
};
#endif // __ECU_TSLM_H_
#endif // EGS53_MODE
#endif // __ECU_TSLM_H_

2
patchappinfo.py
View File

@ -152,7 +152,7 @@ def get_project_name(env) -> Tuple[str, str]:
return progname.fallback_get(
(
lambda: "{}_{}".format(env.GetProjectOption("egs_ver"), branch), #env.GetProjectOption(progname.PROG_NAME_OPTION, None),
lambda: "unified-un52",
"custom_option",
),
(lambda: git_relative_dir(), "git_relative_dir"),

21
platformio.ini
View File

@ -9,10 +9,10 @@
; https://docs.platformio.org/page/projectconf.html
[platformio]
default_envs = egs52
default_envs = unified
[env]
platform = espressif32@5.0.0
platform = espressif32@5.2.0
board = esp-wrover-kit
framework = espidf
board_build.f_flash = 80000000L
@ -24,21 +24,8 @@ monitor_port = /dev/ttyUSB0
board_build.partitions = partitions.csv
extra_scripts = post:patchappinfo.py
[env:egs51]
build_flags = -Wall -DEGS51_MODE
egs_ver = egs51
[env:egs52]
build_flags = -Wall -DEGS52_MODE
egs_ver = egs52
[env:egs52-red-pcb]
build_flags = -Wall -DEGS52_MODE -DRED_BCB
egs_ver = egs52
[env:egs53]
build_flags = -Wall -DEGS53_MODE
egs_ver = egs53
[env:unified]
build_flags = -Wall
[env:latest_stable]
platform = linux_x86_64

1452
sdkconfig.egs51
View File

File diff suppressed because it is too large Load Diff

1452
sdkconfig.egs52
View File

File diff suppressed because it is too large Load Diff

1452
sdkconfig.egs53
View File

File diff suppressed because it is too large Load Diff

167
sdkconfig.egs52-red-pcb → sdkconfig.unified
View File

@ -55,6 +55,7 @@ CONFIG_BOOTLOADER_SPI_WP_PIN=7
CONFIG_BOOTLOADER_VDDSDIO_BOOST_1_9V=y
# CONFIG_BOOTLOADER_FACTORY_RESET is not set
# CONFIG_BOOTLOADER_APP_TEST is not set
CONFIG_BOOTLOADER_REGION_PROTECTION_ENABLE=y
CONFIG_BOOTLOADER_WDT_ENABLE=y
# CONFIG_BOOTLOADER_WDT_DISABLE_IN_USER_CODE is not set
CONFIG_BOOTLOADER_WDT_TIME_MS=9000
@ -96,6 +97,9 @@ CONFIG_ESPTOOLPY_FLASHSIZE_2MB=y
# CONFIG_ESPTOOLPY_FLASHSIZE_4MB is not set
# CONFIG_ESPTOOLPY_FLASHSIZE_8MB is not set
# CONFIG_ESPTOOLPY_FLASHSIZE_16MB is not set
# CONFIG_ESPTOOLPY_FLASHSIZE_32MB is not set
# CONFIG_ESPTOOLPY_FLASHSIZE_64MB is not set
# CONFIG_ESPTOOLPY_FLASHSIZE_128MB is not set
CONFIG_ESPTOOLPY_FLASHSIZE="2MB"
CONFIG_ESPTOOLPY_FLASHSIZE_DETECT=y
CONFIG_ESPTOOLPY_BEFORE_RESET=y
@ -166,26 +170,13 @@ CONFIG_APPTRACE_DEST_NONE=y
CONFIG_APPTRACE_LOCK_ENABLE=y
# end of Application Level Tracing
#
# ESP-ASIO
#
# CONFIG_ASIO_SSL_SUPPORT is not set
# end of ESP-ASIO
#
# Bluetooth
#
# CONFIG_BT_ENABLED is not set
# end of Bluetooth
#
# CoAP Configuration
#
CONFIG_COAP_MBEDTLS_PSK=y
# CONFIG_COAP_MBEDTLS_PKI is not set
# CONFIG_COAP_MBEDTLS_DEBUG is not set
CONFIG_COAP_LOG_DEFAULT_LEVEL=0
# end of CoAP Configuration
#
# Driver configurations
@ -266,7 +257,6 @@ CONFIG_EFUSE_MAX_BLK_LEN=192
CONFIG_ESP_TLS_USING_MBEDTLS=y
# CONFIG_ESP_TLS_USE_SECURE_ELEMENT is not set
# CONFIG_ESP_TLS_SERVER is not set
# CONFIG_ESP_TLS_CLIENT_SESSION_TICKETS is not set
# CONFIG_ESP_TLS_PSK_VERIFICATION is not set
# CONFIG_ESP_TLS_INSECURE is not set
# end of ESP-TLS
@ -392,7 +382,7 @@ CONFIG_ESP32_XTAL_FREQ_40=y
# CONFIG_ESP32_XTAL_FREQ_AUTO is not set
CONFIG_ESP32_XTAL_FREQ=40
# CONFIG_ESP32_DISABLE_BASIC_ROM_CONSOLE is not set
# CONFIG_ESP32_NO_BLOBS is not set
CONFIG_ESP32_NO_BLOBS=y
# CONFIG_ESP32_COMPATIBLE_PRE_V2_1_BOOTLOADERS is not set
# CONFIG_ESP32_COMPATIBLE_PRE_V3_1_BOOTLOADERS is not set
# CONFIG_ESP32_USE_FIXED_STATIC_RAM_SIZE is not set
@ -438,7 +428,7 @@ CONFIG_ESP_ERR_TO_NAME_LOOKUP=y
#
# CONFIG_ESP_HTTP_CLIENT_ENABLE_HTTPS is not set
# CONFIG_ESP_HTTP_CLIENT_ENABLE_BASIC_AUTH is not set
CONFIG_ESP_HTTP_CLIENT_ENABLE_DIGEST_AUTH=y
# CONFIG_ESP_HTTP_CLIENT_ENABLE_DIGEST_AUTH is not set
# end of ESP HTTP client
#
@ -446,7 +436,7 @@ CONFIG_ESP_HTTP_CLIENT_ENABLE_DIGEST_AUTH=y
#
CONFIG_HTTPD_MAX_REQ_HDR_LEN=512
CONFIG_HTTPD_MAX_URI_LEN=512
CONFIG_HTTPD_ERR_RESP_NO_DELAY=y
# CONFIG_HTTPD_ERR_RESP_NO_DELAY is not set
CONFIG_HTTPD_PURGE_BUF_LEN=32
# CONFIG_HTTPD_LOG_PURGE_DATA is not set
# CONFIG_HTTPD_WS_SUPPORT is not set
@ -518,17 +508,17 @@ CONFIG_LCD_PANEL_IO_FORMAT_BUF_SIZE=32
CONFIG_ESP_NETIF_IP_LOST_TIMER_INTERVAL=120
CONFIG_ESP_NETIF_TCPIP_LWIP=y
# CONFIG_ESP_NETIF_LOOPBACK is not set
CONFIG_ESP_NETIF_TCPIP_ADAPTER_COMPATIBLE_LAYER=y
# CONFIG_ESP_NETIF_TCPIP_ADAPTER_COMPATIBLE_LAYER is not set
# end of ESP NETIF Adapter
#
# PHY
#
CONFIG_ESP_PHY_CALIBRATION_AND_DATA_STORAGE=y
# CONFIG_ESP_PHY_CALIBRATION_AND_DATA_STORAGE is not set
# CONFIG_ESP_PHY_INIT_DATA_IN_PARTITION is not set
CONFIG_ESP_PHY_MAX_WIFI_TX_POWER=20
CONFIG_ESP_PHY_MAX_TX_POWER=20
CONFIG_ESP_PHY_REDUCE_TX_POWER=y
# CONFIG_ESP_PHY_REDUCE_TX_POWER is not set
# end of PHY
#
@ -609,10 +599,8 @@ CONFIG_ESP32_WIFI_TX_BUFFER_TYPE=0
CONFIG_ESP32_WIFI_STATIC_TX_BUFFER_NUM=16
CONFIG_ESP32_WIFI_CACHE_TX_BUFFER_NUM=32
# CONFIG_ESP32_WIFI_CSI_ENABLED is not set
CONFIG_ESP32_WIFI_AMPDU_TX_ENABLED=y
CONFIG_ESP32_WIFI_TX_BA_WIN=6
CONFIG_ESP32_WIFI_AMPDU_RX_ENABLED=y
CONFIG_ESP32_WIFI_RX_BA_WIN=6
# CONFIG_ESP32_WIFI_AMPDU_TX_ENABLED is not set
# CONFIG_ESP32_WIFI_AMPDU_RX_ENABLED is not set
# CONFIG_ESP32_WIFI_AMSDU_TX_ENABLED is not set
# CONFIG_ESP32_WIFI_NVS_ENABLED is not set
CONFIG_ESP32_WIFI_TASK_PINNED_TO_CORE_0=y
@ -624,7 +612,7 @@ CONFIG_ESP32_WIFI_MGMT_SBUF_NUM=32
# CONFIG_ESP_WIFI_SLP_IRAM_OPT is not set
# CONFIG_ESP_WIFI_STA_DISCONNECTED_PM_ENABLE is not set
# CONFIG_ESP_WIFI_GMAC_SUPPORT is not set
CONFIG_ESP_WIFI_SOFTAP_SUPPORT=y
# CONFIG_ESP_WIFI_SOFTAP_SUPPORT is not set
# end of Wi-Fi
#
@ -673,18 +661,15 @@ CONFIG_FATFS_LFN_NONE=y
# CONFIG_FATFS_LFN_STACK is not set
CONFIG_FATFS_FS_LOCK=0
CONFIG_FATFS_TIMEOUT_MS=10000
CONFIG_FATFS_PER_FILE_CACHE=y
CONFIG_FATFS_ALLOC_PREFER_EXTRAM=y
# CONFIG_FATFS_PER_FILE_CACHE is not set
# CONFIG_FATFS_ALLOC_PREFER_EXTRAM is not set
# CONFIG_FATFS_USE_FASTSEEK is not set
# end of FAT Filesystem support
#
# Modbus configuration
#
CONFIG_FMB_COMM_MODE_TCP_EN=y
CONFIG_FMB_TCP_PORT_DEFAULT=502
CONFIG_FMB_TCP_PORT_MAX_CONN=5
CONFIG_FMB_TCP_CONNECTION_TOUT_SEC=20
# CONFIG_FMB_COMM_MODE_TCP_EN is not set
CONFIG_FMB_COMM_MODE_RTU_EN=y
CONFIG_FMB_COMM_MODE_ASCII_EN=y
CONFIG_FMB_MASTER_TIMEOUT_MS_RESPOND=150
@ -824,9 +809,7 @@ CONFIG_LWIP_MAX_SOCKETS=10
# CONFIG_LWIP_SO_RCVBUF is not set
# CONFIG_LWIP_NETBUF_RECVINFO is not set
# CONFIG_LWIP_IP4_FRAG is not set
# CONFIG_LWIP_IP6_FRAG is not set
# CONFIG_LWIP_IP4_REASSEMBLY is not set
# CONFIG_LWIP_IP6_REASSEMBLY is not set
# CONFIG_LWIP_IP_FORWARD is not set
# CONFIG_LWIP_STATS is not set
# CONFIG_LWIP_ETHARP_TRUST_IP_MAC is not set
@ -834,23 +817,18 @@ CONFIG_LWIP_MAX_SOCKETS=10
CONFIG_LWIP_TCPIP_RECVMBOX_SIZE=32
# CONFIG_LWIP_DHCP_DOES_ARP_CHECK is not set
# CONFIG_LWIP_DHCP_DISABLE_CLIENT_ID is not set
CONFIG_LWIP_DHCP_DISABLE_VENDOR_CLASS_ID=y
# CONFIG_LWIP_DHCP_DISABLE_VENDOR_CLASS_ID is not set
# CONFIG_LWIP_DHCP_RESTORE_LAST_IP is not set
CONFIG_LWIP_DHCP_OPTIONS_LEN=68
#
# DHCP server
#
CONFIG_LWIP_DHCPS=y
CONFIG_LWIP_DHCPS_LEASE_UNIT=60
CONFIG_LWIP_DHCPS_MAX_STATION_NUM=8
# CONFIG_LWIP_DHCPS is not set
# end of DHCP server
# CONFIG_LWIP_AUTOIP is not set
CONFIG_LWIP_IPV6=y
# CONFIG_LWIP_IPV6_AUTOCONFIG is not set
CONFIG_LWIP_IPV6_NUM_ADDRESSES=3
# CONFIG_LWIP_IPV6_FORWARD is not set
# CONFIG_LWIP_IPV6 is not set
# CONFIG_LWIP_NETIF_STATUS_CALLBACK is not set
# CONFIG_LWIP_NETIF_LOOPBACK is not set
@ -889,7 +867,7 @@ CONFIG_LWIP_UDP_RECVMBOX_SIZE=6
#
# CONFIG_LWIP_CHECKSUM_CHECK_IP is not set
# CONFIG_LWIP_CHECKSUM_CHECK_UDP is not set
CONFIG_LWIP_CHECKSUM_CHECK_ICMP=y
# CONFIG_LWIP_CHECKSUM_CHECK_ICMP is not set
# end of Checksums
CONFIG_LWIP_TCPIP_TASK_STACK_SIZE=3072
@ -898,16 +876,12 @@ CONFIG_LWIP_TCPIP_TASK_AFFINITY_NO_AFFINITY=y
# CONFIG_LWIP_TCPIP_TASK_AFFINITY_CPU1 is not set
CONFIG_LWIP_TCPIP_TASK_AFFINITY=0x7FFFFFFF
# CONFIG_LWIP_PPP_SUPPORT is not set
CONFIG_LWIP_IPV6_MEMP_NUM_ND6_QUEUE=3
CONFIG_LWIP_IPV6_ND6_NUM_NEIGHBORS=5
# CONFIG_LWIP_SLIP_SUPPORT is not set
#
# ICMP
#
CONFIG_LWIP_ICMP=y
# CONFIG_LWIP_MULTICAST_PING is not set
# CONFIG_LWIP_BROADCAST_PING is not set
# CONFIG_LWIP_ICMP is not set
# end of ICMP
#
@ -929,15 +903,9 @@ CONFIG_LWIP_SNTP_UPDATE_DELAY=3600000
#
# Hooks
#
# CONFIG_LWIP_HOOK_TCP_ISN_NONE is not set
CONFIG_LWIP_HOOK_TCP_ISN_DEFAULT=y
CONFIG_LWIP_HOOK_TCP_ISN_NONE=y
# CONFIG_LWIP_HOOK_TCP_ISN_DEFAULT is not set
# CONFIG_LWIP_HOOK_TCP_ISN_CUSTOM is not set
CONFIG_LWIP_HOOK_IP6_ROUTE_NONE=y
# CONFIG_LWIP_HOOK_IP6_ROUTE_DEFAULT is not set
# CONFIG_LWIP_HOOK_IP6_ROUTE_CUSTOM is not set
CONFIG_LWIP_HOOK_ND6_GET_GW_NONE=y
# CONFIG_LWIP_HOOK_ND6_GET_GW_DEFAULT is not set
# CONFIG_LWIP_HOOK_ND6_GET_GW_CUSTOM is not set
CONFIG_LWIP_HOOK_NETCONN_EXT_RESOLVE_NONE=y
# CONFIG_LWIP_HOOK_NETCONN_EXT_RESOLVE_DEFAULT is not set
# CONFIG_LWIP_HOOK_NETCONN_EXT_RESOLVE_CUSTOM is not set
@ -953,9 +921,8 @@ CONFIG_MBEDTLS_INTERNAL_MEM_ALLOC=y
# CONFIG_MBEDTLS_EXTERNAL_MEM_ALLOC is not set
# CONFIG_MBEDTLS_DEFAULT_MEM_ALLOC is not set
# CONFIG_MBEDTLS_CUSTOM_MEM_ALLOC is not set
CONFIG_MBEDTLS_ASYMMETRIC_CONTENT_LEN=y
CONFIG_MBEDTLS_SSL_IN_CONTENT_LEN=16384
CONFIG_MBEDTLS_SSL_OUT_CONTENT_LEN=4096
CONFIG_MBEDTLS_SSL_MAX_CONTENT_LEN=16384
# CONFIG_MBEDTLS_ASYMMETRIC_CONTENT_LEN is not set
# CONFIG_MBEDTLS_DYNAMIC_BUFFER is not set
# CONFIG_MBEDTLS_DEBUG is not set
@ -965,69 +932,34 @@ CONFIG_MBEDTLS_SSL_OUT_CONTENT_LEN=4096
# CONFIG_MBEDTLS_SSL_VARIABLE_BUFFER_LENGTH is not set
# CONFIG_MBEDTLS_X509_TRUSTED_CERT_CALLBACK is not set
# CONFIG_MBEDTLS_SSL_CONTEXT_SERIALIZATION is not set
CONFIG_MBEDTLS_SSL_KEEP_PEER_CERTIFICATE=y
# CONFIG_MBEDTLS_SSL_KEEP_PEER_CERTIFICATE is not set
# end of mbedTLS v2.28.x related
#
# Certificate Bundle
#
CONFIG_MBEDTLS_CERTIFICATE_BUNDLE=y
# CONFIG_MBEDTLS_CERTIFICATE_BUNDLE_DEFAULT_FULL is not set
CONFIG_MBEDTLS_CERTIFICATE_BUNDLE_DEFAULT_CMN=y
# CONFIG_MBEDTLS_CERTIFICATE_BUNDLE_DEFAULT_NONE is not set
# CONFIG_MBEDTLS_CUSTOM_CERTIFICATE_BUNDLE is not set
# CONFIG_MBEDTLS_CERTIFICATE_BUNDLE is not set
# end of Certificate Bundle
# CONFIG_MBEDTLS_ECP_RESTARTABLE is not set
# CONFIG_MBEDTLS_CMAC_C is not set
CONFIG_MBEDTLS_HARDWARE_AES=y
CONFIG_MBEDTLS_HARDWARE_MPI=y
CONFIG_MBEDTLS_HARDWARE_SHA=y
CONFIG_MBEDTLS_ROM_MD5=y
# CONFIG_MBEDTLS_ATCA_HW_ECDSA_SIGN is not set
# CONFIG_MBEDTLS_ATCA_HW_ECDSA_VERIFY is not set
CONFIG_MBEDTLS_HAVE_TIME=y
# CONFIG_MBEDTLS_HAVE_TIME_DATE is not set
CONFIG_MBEDTLS_ECDSA_DETERMINISTIC=y
CONFIG_MBEDTLS_SHA512_C=y
CONFIG_MBEDTLS_TLS_SERVER_AND_CLIENT=y
# CONFIG_MBEDTLS_HAVE_TIME is not set
# CONFIG_MBEDTLS_ECDSA_DETERMINISTIC is not set
# CONFIG_MBEDTLS_SHA512_C is not set
# CONFIG_MBEDTLS_TLS_SERVER_AND_CLIENT is not set
# CONFIG_MBEDTLS_TLS_SERVER_ONLY is not set
# CONFIG_MBEDTLS_TLS_CLIENT_ONLY is not set
# CONFIG_MBEDTLS_TLS_DISABLED is not set
CONFIG_MBEDTLS_TLS_SERVER=y
CONFIG_MBEDTLS_TLS_CLIENT=y
CONFIG_MBEDTLS_TLS_ENABLED=y
#
# TLS Key Exchange Methods
#
# CONFIG_MBEDTLS_PSK_MODES is not set
CONFIG_MBEDTLS_KEY_EXCHANGE_RSA=y
CONFIG_MBEDTLS_KEY_EXCHANGE_DHE_RSA=y
CONFIG_MBEDTLS_KEY_EXCHANGE_ELLIPTIC_CURVE=y
CONFIG_MBEDTLS_KEY_EXCHANGE_ECDHE_RSA=y
CONFIG_MBEDTLS_KEY_EXCHANGE_ECDHE_ECDSA=y
CONFIG_MBEDTLS_KEY_EXCHANGE_ECDH_ECDSA=y
CONFIG_MBEDTLS_KEY_EXCHANGE_ECDH_RSA=y
# end of TLS Key Exchange Methods
CONFIG_MBEDTLS_SSL_RENEGOTIATION=y
# CONFIG_MBEDTLS_SSL_PROTO_SSL3 is not set
CONFIG_MBEDTLS_SSL_PROTO_TLS1=y
CONFIG_MBEDTLS_SSL_PROTO_TLS1_1=y
CONFIG_MBEDTLS_SSL_PROTO_TLS1_2=y
# CONFIG_MBEDTLS_SSL_PROTO_GMTSSL1_1 is not set
# CONFIG_MBEDTLS_SSL_PROTO_DTLS is not set
CONFIG_MBEDTLS_SSL_ALPN=y
CONFIG_MBEDTLS_CLIENT_SSL_SESSION_TICKETS=y
CONFIG_MBEDTLS_X509_CHECK_KEY_USAGE=y
CONFIG_MBEDTLS_X509_CHECK_EXTENDED_KEY_USAGE=y
CONFIG_MBEDTLS_SERVER_SSL_SESSION_TICKETS=y
CONFIG_MBEDTLS_TLS_DISABLED=y
#
# Symmetric Ciphers
#
CONFIG_MBEDTLS_AES_C=y
# CONFIG_MBEDTLS_AES_C is not set
# CONFIG_MBEDTLS_CAMELLIA_C is not set
# CONFIG_MBEDTLS_DES_C is not set
CONFIG_MBEDTLS_RC4_DISABLED=y
@ -1035,9 +967,6 @@ CONFIG_MBEDTLS_RC4_DISABLED=y
# CONFIG_MBEDTLS_RC4_ENABLED is not set
# CONFIG_MBEDTLS_BLOWFISH_C is not set
# CONFIG_MBEDTLS_XTEA_C is not set
CONFIG_MBEDTLS_CCM_C=y
CONFIG_MBEDTLS_GCM_C=y
# CONFIG_MBEDTLS_NIST_KW_C is not set
# end of Symmetric Ciphers
# CONFIG_MBEDTLS_RIPEMD160_C is not set
@ -1051,23 +980,7 @@ CONFIG_MBEDTLS_X509_CRL_PARSE_C=y
CONFIG_MBEDTLS_X509_CSR_PARSE_C=y
# end of Certificates
CONFIG_MBEDTLS_ECP_C=y
CONFIG_MBEDTLS_ECDH_C=y
CONFIG_MBEDTLS_ECDSA_C=y
# CONFIG_MBEDTLS_ECJPAKE_C is not set
CONFIG_MBEDTLS_ECP_DP_SECP192R1_ENABLED=y
CONFIG_MBEDTLS_ECP_DP_SECP224R1_ENABLED=y
CONFIG_MBEDTLS_ECP_DP_SECP256R1_ENABLED=y
CONFIG_MBEDTLS_ECP_DP_SECP384R1_ENABLED=y
CONFIG_MBEDTLS_ECP_DP_SECP521R1_ENABLED=y
CONFIG_MBEDTLS_ECP_DP_SECP192K1_ENABLED=y
CONFIG_MBEDTLS_ECP_DP_SECP224K1_ENABLED=y
CONFIG_MBEDTLS_ECP_DP_SECP256K1_ENABLED=y
CONFIG_MBEDTLS_ECP_DP_BP256R1_ENABLED=y
CONFIG_MBEDTLS_ECP_DP_BP384R1_ENABLED=y
CONFIG_MBEDTLS_ECP_DP_BP512R1_ENABLED=y
CONFIG_MBEDTLS_ECP_DP_CURVE25519_ENABLED=y
CONFIG_MBEDTLS_ECP_NIST_OPTIM=y
# CONFIG_MBEDTLS_ECP_C is not set
# CONFIG_MBEDTLS_POLY1305_C is not set
# CONFIG_MBEDTLS_CHACHA20_C is not set
# CONFIG_MBEDTLS_HKDF_C is not set
@ -1090,7 +1003,7 @@ CONFIG_MDNS_SERVICE_ADD_TIMEOUT_MS=2000
# CONFIG_MDNS_STRICT_MODE is not set
CONFIG_MDNS_TIMER_PERIOD_MS=100
# CONFIG_MDNS_NETWORKING_SOCKET is not set
CONFIG_MDNS_MULTIPLE_INSTANCE=y
# CONFIG_MDNS_MULTIPLE_INSTANCE is not set
# end of mDNS
#
@ -1121,6 +1034,7 @@ CONFIG_NEWLIB_STDIN_LINE_ENDING_LF=y
#
# NVS
#
# CONFIG_NVS_ASSERT_ERROR_CHECK is not set
# end of NVS
#
@ -1263,6 +1177,7 @@ CONFIG_WL_SECTOR_SIZE=4096
#
CONFIG_WIFI_PROV_SCAN_MAX_ENTRIES=16
CONFIG_WIFI_PROV_AUTOSTOP_TIMEOUT=30
# CONFIG_WIFI_PROV_BLE_FORCE_ENCRYPTION is not set
# end of Wi-Fi Provisioning Manager
#
@ -1275,6 +1190,8 @@ CONFIG_WIFI_PROV_AUTOSTOP_TIMEOUT=30
# CONFIG_WPA_TESTING_OPTIONS is not set
# CONFIG_WPA_WPS_STRICT is not set
# CONFIG_WPA_11KV_SUPPORT is not set
# CONFIG_WPA_MBO_SUPPORT is not set
# CONFIG_WPA_DPP_SUPPORT is not set
# end of Supplicant
# end of Component config
@ -1346,7 +1263,7 @@ CONFIG_ESP32_RTC_CLOCK_SOURCE_INTERNAL_RC=y
# CONFIG_ESP32_RTC_CLOCK_SOURCE_EXTERNAL_OSC is not set
# CONFIG_ESP32_RTC_CLOCK_SOURCE_INTERNAL_8MD256 is not set
# CONFIG_DISABLE_BASIC_ROM_CONSOLE is not set
# CONFIG_NO_BLOBS is not set
CONFIG_NO_BLOBS=y
# CONFIG_COMPATIBLE_PRE_V2_1_BOOTLOADERS is not set
# CONFIG_EVENT_LOOP_PROFILING is not set
# CONFIG_POST_EVENTS_FROM_ISR is not set
@ -1355,11 +1272,11 @@ CONFIG_TWO_UNIVERSAL_MAC_ADDRESS=y
CONFIG_NUMBER_OF_UNIVERSAL_MAC_ADDRESS=2
# CONFIG_ESP32C3_LIGHTSLEEP_GPIO_RESET_WORKAROUND is not set
CONFIG_IPC_TASK_STACK_SIZE=1024
CONFIG_ESP32_PHY_CALIBRATION_AND_DATA_STORAGE=y
# CONFIG_ESP32_PHY_CALIBRATION_AND_DATA_STORAGE is not set
# CONFIG_ESP32_PHY_INIT_DATA_IN_PARTITION is not set
CONFIG_ESP32_PHY_MAX_WIFI_TX_POWER=20
CONFIG_ESP32_PHY_MAX_TX_POWER=20
CONFIG_ESP32_REDUCE_PHY_TX_POWER=y
# CONFIG_ESP32_REDUCE_PHY_TX_POWER is not set
# CONFIG_ESP32S2_PANIC_PRINT_HALT is not set
CONFIG_ESP32S2_PANIC_PRINT_REBOOT=y
# CONFIG_ESP32S2_PANIC_SILENT_REBOOT is not set

597
src/adaptation/adapt_map.cpp
View File

@ -1,336 +1,337 @@
#include "adaptation/adapt_map.h"
#include "adapt_map.h"
#include <string.h>
#include <esp_log.h>
#include "nvs.h"
#include "nvs/eeprom_config.h"
#include "esp_check.h"
AdaptationMap::AdaptationMap() {
if(!this->load_from_nvs()) { // Init our map
AdaptationMap::AdaptationMap(void)
{
if (this->load_from_nvs() != ESP_OK)
{ // Init our map
ESP_LOG_LEVEL(ESP_LOG_ERROR, "ADAPT_MAP", "Load from NVS failed, starting a new map");
this->reset();
} else {
}
else
{
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Adapt map loaded!");
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Values for 1->2: Pulling: (%d %d %d), 25%%: (%d %d %d), 50%%: (%d %d %d), 75%%: (%d %d %d)",
this->adapt_data[0].trq_neg.spc_fill_adder,
this->adapt_data[0].trq_neg.mpc_fill_adder,
this->adapt_data[0].trq_neg.fill_time_adder,
this->adapt_data[0].trq_25.spc_fill_adder,
this->adapt_data[0].trq_25.mpc_fill_adder,
this->adapt_data[0].trq_25.fill_time_adder,
this->adapt_data[0].trq_50.spc_fill_adder,
this->adapt_data[0].trq_50.mpc_fill_adder,
this->adapt_data[0].trq_50.fill_time_adder,
this->adapt_data[0].trq_75.spc_fill_adder,
this->adapt_data[0].trq_75.mpc_fill_adder,
this->adapt_data[0].trq_75.fill_time_adder
);
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Values for 2->3: Pulling: (%d %d %d), 25%%: (%d %d %d), 50%%: (%d %d %d), 75%%: (%d %d %d)",
this->adapt_data[1].trq_neg.spc_fill_adder,
this->adapt_data[1].trq_neg.mpc_fill_adder,
this->adapt_data[1].trq_neg.fill_time_adder,
this->adapt_data[1].trq_25.spc_fill_adder,
this->adapt_data[1].trq_25.mpc_fill_adder,
this->adapt_data[1].trq_25.fill_time_adder,
this->adapt_data[1].trq_50.spc_fill_adder,
this->adapt_data[1].trq_50.mpc_fill_adder,
this->adapt_data[1].trq_50.fill_time_adder,
this->adapt_data[1].trq_75.spc_fill_adder,
this->adapt_data[1].trq_75.mpc_fill_adder,
this->adapt_data[1].trq_75.fill_time_adder
);
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Values for 3->4: Pulling: (%d %d %d), 25%%: (%d %d %d), 50%%: (%d %d %d), 75%%: (%d %d %d)",
this->adapt_data[2].trq_neg.spc_fill_adder,
this->adapt_data[2].trq_neg.mpc_fill_adder,
this->adapt_data[2].trq_neg.fill_time_adder,
this->adapt_data[2].trq_25.spc_fill_adder,
this->adapt_data[2].trq_25.mpc_fill_adder,
this->adapt_data[2].trq_25.fill_time_adder,
this->adapt_data[2].trq_50.spc_fill_adder,
this->adapt_data[2].trq_50.mpc_fill_adder,
this->adapt_data[2].trq_50.fill_time_adder,
this->adapt_data[2].trq_75.spc_fill_adder,
this->adapt_data[2].trq_75.mpc_fill_adder,
this->adapt_data[2].trq_75.fill_time_adder
);
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Values for 4->5: Pulling: (%d %d %d), 25%%: (%d %d %d), 50%%: (%d %d %d), 75%%: (%d %d %d)",
this->adapt_data[3].trq_neg.spc_fill_adder,
this->adapt_data[3].trq_neg.mpc_fill_adder,
this->adapt_data[3].trq_neg.fill_time_adder,
this->adapt_data[3].trq_25.spc_fill_adder,
this->adapt_data[3].trq_25.mpc_fill_adder,
this->adapt_data[3].trq_25.fill_time_adder,
this->adapt_data[3].trq_50.spc_fill_adder,
this->adapt_data[3].trq_50.mpc_fill_adder,
this->adapt_data[3].trq_50.fill_time_adder,
this->adapt_data[3].trq_75.spc_fill_adder,
this->adapt_data[3].trq_75.mpc_fill_adder,
this->adapt_data[3].trq_75.fill_time_adder
);
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Values for 2->1: Pulling: (%d %d %d), 25%%: (%d %d %d), 50%%: (%d %d %d), 75%%: (%d %d %d)",
this->adapt_data[4].trq_neg.spc_fill_adder,
this->adapt_data[4].trq_neg.mpc_fill_adder,
this->adapt_data[4].trq_neg.fill_time_adder,
this->adapt_data[4].trq_25.spc_fill_adder,
this->adapt_data[4].trq_25.mpc_fill_adder,
this->adapt_data[4].trq_25.fill_time_adder,
this->adapt_data[4].trq_50.spc_fill_adder,
this->adapt_data[4].trq_50.mpc_fill_adder,
this->adapt_data[4].trq_50.fill_time_adder,
this->adapt_data[4].trq_75.spc_fill_adder,
this->adapt_data[4].trq_75.mpc_fill_adder,
this->adapt_data[4].trq_75.fill_time_adder
);
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Values for 3->2: Pulling: (%d %d %d), 25%%: (%d %d %d), 50%%: (%d %d %d), 75%%: (%d %d %d)",
this->adapt_data[5].trq_neg.spc_fill_adder,
this->adapt_data[5].trq_neg.mpc_fill_adder,
this->adapt_data[5].trq_neg.fill_time_adder,
this->adapt_data[5].trq_25.spc_fill_adder,
this->adapt_data[5].trq_25.mpc_fill_adder,
this->adapt_data[5].trq_25.fill_time_adder,
this->adapt_data[5].trq_50.spc_fill_adder,
this->adapt_data[5].trq_50.mpc_fill_adder,
this->adapt_data[5].trq_50.fill_time_adder,
this->adapt_data[5].trq_75.spc_fill_adder,
this->adapt_data[5].trq_75.mpc_fill_adder,
this->adapt_data[5].trq_75.fill_time_adder
);
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Values for 4->3: Pulling: (%d %d %d), 25%%: (%d %d %d), 50%%: (%d %d %d), 75%%: (%d %d %d)",
this->adapt_data[6].trq_neg.spc_fill_adder,
this->adapt_data[6].trq_neg.mpc_fill_adder,
this->adapt_data[6].trq_neg.fill_time_adder,
this->adapt_data[6].trq_25.spc_fill_adder,
this->adapt_data[6].trq_25.mpc_fill_adder,
this->adapt_data[6].trq_25.fill_time_adder,
this->adapt_data[6].trq_50.spc_fill_adder,
this->adapt_data[6].trq_50.mpc_fill_adder,
this->adapt_data[6].trq_50.fill_time_adder,
this->adapt_data[6].trq_75.spc_fill_adder,
this->adapt_data[6].trq_75.mpc_fill_adder,
this->adapt_data[6].trq_75.fill_time_adder
);
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Values for 5->4: Pulling: (%d %d %d), 25%%: (%d %d %d), 50%%: (%d %d %d), 75%%: (%d %d %d)",
this->adapt_data[7].trq_neg.spc_fill_adder,
this->adapt_data[7].trq_neg.mpc_fill_adder,
this->adapt_data[7].trq_neg.fill_time_adder,
this->adapt_data[7].trq_25.spc_fill_adder,
this->adapt_data[7].trq_25.mpc_fill_adder,
this->adapt_data[7].trq_25.fill_time_adder,
this->adapt_data[7].trq_50.spc_fill_adder,
this->adapt_data[7].trq_50.mpc_fill_adder,
this->adapt_data[7].trq_50.fill_time_adder,
this->adapt_data[7].trq_75.spc_fill_adder,
this->adapt_data[7].trq_75.mpc_fill_adder,
this->adapt_data[7].trq_75.fill_time_adder
);
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Values for 1->2: Pulling: (%d %d %d), 25%%: (%d %d %d), 50%%: (%d %d %d), 75%%: (%d %d %d)",
this->adapt_data[0].trq_neg.spc_fill_adder,
this->adapt_data[0].trq_neg.mpc_fill_adder,
this->adapt_data[0].trq_neg.fill_time_adder,
this->adapt_data[0].trq_25.spc_fill_adder,
this->adapt_data[0].trq_25.mpc_fill_adder,
this->adapt_data[0].trq_25.fill_time_adder,
this->adapt_data[0].trq_50.spc_fill_adder,
this->adapt_data[0].trq_50.mpc_fill_adder,
this->adapt_data[0].trq_50.fill_time_adder,
this->adapt_data[0].trq_75.spc_fill_adder,
this->adapt_data[0].trq_75.mpc_fill_adder,
this->adapt_data[0].trq_75.fill_time_adder);
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Values for 2->3: Pulling: (%d %d %d), 25%%: (%d %d %d), 50%%: (%d %d %d), 75%%: (%d %d %d)",
this->adapt_data[1].trq_neg.spc_fill_adder,
this->adapt_data[1].trq_neg.mpc_fill_adder,
this->adapt_data[1].trq_neg.fill_time_adder,
this->adapt_data[1].trq_25.spc_fill_adder,
this->adapt_data[1].trq_25.mpc_fill_adder,
this->adapt_data[1].trq_25.fill_time_adder,
this->adapt_data[1].trq_50.spc_fill_adder,
this->adapt_data[1].trq_50.mpc_fill_adder,
this->adapt_data[1].trq_50.fill_time_adder,
this->adapt_data[1].trq_75.spc_fill_adder,
this->adapt_data[1].trq_75.mpc_fill_adder,
this->adapt_data[1].trq_75.fill_time_adder);
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Values for 3->4: Pulling: (%d %d %d), 25%%: (%d %d %d), 50%%: (%d %d %d), 75%%: (%d %d %d)",
this->adapt_data[2].trq_neg.spc_fill_adder,
this->adapt_data[2].trq_neg.mpc_fill_adder,
this->adapt_data[2].trq_neg.fill_time_adder,
this->adapt_data[2].trq_25.spc_fill_adder,
this->adapt_data[2].trq_25.mpc_fill_adder,
this->adapt_data[2].trq_25.fill_time_adder,
this->adapt_data[2].trq_50.spc_fill_adder,
this->adapt_data[2].trq_50.mpc_fill_adder,
this->adapt_data[2].trq_50.fill_time_adder,
this->adapt_data[2].trq_75.spc_fill_adder,
this->adapt_data[2].trq_75.mpc_fill_adder,
this->adapt_data[2].trq_75.fill_time_adder);
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Values for 4->5: Pulling: (%d %d %d), 25%%: (%d %d %d), 50%%: (%d %d %d), 75%%: (%d %d %d)",
this->adapt_data[3].trq_neg.spc_fill_adder,
this->adapt_data[3].trq_neg.mpc_fill_adder,
this->adapt_data[3].trq_neg.fill_time_adder,
this->adapt_data[3].trq_25.spc_fill_adder,
this->adapt_data[3].trq_25.mpc_fill_adder,
this->adapt_data[3].trq_25.fill_time_adder,
this->adapt_data[3].trq_50.spc_fill_adder,
this->adapt_data[3].trq_50.mpc_fill_adder,
this->adapt_data[3].trq_50.fill_time_adder,
this->adapt_data[3].trq_75.spc_fill_adder,
this->adapt_data[3].trq_75.mpc_fill_adder,
this->adapt_data[3].trq_75.fill_time_adder);
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Values for 2->1: Pulling: (%d %d %d), 25%%: (%d %d %d), 50%%: (%d %d %d), 75%%: (%d %d %d)",
this->adapt_data[4].trq_neg.spc_fill_adder,
this->adapt_data[4].trq_neg.mpc_fill_adder,
this->adapt_data[4].trq_neg.fill_time_adder,
this->adapt_data[4].trq_25.spc_fill_adder,
this->adapt_data[4].trq_25.mpc_fill_adder,
this->adapt_data[4].trq_25.fill_time_adder,
this->adapt_data[4].trq_50.spc_fill_adder,
this->adapt_data[4].trq_50.mpc_fill_adder,
this->adapt_data[4].trq_50.fill_time_adder,
this->adapt_data[4].trq_75.spc_fill_adder,
this->adapt_data[4].trq_75.mpc_fill_adder,
this->adapt_data[4].trq_75.fill_time_adder);
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Values for 3->2: Pulling: (%d %d %d), 25%%: (%d %d %d), 50%%: (%d %d %d), 75%%: (%d %d %d)",
this->adapt_data[5].trq_neg.spc_fill_adder,
this->adapt_data[5].trq_neg.mpc_fill_adder,
this->adapt_data[5].trq_neg.fill_time_adder,
this->adapt_data[5].trq_25.spc_fill_adder,
this->adapt_data[5].trq_25.mpc_fill_adder,
this->adapt_data[5].trq_25.fill_time_adder,
this->adapt_data[5].trq_50.spc_fill_adder,
this->adapt_data[5].trq_50.mpc_fill_adder,
this->adapt_data[5].trq_50.fill_time_adder,
this->adapt_data[5].trq_75.spc_fill_adder,
this->adapt_data[5].trq_75.mpc_fill_adder,
this->adapt_data[5].trq_75.fill_time_adder);
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Values for 4->3: Pulling: (%d %d %d), 25%%: (%d %d %d), 50%%: (%d %d %d), 75%%: (%d %d %d)",
this->adapt_data[6].trq_neg.spc_fill_adder,
this->adapt_data[6].trq_neg.mpc_fill_adder,
this->adapt_data[6].trq_neg.fill_time_adder,
this->adapt_data[6].trq_25.spc_fill_adder,
this->adapt_data[6].trq_25.mpc_fill_adder,
this->adapt_data[6].trq_25.fill_time_adder,
this->adapt_data[6].trq_50.spc_fill_adder,
this->adapt_data[6].trq_50.mpc_fill_adder,
this->adapt_data[6].trq_50.fill_time_adder,
this->adapt_data[6].trq_75.spc_fill_adder,
this->adapt_data[6].trq_75.mpc_fill_adder,
this->adapt_data[6].trq_75.fill_time_adder);
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Values for 5->4: Pulling: (%d %d %d), 25%%: (%d %d %d), 50%%: (%d %d %d), 75%%: (%d %d %d)",
this->adapt_data[7].trq_neg.spc_fill_adder,
this->adapt_data[7].trq_neg.mpc_fill_adder,
this->adapt_data[7].trq_neg.fill_time_adder,
this->adapt_data[7].trq_25.spc_fill_adder,
this->adapt_data[7].trq_25.mpc_fill_adder,
this->adapt_data[7].trq_25.fill_time_adder,
this->adapt_data[7].trq_50.spc_fill_adder,
this->adapt_data[7].trq_50.mpc_fill_adder,
this->adapt_data[7].trq_50.fill_time_adder,
this->adapt_data[7].trq_75.spc_fill_adder,
this->adapt_data[7].trq_75.mpc_fill_adder,
this->adapt_data[7].trq_75.fill_time_adder);
}
}
void AdaptationMap::reset() {
for (int i = 0; i < 8; i++) {
esp_err_t AdaptationMap::reset(void)
{
for (int i = 0; i < 8; i++)
{
this->adapt_data[i].trq_neg = DEFAULT_CELL;
this->adapt_data[i].trq_25 = DEFAULT_CELL;
this->adapt_data[i].trq_50 = DEFAULT_CELL;
this->adapt_data[i].trq_75 = DEFAULT_CELL;
}
ESP_LOGI("ADAPT_MAP", "Adaptation reset!");
this->save();
return this->save();
}
bool AdaptationMap::load_from_nvs() {
nvs_handle_t config_handle;
esp_err_t err = nvs_open("Configuration", NVS_READWRITE, &config_handle);
if (err != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "ADAPT_MAP", "EEPROM NVS handle failed! %s", esp_err_to_name(err));
return false;
}
esp_err_t AdaptationMap::load_from_nvs(void)
{
esp_err_t result = ESP_OK;
nvs_handle_t config_handle;
ESP_RETURN_ON_ERROR(nvs_open("Configuration", NVS_READWRITE, &config_handle), "ADAPT_MAP", "Failed to open NVS");
size_t map_size = sizeof(this->adapt_data);
err = nvs_get_blob(config_handle, NVS_KEY_GEAR_ADAPTATION, &this->adapt_data, &map_size);
if (err == ESP_ERR_NVS_NOT_FOUND) { // Not found, need to create a new fresh map
result = nvs_get_blob(config_handle, NVS_KEY_GEAR_ADAPTATION, &this->adapt_data, &map_size);
if (result == ESP_ERR_NVS_NOT_FOUND)
{
// Not found, need to create a new fresh map
this->reset(); // Init default
return this->save();
} else {
return (err == ESP_OK);
result = this->save();
}
return result;
}
bool AdaptationMap::save() {
esp_err_t AdaptationMap::save(void)
{
nvs_handle_t handle;
nvs_open("Configuration", NVS_READWRITE, &handle); // Must succeed as we have already opened it!
esp_err_t e = nvs_set_blob(handle, NVS_KEY_GEAR_ADAPTATION, &this->adapt_data, sizeof(this->adapt_data));
if (e != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "ADAPT_MAP", "Error setting AdaptMap blob (%s)", esp_err_to_name(e));
return false;
}
e = nvs_commit(handle);
if (e != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "ADAPT_MAP", "Error calling nvs_commit: %s", esp_err_to_name(e));
return false;
}
return true;
ESP_RETURN_ON_ERROR(nvs_open("Configuration", NVS_READWRITE, &handle), "ADAPT_MAP", "Cannot open config handle");
ESP_RETURN_ON_ERROR(nvs_set_blob(handle, NVS_KEY_GEAR_ADAPTATION, &this->adapt_data, sizeof(this->adapt_data)), "ADAPT_MAP", "Error setting adapt map blob");
return nvs_commit(handle);
}
const AdaptationCell* AdaptationMap::get_adapt_cell(SensorData* sensors, ProfileGearChange change, uint16_t gb_max_torque) {
int adaptation_idx = 0;
switch(change) {
case ProfileGearChange::ONE_TWO:
adaptation_idx = 0;
break;
case ProfileGearChange::TWO_THREE:
adaptation_idx = 1;
break;
case ProfileGearChange::THREE_FOUR:
adaptation_idx = 2;
break;
case ProfileGearChange::FOUR_FIVE:
adaptation_idx = 3;
break;
case ProfileGearChange::TWO_ONE:
adaptation_idx = 4;
break;
case ProfileGearChange::THREE_TWO:
adaptation_idx = 5;
break;
case ProfileGearChange::FOUR_THREE:
adaptation_idx = 6;
break;
case ProfileGearChange::FIVE_FOUR:
adaptation_idx = 7;
break;
default:
return &DEFAULT_CELL; // huh invalid index??
inline AdaptationCell *AdaptationMap::get_adapt_cell_from_torque(SensorData *sensors, uint16_t gb_max_torque, uint16_t adaptation_idx, AdaptationMap *adaptationmap_var)
{
AdaptationCell *result;
// divide by 4 is equivalent to right shift by 2 on unsigned integer variables
uint16_t quarter = gb_max_torque >> 2;
if (0 >= sensors->static_torque)
{
result = &adaptationmap_var->adapt_data[adaptation_idx].trq_neg;
}
uint16_t quarter = (uint16_t)((float)gb_max_torque/4.0);
if (sensors->static_torque <= 0) {
return &this->adapt_data[adaptation_idx].trq_neg;
} else if (sensors->static_torque <= quarter) {
return &this->adapt_data[adaptation_idx].trq_25;
} else if (sensors->static_torque <= quarter*2) {
return &this->adapt_data[adaptation_idx].trq_50;
} else {
return &this->adapt_data[adaptation_idx].trq_75;
else if (sensors->static_torque <= quarter)
{
result = &adaptationmap_var->adapt_data[adaptation_idx].trq_25;
}
else if (((uint16_t)sensors->static_torque) <= (quarter * 2u))
{
result = &adaptationmap_var->adapt_data[adaptation_idx].trq_50;
}
else if (((uint16_t)sensors->static_torque) <= (quarter * 3u))
{
result = &adaptationmap_var->adapt_data[adaptation_idx].trq_75;
}
else {
result = nullptr;
}
return result;
}
void AdaptationMap::perform_adaptation(SensorData* sensors, ShiftReport* rpt, ProfileGearChange change, bool is_valid_rpt, uint16_t gb_max_torque) {
inline uint16_t AdaptationMap::get_idx_from_change(ProfileGearChange change)
{
// initialization for invalid index
uint16_t result = 0xFFFFu;
switch (change)
{
case ProfileGearChange::ONE_TWO:
result = 0;
break;
case ProfileGearChange::TWO_THREE:
result = 1u;
break;
case ProfileGearChange::THREE_FOUR:
result = 2u;
break;
case ProfileGearChange::FOUR_FIVE:
result = 3u;
break;
case ProfileGearChange::TWO_ONE:
result = 4u;
break;
case ProfileGearChange::THREE_TWO:
result = 5u;
break;
case ProfileGearChange::FOUR_THREE:
result = 6u;
break;
case ProfileGearChange::FIVE_FOUR:
result = 7u;
break;
default:
break;
}
return result;
}
AdaptationCell *AdaptationMap::get_adapt_cell(SensorData *sensors, ProfileGearChange change, uint16_t gb_max_torque)
{
uint16_t adaptation_idx = AdaptationMap::get_idx_from_change(change);
return (7u >= adaptation_idx) ? this->get_adapt_cell_from_torque(sensors, gb_max_torque, adaptation_idx, this) : const_cast<AdaptationCell *>(&DEFAULT_CELL);
}
void AdaptationMap::perform_adaptation(SensorData *sensors, ShiftReport *rpt, ProfileGearChange change, bool is_valid_rpt, uint16_t gb_max_torque)
{
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Adapting called");
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT MAP",
"Start at %d: \n"
"End at %d\n",
rpt->transition_start, rpt->transition_end);
if (!is_valid_rpt) {
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP","Not adapting as shift was not measurable");
return;
}
if (sensors->engine_rpm > ADAPT_RPM_LIMIT) {
ESP_LOG_LEVEL(ESP_LOG_WARN, "ADAPT_MAP", "Cannot adapt. Engine RPM outside limits. Got %d RPM, must be below %d RPM",
sensors->engine_rpm,
ADAPT_RPM_LIMIT
);
return;
}
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT MAP",
"Start at %d: \n"
"End at %d\n",
rpt->transition_start, rpt->transition_end);
if (is_valid_rpt)
{
if (sensors->engine_rpm <= ADAPT_RPM_LIMIT)
{
if ((sensors->atf_temp >= ADAPT_TEMP_THRESH) && (sensors->atf_temp <= ADAPT_TEMP_LIMIT))
{
AdaptationCell *dest = this->get_adapt_cell(sensors, change, gb_max_torque);
if (&DEFAULT_CELL != dest)
{
// Calc when the transitions started
int start_fill = rpt->bleed_data.hold_time + rpt->bleed_data.ramp_time;
int start_overlap = start_fill + rpt->fill_data.hold_time + rpt->fill_data.ramp_time;
int start_max_p = start_overlap + rpt->overlap_data.hold_time + rpt->overlap_data.ramp_time;
if (sensors->atf_temp < ADAPT_TEMP_THRESH || sensors->atf_temp > ADAPT_TEMP_LIMIT) {
ESP_LOG_LEVEL(ESP_LOG_WARN, "ADAPT_MAP", "Cannot adapt. ATF temp outside limits. ATF at %d C, must be between %d and %d C",
sensors->atf_temp,
ADAPT_TEMP_THRESH,
ADAPT_TEMP_LIMIT
);
return; // Too cold/hot to adapt
}
int adaptation_idx = 0;
switch(change) {
case ProfileGearChange::ONE_TWO:
adaptation_idx = 0;
break;
case ProfileGearChange::TWO_THREE:
adaptation_idx = 1;
break;
case ProfileGearChange::THREE_FOUR:
adaptation_idx = 2;
break;
case ProfileGearChange::FOUR_FIVE:
adaptation_idx = 3;
break;
case ProfileGearChange::TWO_ONE:
adaptation_idx = 4;
break;
case ProfileGearChange::THREE_TWO:
adaptation_idx = 5;
break;
case ProfileGearChange::FOUR_THREE:
adaptation_idx = 6;
break;
case ProfileGearChange::FIVE_FOUR:
adaptation_idx = 7;
break;
default:
return; // huh??
}
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Adapting...");
bool accel_shift = sensors->d_output_rpm > 0;
bool idle_shift = sensors->static_torque > 0;
AdaptationCell* dest;
float q_trq = (float)gb_max_torque/4.0;
if (sensors->static_torque <= 0) {
dest = &this->adapt_data[adaptation_idx].trq_neg;
} else if (sensors->static_torque <= q_trq) {
dest = &this->adapt_data[adaptation_idx].trq_25;
} else if (sensors->static_torque <= q_trq*2) {
dest = &this->adapt_data[adaptation_idx].trq_25;
} else if (sensors->static_torque <= q_trq*3) {
dest = &this->adapt_data[adaptation_idx].trq_25;
} else {
ESP_LOG_LEVEL(ESP_LOG_WARN, "ADAPT_MAP", "Cannot adapt. Torque outside limit. Got %d Nm, Max allowed: %d Nm",
sensors->static_torque,
(uint16_t)q_trq*3
);
return;
}
// Calc when the transitions started
int start_fill = rpt->bleed_data.hold_time+rpt->bleed_data.ramp_time;
int start_overlap = start_fill + rpt->fill_data.hold_time+rpt->fill_data.ramp_time;
int start_max_p = start_overlap + rpt->overlap_data.hold_time+rpt->overlap_data.ramp_time;
int shift_time = rpt->transition_end - rpt->transition_start;
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Shift took %d ms", shift_time);
if (rpt->transition_start < start_fill) {
ESP_LOGW("ADAPT_MAP", "Shift started BEFORE FILL!");
if (rpt->flare_timestamp != 0) {
dest->mpc_fill_adder += 20;
} else {
dest->fill_time_adder += 10;
int shift_time = rpt->transition_end - rpt->transition_start;
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Shift took %d ms", shift_time);
if (rpt->transition_start < start_fill)
{
ESP_LOGW("ADAPT_MAP", "Shift started BEFORE FILL!");
if (rpt->flare_timestamp != 0)
{
dest->mpc_fill_adder += 20;
}
else
{
dest->fill_time_adder += 10;
}
}
else if (rpt->transition_start < start_overlap)
{
ESP_LOGW("ADAPT_MAP", "Shift started BEFORE overlap phase! (%d ms into fill phase)", rpt->transition_start - start_fill);
}
else if (rpt->transition_start < start_max_p)
{
ESP_LOGW("ADAPT_MAP", "Shift started in overlap phase! (%d ms into overlap phase)", rpt->transition_start - start_overlap);
}
else if (rpt->transition_start > start_max_p)
{
ESP_LOGW("ADAPT_MAP", "Shift started after overlap phase! (%d ms into max pressure phase)", rpt->transition_start - start_max_p);
dest->spc_fill_adder += 20;
}
else
{
}
if (0 != rpt->flare_timestamp)
{
if (rpt->flare_timestamp < start_fill)
{
ESP_LOGW("ADAPT_MAP", "Flare detected BEFORE FILL!");
}
else if (rpt->flare_timestamp < start_overlap)
{
ESP_LOGW("ADAPT_MAP", "Flare detected %d ms into fill phase", rpt->flare_timestamp - start_fill);
}
else if (rpt->flare_timestamp < start_max_p)
{
ESP_LOGW("ADAPT_MAP", "Flare detected %d ms into overlap phase", rpt->flare_timestamp - start_overlap);
}
else if (rpt->flare_timestamp > start_max_p)
{
ESP_LOGW("ADAPT_MAP", "Flare detected %d ms into max pressure phase", rpt->flare_timestamp - start_max_p);
}
else
{
}
}
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Values for target cell are (S %d mBar, M %d mBar) and %d ms",
dest->spc_fill_adder,
dest->mpc_fill_adder,
dest->fill_time_adder);
}
else
{
ESP_LOG_LEVEL(ESP_LOG_WARN, "ADAPT_MAP", "Cannot adapt. Torque outside limit. Got %d Nm, Max allowed: %d Nm",
sensors->static_torque,
((gb_max_torque >> 2) * 3u));
}
}
else
{
// Too cold/hot to adapt
ESP_LOG_LEVEL(ESP_LOG_WARN, "ADAPT_MAP", "Cannot adapt. ATF temp outside limits. ATF at %d C, must be between %d and %d C",
sensors->atf_temp,
ADAPT_TEMP_THRESH,
ADAPT_TEMP_LIMIT);
}
}
} else if (rpt->transition_start < start_overlap) {
ESP_LOGW("ADAPT_MAP", "Shift started BEFORE overlap phase! (%d ms into fill phase)", rpt->transition_start-start_fill);
} else if (rpt->transition_start < start_max_p) {
ESP_LOGW("ADAPT_MAP", "Shift started in overlap phase! (%d ms into overlap phase)", rpt->transition_start-start_overlap);
} else if (rpt->transition_start > start_max_p) {
ESP_LOGW("ADAPT_MAP", "Shift started after overlap phase! (%d ms into max pressure phase)", rpt->transition_start-start_max_p);
dest->spc_fill_adder += 20;
}
if (rpt->flare_timestamp != 0) {
if (rpt->flare_timestamp < start_fill) {
ESP_LOGW("ADAPT_MAP", "Flare detected BEFORE FILL!");
} else if (rpt->flare_timestamp < start_overlap) {
ESP_LOGW("ADAPT_MAP", "Flare detected %d ms into fill phase", rpt->flare_timestamp-start_fill);
} else if (rpt->flare_timestamp < start_max_p) {
ESP_LOGW("ADAPT_MAP", "Flare detected %d ms into overlap phase", rpt->flare_timestamp-start_overlap);
} else if (rpt->flare_timestamp > start_max_p) {
ESP_LOGW("ADAPT_MAP", "Flare detected %d ms into max pressure phase", rpt->flare_timestamp-start_max_p);
else
{
ESP_LOG_LEVEL(ESP_LOG_WARN, "ADAPT_MAP", "Cannot adapt. Engine RPM outside limits. Got %d RPM, must be below %d RPM",
sensors->engine_rpm,
ADAPT_RPM_LIMIT);
}
}
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Values for target cell are (S %d mBar, M %d mBar) and %d ms",
dest->spc_fill_adder,
dest->mpc_fill_adder,
dest->fill_time_adder
);
else
{
ESP_LOG_LEVEL(ESP_LOG_INFO, "ADAPT_MAP", "Not adapting as shift was not measurable");
}
}

34
src/adaptation/adapt_map.h
View File

@ -1,21 +1,10 @@
#ifndef __ADAPT_MAP_H__
#define __ADAPT_MAP_H__
#ifndef ADAPT_MAP_H
#define ADAPT_MAP_H
#include <stdint.h>
#include <gearbox_config.h>
#include "common_structs.h"
// Adaptation is only performed if engine is under 2500RPM
// and Engine output torque is as defined:
#ifdef LARGE_NAG
#define ADAPT_TORQUE_LIMIT 250
#else
#define ADAPT_TORQUE_LIMIT 150
#endif
#define ADAPT_RPM_LIMIT 2500
#define ADAPT_TEMP_THRESH 60
#define ADAPT_TEMP_LIMIT 120
#include "esp_err.h"
struct AdaptationCell {
int16_t spc_fill_adder;
@ -39,16 +28,21 @@ const static AdaptationCell DEFAULT_CELL {
// For now, we just do what EGS52 does
class AdaptationMap {
public:
AdaptationMap();
AdaptationMap(void);
// Reset map to everything default
void reset();
bool save();
esp_err_t reset(void);
esp_err_t save(void);
void perform_adaptation(SensorData* sensors, ShiftReport* rpt, ProfileGearChange change, bool is_valid_rpt, uint16_t gb_max_torque);
const AdaptationCell* get_adapt_cell(SensorData* sensors, ProfileGearChange change, uint16_t gb_max_torque);
AdaptationCell* get_adapt_cell(SensorData* sensors, ProfileGearChange change, uint16_t gb_max_torque);
private:
const uint16_t ADAPT_RPM_LIMIT = 2500u;
const int16_t ADAPT_TEMP_THRESH = 60;
const int16_t ADAPT_TEMP_LIMIT = 120;
inline static AdaptationCell* get_adapt_cell_from_torque(SensorData *sensors, uint16_t gb_max_torque, uint16_t adaptation_idx, AdaptationMap* adaptationmap_var);
inline static uint16_t get_idx_from_change(ProfileGearChange change);
AdaptationData adapt_data[8];
bool load_from_nvs();
esp_err_t load_from_nvs(void);
};
#endif // __ADAPT_MAP_H__
#endif // ADAPT_MAP_H

10
src/adaptation/shift_report.cpp
View File

@ -9,7 +9,7 @@
uint16_t calc_shift_report_group_crc(ShiftReportNvsGroup* rpt) {
size_t byte_count = sizeof(ShiftReportNvsGroup) - 2; // CRC MUST be at the end
uint16_t total = 0;
uint8_t* ptr = (uint8_t*)(rpt);
uint8_t* ptr = reinterpret_cast<uint8_t*>(rpt);
for (int i = 0; i < byte_count; i++) {
total += ptr[i] + i;
}
@ -40,8 +40,8 @@ bool save_shift_report_to_partition(ShiftReportNvsGroup* src, uint32_t addr) {
}
ShiftReporter::ShiftReporter() {
this->report_group = (ShiftReportNvsGroup*)heap_caps_malloc(sizeof(ShiftReportNvsGroup), MALLOC_CAP_SPIRAM);
ShiftReporter::ShiftReporter(void) {
this->report_group = static_cast<ShiftReportNvsGroup*>(heap_caps_malloc(sizeof(ShiftReportNvsGroup), MALLOC_CAP_SPIRAM));
if (this->report_group == nullptr) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "SHIFT_REPORT", "Cannot allocate memory!");
return;
@ -61,7 +61,7 @@ ShiftReporter::ShiftReporter() {
}
}
ShiftReporter::~ShiftReporter() {
ShiftReporter::~ShiftReporter(void) {
}
void ShiftReporter::add_report(ShiftReport src) {
@ -80,7 +80,7 @@ void ShiftReporter::add_report(ShiftReport src) {
}
void ShiftReporter::save() {
void ShiftReporter::save(void) {
if (!has_change || this->report_group == nullptr || this->spiffs_addr == 0) {
return;
}

20
src/adaptation/shift_report.h
View File

@ -1,28 +1,28 @@
#ifndef __SHIFT_REPORT_H__
#define __SHIFT_REPORT_H__
#ifndef SHIFT_REPORT_H
#define SHIFT_REPORT_H
#include "common_structs.h"
#include "nvs.h"
#define MAX_REPORTS 10
static const uint16_t MAX_REPORTS = 10u;
typedef struct {
struct __attribute__ ((packed)) ShiftReportNvsGroup{
ShiftReport reports[MAX_REPORTS];
uint8_t index;
uint16_t crc;
} __attribute__ ((packed)) ShiftReportNvsGroup;
};
// 0x7D000 from partitions.csv (tcm_shift_store) partition
static_assert(sizeof(ShiftReportNvsGroup) < 0x7D000, "ShiftReportNvsGroup cannot fit into designated partition!");
static_assert(sizeof(ShiftReportNvsGroup) < 0x7D000u, "ShiftReportNvsGroup cannot fit into designated partition!");
class ShiftReporter {
public:
ShiftReporter();
~ShiftReporter();
ShiftReporter(void);
~ShiftReporter(void);
void add_report(ShiftReport src);
void save();
void save(void);
ShiftReportNvsGroup diag_get_nvs_group_ptr() { return *this->report_group; }
private:
bool has_change = false;
@ -31,4 +31,4 @@ private:
};
#endif // __SHIFT_REPORT_H__
#endif // SHIFT_REPORT_H

12
src/behaviour/data.h
View File

@ -15,7 +15,7 @@ enum class DriveStyle {
Grandma,
};
typedef struct {
struct DrivingData{
// Current speed in KMH
int current_vehicle_speed_kmh;
// Pedal acceleration (%/sec)
@ -35,13 +35,9 @@ typedef struct {
// Based on current deceleration of the car, how many seconds until we stop?
// If INT32_MAX is returned, that is because we are accelerating (Not possible to stop)
int num_seconds_to_stop() {
if (vehicle_acceleration >= 0) {
return __INT32_MAX__;
} else {
return (current_vehicle_speed_kmh/vehicle_acceleration) * -1;
}
int num_seconds_to_stop(void) {
return (vehicle_acceleration >= 0) ? __INT32_MAX__ : (current_vehicle_speed_kmh/vehicle_acceleration) * -1;
}
} DrivingData;
};
#endif

112
src/board_config.cpp Normal file
View File

@ -0,0 +1,112 @@
#include "board_config.h"
#include "esp_log.h"
BoardV11GpioMatrix::BoardV11GpioMatrix() {
ESP_LOGI("GPIO_MATRIX", "GPIO Matrix version 1.1 (12/12/21)");
this->io_pin = gpio_num_t::GPIO_NUM_NC; // Not available on this board version
this->i2c_sda = gpio_num_t::GPIO_NUM_NC; // Not available on this board version
this->i2c_scl = gpio_num_t::GPIO_NUM_NC; // Not available on this board version
this->can_tx_pin = gpio_num_t::GPIO_NUM_5; // CAN TWAI Tx
this->can_rx_pin = gpio_num_t::GPIO_NUM_4; // CAN TWAI Rx
this->spkr_pin = gpio_num_t::GPIO_NUM_15; // Piezo speaker
this->vsense_pin = gpio_num_t::GPIO_NUM_25; // Battery voltage feedback
this->atf_pin = gpio_num_t::GPIO_NUM_26; // ATF temp sensor and lockout
this->n3_pin = gpio_num_t::GPIO_NUM_27; // N3 speed sensor
this->n2_pin = gpio_num_t::GPIO_NUM_14; // N2 speed sensor
this->y3_sense = gpio_num_t::GPIO_NUM_36; // Y3 (1-2/4-5) shift solenoid (Current feedback)
this->y3_pwm = gpio_num_t::GPIO_NUM_23; // Y3 (1-2/4-5) shift solenoid (PWM output)
this->y4_sense = gpio_num_t::GPIO_NUM_39; // Y4 (3-4) shift solenoid (Current feedback)
this->y4_pwm = gpio_num_t::GPIO_NUM_22; // Y4 (3-4) shift solenoid (PWM output)
this->y5_sense = gpio_num_t::GPIO_NUM_35; // Y5 (2-3) shift solenoid (Current feedback)
this->y5_pwm = gpio_num_t::GPIO_NUM_19; // Y5 (2-3) shift solenoid (PWM output)
this->mpc_sense = gpio_num_t::GPIO_NUM_34; // Modulating pressure solenoid (Current feedback)
this->mpc_pwm = gpio_num_t::GPIO_NUM_21; // Modulating pressure solenoid (PWM output)
this->spc_sense = gpio_num_t::GPIO_NUM_32; // Shift pressure solenoid (Current feedback)
this->spc_pwm = gpio_num_t::GPIO_NUM_12; // Shift pressure solenoid (PWM output)
this->tcc_sense = gpio_num_t::GPIO_NUM_33; // Torque converter solenoid(Current feedback)
this->tcc_pwm = gpio_num_t::GPIO_NUM_13; // Torque converter solenoid (PWM output)
#define ADC_CHANNEL_VBATT_V12 adc2_channel_t::ADC2_CHANNEL_8
#define ADC_CHANNEL_ATF_V12 adc2_channel_t::ADC2_CHANNEL_7
this->sensor_data = SensorFuncData {
.batt_channel = adc2_channel_t::ADC2_CHANNEL_8,
.atf_channel = adc2_channel_t::ADC2_CHANNEL_9,
.adc_batt = adc_channel_t::ADC_CHANNEL_8,
.adc_atf = adc_channel_t::ADC_CHANNEL_9,
.atf_calibration_curve = atf_temp_lookup_V11,
.current_sense_multi = 2.0,
};
}
BoardV12GpioMatrix::BoardV12GpioMatrix() {
ESP_LOGI("GPIO_MATRIX", "GPIO Matrix version 1.2 (07/07/22)");
this->io_pin = gpio_num_t::GPIO_NUM_NC; // Not available on this board version
this->can_tx_pin = gpio_num_t::GPIO_NUM_5; // CAN TWAI Tx
this->can_rx_pin = gpio_num_t::GPIO_NUM_18; // CAN TWAI Rx
this->spkr_pin = gpio_num_t::GPIO_NUM_4; // Piezo speaker
this->vsense_pin = gpio_num_t::GPIO_NUM_25; // Battery voltage feedback
this->atf_pin = gpio_num_t::GPIO_NUM_27; // ATF temp sensor and lockout
this->n3_pin = gpio_num_t::GPIO_NUM_14; // N3 speed sensor
this->n2_pin = gpio_num_t::GPIO_NUM_26; // N2 speed sensor
this->y3_sense = gpio_num_t::GPIO_NUM_36; // Y3 (1-2/4-5) shift solenoid (Current feedback)
this->y3_pwm = gpio_num_t::GPIO_NUM_23; // Y3 (1-2/4-5) shift solenoid (PWM output)
this->y4_sense = gpio_num_t::GPIO_NUM_39; // Y4 (3-4) shift solenoid (Current feedback)
this->y4_pwm = gpio_num_t::GPIO_NUM_22; // Y4 (3-4) shift solenoid (PWM output)
this->y5_sense = gpio_num_t::GPIO_NUM_35; // Y5 (2-3) shift solenoid (Current feedback)
this->y5_pwm = gpio_num_t::GPIO_NUM_19; // Y5 (2-3) shift solenoid (PWM output)
this->mpc_sense = gpio_num_t::GPIO_NUM_34; // Modulating pressure solenoid (Current feedback)
this->mpc_pwm = gpio_num_t::GPIO_NUM_21; // Modulating pressure solenoid (PWM output)
this->spc_sense = gpio_num_t::GPIO_NUM_32; // Shift pressure solenoid (Current feedback)
this->spc_pwm = gpio_num_t::GPIO_NUM_12; // Shift pressure solenoid (PWM output)
this->tcc_sense = gpio_num_t::GPIO_NUM_33; // Torque converter solenoid(Current feedback)
this->tcc_pwm = gpio_num_t::GPIO_NUM_13; // Torque converter solenoid (PWM output)
this->i2c_sda = gpio_num_t::GPIO_NUM_15; // I2C clock
this->i2c_scl = gpio_num_t::GPIO_NUM_2; // I2C data
this->sensor_data = SensorFuncData {
.batt_channel = adc2_channel_t::ADC2_CHANNEL_8,
.atf_channel = adc2_channel_t::ADC2_CHANNEL_7,
.adc_batt = adc_channel_t::ADC_CHANNEL_8,
.adc_atf = adc_channel_t::ADC_CHANNEL_7,
.atf_calibration_curve = atf_temp_lookup_V12,
.current_sense_multi = 1.0,
};
}
BoardV13GpioMatrix::BoardV13GpioMatrix() {
ESP_LOGI("GPIO_MATRIX", "GPIO Matrix version 1.3 (29/11/22)");
this->io_pin = gpio_num_t::GPIO_NUM_4;
this->can_tx_pin = gpio_num_t::GPIO_NUM_5; // CAN TWAI Tx
this->can_rx_pin = gpio_num_t::GPIO_NUM_18; // CAN TWAI Rx
this->spkr_pin = gpio_num_t::GPIO_NUM_0; // Piezo speaker
this->vsense_pin = gpio_num_t::GPIO_NUM_25; // Battery voltage feedback
this->atf_pin = gpio_num_t::GPIO_NUM_27; // ATF temp sensor and lockout
this->n3_pin = gpio_num_t::GPIO_NUM_14; // N3 speed sensor
this->n2_pin = gpio_num_t::GPIO_NUM_26; // N2 speed sensor
this->y3_sense = gpio_num_t::GPIO_NUM_36; // Y3 (1-2/4-5) shift solenoid (Current feedback)
this->y3_pwm = gpio_num_t::GPIO_NUM_23; // Y3 (1-2/4-5) shift solenoid (PWM output)
this->y4_sense = gpio_num_t::GPIO_NUM_39; // Y4 (3-4) shift solenoid (Current feedback)
this->y4_pwm = gpio_num_t::GPIO_NUM_22; // Y4 (3-4) shift solenoid (PWM output)
this->y5_sense = gpio_num_t::GPIO_NUM_35; // Y5 (2-3) shift solenoid (Current feedback)
this->y5_pwm = gpio_num_t::GPIO_NUM_19; // Y5 (2-3) shift solenoid (PWM output)
this->mpc_sense = gpio_num_t::GPIO_NUM_34; // Modulating pressure solenoid (Current feedback)
this->mpc_pwm = gpio_num_t::GPIO_NUM_21; // Modulating pressure solenoid (PWM output)
this->spc_sense = gpio_num_t::GPIO_NUM_32; // Shift pressure solenoid (Current feedback)
this->spc_pwm = gpio_num_t::GPIO_NUM_12; // Shift pressure solenoid (PWM output)
this->tcc_sense = gpio_num_t::GPIO_NUM_33; // Torque converter solenoid(Current feedback)
this->tcc_pwm = gpio_num_t::GPIO_NUM_13; // Torque converter solenoid (PWM output)
this->i2c_sda = gpio_num_t::GPIO_NUM_15; // I2C clock
this->i2c_scl = gpio_num_t::GPIO_NUM_2; // I2C data
this->sensor_data = SensorFuncData {
.batt_channel = adc2_channel_t::ADC2_CHANNEL_8,
.atf_channel = adc2_channel_t::ADC2_CHANNEL_7,
.adc_batt = adc_channel_t::ADC_CHANNEL_8,
.adc_atf = adc_channel_t::ADC_CHANNEL_7,
.atf_calibration_curve = atf_temp_lookup_V12,
.current_sense_multi = 1.0,
};
}
BoardGpioMatrix* pcb_gpio_matrix = nullptr;

151
src/board_config.h Normal file
View File

@ -0,0 +1,151 @@
#ifndef PINS_H
#define PINS_H
#include <driver/gpio.h>
#include <driver/adc.h>
static const uint16_t NUM_TEMP_POINTS = 22u;
struct temp_reading_t{
// Voltage in mV
uint16_t v;
// ATF Temp in degrees C * 10
int temp;
};
const static temp_reading_t atf_temp_lookup_V12[NUM_TEMP_POINTS] = {
// mV, Temp(x10)
// mV Values are calibrated on 3.45V rail
// as that is how much the ATF sensor power gets
{725, -400},
{784, -300},
{842, -200},
{900, -100},
{957, 0},
{1013, 100},
{1068, 200},
{1123, 300},
{1177, 400},
{1230, 500},
{1281, 600},
{1332, 700},
{1384, 800},
{1438, 900},
{1488, 1000},
{1538, 1100},
{1587, 1200},
{1636, 1300},
{1685, 1400},
{1732, 1500},
{1779, 1600},
{1826, 1700}
};
const static temp_reading_t atf_temp_lookup_V11[NUM_TEMP_POINTS] = {
// mV, Temp(x10)
// mV Values are calibrated on 3.45V rail
// as that is how much the ATF sensor power gets
{446, -400},
{461, -300},
{476, -200},
{491, -100},
{507, 0},
{523, 100},
{540, 200},
{557, 300},
{574, 400},
{592, 500},
{610, 600},
{629, 700},
{648, 800},
{669, 900},
{690, 1000},
{711, 1100},
{732, 1200},
{755, 1300},
{778, 1400},
{802, 1500},
{814, 1600},
{851, 1700}
};
typedef struct {
adc2_channel_t batt_channel;
adc2_channel_t atf_channel;
adc_channel_t adc_batt;
adc_channel_t adc_atf;
const temp_reading_t* atf_calibration_curve;
float current_sense_multi;
} SensorFuncData;
class BoardGpioMatrix {
public:
gpio_num_t can_tx_pin;
gpio_num_t can_rx_pin;
gpio_num_t spkr_pin;
gpio_num_t vsense_pin;
gpio_num_t atf_pin;
gpio_num_t n3_pin;
gpio_num_t n2_pin;
gpio_num_t io_pin; // Only on 1.3 and newer
gpio_num_t y3_sense;
gpio_num_t y3_pwm;
gpio_num_t y4_sense;
gpio_num_t y4_pwm;
gpio_num_t y5_sense;
gpio_num_t y5_pwm;
gpio_num_t mpc_sense;
gpio_num_t mpc_pwm;
gpio_num_t spc_sense;
gpio_num_t spc_pwm;
gpio_num_t tcc_sense;
gpio_num_t tcc_pwm;
gpio_num_t i2c_sda; // Only on 1.2 and newer
gpio_num_t i2c_scl; // Only on 1.2 and newer
SensorFuncData sensor_data;
};
/**
* @brief GPIO Matrix for the Red beta PCB (V1.1 12/12/21)
*
* NOTE: This will be removed from the code base once all beta boards have been replaced
*
*/
class BoardV11GpioMatrix: public BoardGpioMatrix {
public:
BoardV11GpioMatrix(void);
};
/**
* @brief GPIO Matrix for the Gen 1 production PCB (V1.2 07/07/22)
*
*/
class BoardV12GpioMatrix: public BoardGpioMatrix {
public:
BoardV12GpioMatrix(void);
};
/**
* @brief GPIO Matrix for the Gen 2 production PCB (V1.3 29/11/22)
* NOTE: This PCB is in development, so its matrix can change
*/
class BoardV13GpioMatrix: public BoardGpioMatrix {
public:
BoardV13GpioMatrix(void);
};
extern BoardGpioMatrix* pcb_gpio_matrix;
#endif

463
src/canbus/can_egs51.cpp
View File

@ -1,134 +1,95 @@
#include "can_egs51.h"
#ifdef EGS51_MODE
#ifndef BOARD_V2
#error "EGS51 CAN Support is only supported on V2 PCBs!"
#endif
#define IO_ADDR 0x20
#include "driver/twai.h"
#include "pins.h"
#include "gearbox_config.h"
#include "driver/i2c.h"
#include "board_config.h"
#include "nvs/eeprom_config.h"
Egs51Can::Egs51Can(const char* name, uint8_t tx_time_ms)
: AbstractCan(name, tx_time_ms)
{
// Firstly try to init CAN
ESP_LOG_LEVEL(ESP_LOG_INFO, "EGS51_CAN", "CAN constructor called");
twai_general_config_t gen_config = TWAI_GENERAL_CONFIG_DEFAULT(PIN_CAN_TX, PIN_CAN_RX, TWAI_MODE_NORMAL);
gen_config.intr_flags = ESP_INTR_FLAG_IRAM; // Set TWAI interrupt to IRAM (Enabled in menuconfig)!
gen_config.rx_queue_len = 32;
gen_config.tx_queue_len = 32;
twai_timing_config_t timing_config = TWAI_TIMING_CONFIG_500KBITS();
twai_filter_config_t filter_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
typedef struct {
bool a;
bool b;
bool c;
bool d;
ShifterPosition pos;
} TRRSPos;
esp_err_t res;
res = twai_driver_install(&gen_config, &timing_config, &filter_config);
if (res != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EGS51_CAN", "TWAI_DRIVER_INSTALL FAILED!: %s", esp_err_to_name(res));
const static TRRSPos TRRS_SHIFTER_TABLE[8] = {
TRRSPos { .a = 1, .b = 1, .c = 1, .d = 0, .pos = ShifterPosition::P },
TRRSPos { .a = 0, .b = 1, .c = 1, .d = 1, .pos = ShifterPosition::R },
TRRSPos { .a = 1, .b = 0, .c = 1, .d = 1, .pos = ShifterPosition::N },
TRRSPos { .a = 0, .b = 0, .c = 1, .d = 0, .pos = ShifterPosition::D },
TRRSPos { .a = 0, .b = 0, .c = 0, .d = 1, .pos = ShifterPosition::FOUR },
TRRSPos { .a = 0, .b = 1, .c = 0, .d = 0, .pos = ShifterPosition::THREE },
TRRSPos { .a = 1, .b = 0, .c = 0, .d = 0, .pos = ShifterPosition::TWO },
TRRSPos { .a = 1, .b = 1, .c = 0, .d = 1, .pos = ShifterPosition::ONE },
};
Egs51Can::Egs51Can(const char* name, uint8_t tx_time_ms, uint32_t baud) : EgsBaseCan(name, tx_time_ms, baud) {
ESP_LOGI("EGS51", "SETUP CALLED");
if (pcb_gpio_matrix->i2c_sda == gpio_num_t::GPIO_NUM_NC || pcb_gpio_matrix->i2c_scl == gpio_num_t::GPIO_NUM_NC) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EGS51_CAN", "Cannot launch TRRS on board without I2C!");
this->can_init_status = ESP_ERR_INVALID_VERSION;
}
res = twai_start();
if (res != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EGS51_CAN", "TWAI_START FAILED!: %s", esp_err_to_name(res));
}
// Init TRRS sensors
i2c_config_t conf = {
.mode = I2C_MODE_MASTER,
.sda_io_num = PIN_I2C_SDA,
.scl_io_num = PIN_I2C_SCL,
.sda_pullup_en = GPIO_PULLUP_ENABLE,
.scl_pullup_en = GPIO_PULLUP_ENABLE,
};
conf.master.clk_speed = 400000;
res = i2c_driver_install(I2C_NUM_0, i2c_mode_t::I2C_MODE_MASTER, 0, 0, 0);
if (res != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "I2C", "Failed to install driver %s", esp_err_to_name(res));
return;
}
res = i2c_param_config(I2C_NUM_0, &conf);
if (res != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "I2C", "Failed to set param config %s", esp_err_to_name(res));
return;
if (this->can_init_status == ESP_OK) {
// Init TRRS sensors
i2c_config_t conf = {
.mode = I2C_MODE_MASTER,
.sda_io_num = pcb_gpio_matrix->i2c_sda,
.scl_io_num = pcb_gpio_matrix->i2c_scl,
.sda_pullup_en = GPIO_PULLUP_ENABLE,
.scl_pullup_en = GPIO_PULLUP_ENABLE,
};
conf.master.clk_speed = 400000;
this->can_init_status = i2c_driver_install(I2C_NUM_0, i2c_mode_t::I2C_MODE_MASTER, 0, 0, 0);
if (this->can_init_status == ESP_OK) {
this->can_init_status = i2c_param_config(I2C_NUM_0, &conf);
if (this->can_init_status != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, this->name, "Failed to set param config");
}
} else {
ESP_LOG_LEVEL(ESP_LOG_ERROR, this->name, "Failed to install driver");
}
}
this->start_enable = true;
this->gs218.set_TORQUE_REQ(0xFE);
this->gs218.bytes[7] = 0xFE;
this->gs218.bytes[4] = 0x48;
this->gs218.bytes[3] = 0x64;
// CAN is OK!
this->can_init_ok = true;
}
bool Egs51Can::begin_tasks() {
if (!this->can_init_ok) { // Cannot init tasks if CAN is dead!
return false;
}
// Prevent starting again
if (this->rx_task == nullptr) {
ESP_LOG_LEVEL(ESP_LOG_INFO, "EGS51_CAN", "Starting CAN Rx task");
if (xTaskCreate(this->start_rx_task_loop, "EGS51_CAN_RX", 8192, this, 5, this->rx_task) != pdPASS) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EGS51_CAN", "CAN Rx task creation failed!");
return false;
}
}
if (this->tx_task == nullptr) {
ESP_LOG_LEVEL(ESP_LOG_INFO, "EGS51_CAN", "Starting CAN Tx task");
if (xTaskCreate(this->start_tx_task_loop, "EGS51_CAN_TX", 4096, this, 5, this->tx_task) != pdPASS) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EGS51_CAN", "CAN Tx task creation failed!");
return false;
}
}
return true; // Ready!
}
Egs51Can::~Egs51Can()
{
if (this->rx_task != nullptr) {
vTaskDelete(this->rx_task);
}
if (this->tx_task != nullptr) {
vTaskDelete(this->tx_task);
}
// Delete CAN
if (this->can_init_ok) {
twai_stop();
twai_driver_uninstall();
}
}
WheelData Egs51Can::get_front_right_wheel(uint64_t now, uint64_t expire_time_ms) { // TODO
return WheelData {
.double_rpm = 0,
.current_dir = WheelDirection::SignalNotAvaliable
.current_dir = WheelDirection::SignalNotAvailable
};
}
WheelData Egs51Can::get_front_left_wheel(uint64_t now, uint64_t expire_time_ms) { // TODO
return WheelData {
.double_rpm = 0,
.current_dir = WheelDirection::SignalNotAvaliable
.current_dir = WheelDirection::SignalNotAvailable
};
}
WheelData Egs51Can::get_rear_right_wheel(uint64_t now, uint64_t expire_time_ms) {
BS_208 bs208;
BS_208EGS51 bs208;
if (this->esp51.get_BS_208(now, expire_time_ms, &bs208)) {
WheelDirection d = WheelDirection::SignalNotAvaliable;
WheelDirection d = WheelDirection::SignalNotAvailable;
switch(bs208.get_DRTGHR()) {
case BS_208h_DRTGHR::FWD:
case BS_208h_DRTGHREGS51::FWD:
d = WheelDirection::Forward;
break;
case BS_208h_DRTGHR::REV:
case BS_208h_DRTGHREGS51::REV:
d = WheelDirection::Reverse;
break;
case BS_208h_DRTGHR::PASSIVE:
case BS_208h_DRTGHREGS51::PASSIVE:
d = WheelDirection::Stationary;
break;
case BS_208h_DRTGHR::SNV:
case BS_208h_DRTGHREGS51::SNV:
default:
break;
}
@ -140,26 +101,26 @@ WheelData Egs51Can::get_rear_right_wheel(uint64_t now, uint64_t expire_time_ms)
} else {
return WheelData {
.double_rpm = 0,
.current_dir = WheelDirection::SignalNotAvaliable
.current_dir = WheelDirection::SignalNotAvailable
};
}
}
WheelData Egs51Can::get_rear_left_wheel(uint64_t now, uint64_t expire_time_ms) {
BS_208 bs208;
BS_208EGS51 bs208;
if (this->esp51.get_BS_208(now, expire_time_ms, &bs208)) {
WheelDirection d = WheelDirection::SignalNotAvaliable;
WheelDirection d = WheelDirection::SignalNotAvailable;
switch(bs208.get_DRTGHL()) {
case BS_208h_DRTGHL::FWD:
case BS_208h_DRTGHLEGS51::FWD:
d = WheelDirection::Forward;
break;
case BS_208h_DRTGHL::REV:
case BS_208h_DRTGHLEGS51::REV:
d = WheelDirection::Reverse;
break;
case BS_208h_DRTGHL::PASSIVE:
case BS_208h_DRTGHLEGS51::PASSIVE:
d = WheelDirection::Stationary;
break;
case BS_208h_DRTGHL::SNV:
case BS_208h_DRTGHLEGS51::SNV:
default:
break;
}
@ -171,59 +132,90 @@ WheelData Egs51Can::get_rear_left_wheel(uint64_t now, uint64_t expire_time_ms) {
} else {
return WheelData {
.double_rpm = 0,
.current_dir = WheelDirection::SignalNotAvaliable
.current_dir = WheelDirection::SignalNotAvailable
};
}
}
ShifterPosition Egs51Can::get_shifter_position_ewm(uint64_t now, uint64_t expire_time_ms) {
if (now - this->last_i2c_query_time > expire_time_ms) {
return ShifterPosition::SignalNotAvaliable;
}
// Data is valid time range!
uint8_t tmp = this->i2c_rx_bytes[0];
bool TRRS_A = (tmp & (uint8_t)BIT(5)) != 0;
bool TRRS_B = (tmp & (uint8_t)BIT(6)) != 0;
bool TRRS_C = (tmp & (uint8_t)BIT(3)) != 0;
bool TRRS_D = (tmp & (uint8_t)BIT(4)) != 0;
if (TRRS_A && TRRS_B && TRRS_C && !TRRS_D) {
this->last_valid_position = ShifterPosition::P;
return ShifterPosition::P;
} else if (!TRRS_A && TRRS_B && TRRS_C && TRRS_D) {
this->last_valid_position = ShifterPosition::R;
return ShifterPosition::R;
} else if (TRRS_A && !TRRS_B && TRRS_C && TRRS_D) {
this->last_valid_position = ShifterPosition::N;
return ShifterPosition::N;
} else if (!TRRS_A && !TRRS_B && TRRS_C && !TRRS_D) {
this->last_valid_position = ShifterPosition::D;
return ShifterPosition::D;
} else if (!TRRS_A && !TRRS_B && !TRRS_C && TRRS_D) {
this->last_valid_position = ShifterPosition::FOUR;
return ShifterPosition::FOUR;
} else if (!TRRS_A && TRRS_B && !TRRS_C && !TRRS_D) {
this->last_valid_position = ShifterPosition::THREE;
return ShifterPosition::THREE;
} else if (TRRS_A && !TRRS_B && !TRRS_C && !TRRS_D) {
this->last_valid_position = ShifterPosition::TWO;
return ShifterPosition::TWO;
} else if (TRRS_A && TRRS_B && !TRRS_C && TRRS_D) {
this->last_valid_position = ShifterPosition::ONE;
return ShifterPosition::ONE;
} else if (!TRRS_A && !TRRS_B && !TRRS_C && !TRRS_D) { // Intermediate position, now work out which one
if (this->last_valid_position == ShifterPosition::P) {
return ShifterPosition::P_R;
} else if (this->last_valid_position == ShifterPosition::R) {
return ShifterPosition::R_N;
} else if (this->last_valid_position == ShifterPosition::D || this->last_valid_position == ShifterPosition::N) {
return ShifterPosition::N_D;
} else {
return ShifterPosition::SignalNotAvaliable; // invalid combination
ShifterPosition ret = ShifterPosition::SignalNotAvailable;
if (VEHICLE_CONFIG.shifter_style == SHIFTER_STYLE_EWM) {
EWM_230 dest;
if (this->ewm.get_EWM_230(now, expire_time_ms, &dest)) {
switch (dest.get_WHC()) {
case EWM_230h_WHC::D:
ret = ShifterPosition::D;
break;
case EWM_230h_WHC::N:
ret = ShifterPosition::N;
break;
case EWM_230h_WHC::R:
ret = ShifterPosition::R;
break;
case EWM_230h_WHC::P:
ret = ShifterPosition::P;
break;
case EWM_230h_WHC::PLUS:
ret = ShifterPosition::PLUS;
break;
case EWM_230h_WHC::MINUS:
ret = ShifterPosition::MINUS;
break;
case EWM_230h_WHC::N_ZW_D:
ret = ShifterPosition::N_D;
break;
case EWM_230h_WHC::R_ZW_N:
ret = ShifterPosition::R_N;
break;
case EWM_230h_WHC::P_ZW_R:
ret = ShifterPosition::P_R;
break;
case EWM_230h_WHC::SNV:
default:
break;
}
}
} else {
return ShifterPosition::SignalNotAvaliable;
if (now - this->last_i2c_query_time < expire_time_ms) {
// Data is valid time range!
uint8_t tmp = this->i2c_rx_bytes[0];
bool TRRS_A;
bool TRRS_B;
bool TRRS_C;
bool TRRS_D;
if (BOARD_CONFIG.board_ver == 2) { // V1.2 layout
TRRS_A = (tmp & (uint8_t)BIT(5)) != 0;
TRRS_B = (tmp & (uint8_t)BIT(6)) != 0;
TRRS_C = (tmp & (uint8_t)BIT(3)) != 0;
TRRS_D = (tmp & (uint8_t)BIT(4)) != 0;
} else { // V1.3+ layout
TRRS_A = (tmp & (uint8_t)BIT(5)) != 0;
TRRS_B = (tmp & (uint8_t)BIT(6)) != 0;
TRRS_C = (tmp & (uint8_t)BIT(4)) != 0;
TRRS_D = (tmp & (uint8_t)BIT(3)) != 0;
}
if (!TRRS_A && !TRRS_B && !TRRS_C && !TRRS_D) { // Intermediate position, now work out which one
if (this->last_valid_position == ShifterPosition::P) {
ret = ShifterPosition::P_R;
} else if (this->last_valid_position == ShifterPosition::R) {
ret = ShifterPosition::R_N;
} else if (this->last_valid_position == ShifterPosition::D || this->last_valid_position == ShifterPosition::N) {
ret = ShifterPosition::N_D;
}
} else {
// Check truth table
for (uint8_t i = 0; i < 8; i++) {
TRRSPos pos = TRRS_SHIFTER_TABLE[i];
if (pos.a == TRRS_A && pos.b == TRRS_B && pos.c == TRRS_C && pos.d == TRRS_D) {
ret = pos.pos;
break;
}
}
}
}
}
return ret;
}
EngineType Egs51Can::get_engine_type(uint64_t now, uint64_t expire_time_ms) {
@ -239,7 +231,7 @@ bool Egs51Can::get_kickdown(uint64_t now, uint64_t expire_time_ms) { // TODO
}
uint8_t Egs51Can::get_pedal_value(uint64_t now, uint64_t expire_time_ms) { // TODO
MS_210 ms210;
MS_210EGS51 ms210;
if (this->ms51.get_MS_210(now, expire_time_ms, &ms210)) {
return ms210.get_PW();
} else {
@ -248,21 +240,25 @@ uint8_t Egs51Can::get_pedal_value(uint64_t now, uint64_t expire_time_ms) { // TO
}
int Egs51Can::get_static_engine_torque(uint64_t now, uint64_t expire_time_ms) { // TODO
MS_310 ms310;
MS_310EGS51 ms310;
if (this->ms51.get_MS_310(now, expire_time_ms, &ms310)) {
return (int)ms310.get_STA_TORQUE()*2;
} else {
return 0xFF;
return INT_MAX;
}
return INT_MAX;
}
int Egs51Can::get_driver_engine_torque(uint64_t now, uint64_t expire_time_ms) {
return this->get_static_engine_torque(now, expire_time_ms);
}
int Egs51Can::get_maximum_engine_torque(uint64_t now, uint64_t expire_time_ms) { // TODO
MS_310 ms310;
MS_310EGS51 ms310;
if (this->ms51.get_MS_310(now, expire_time_ms, &ms310)) {
return (int)ms310.get_MAX_TORQUE()*2;
} else {
return 0xFF;
return INT_MAX;
}
return INT_MAX;
}
@ -276,7 +272,7 @@ PaddlePosition Egs51Can::get_paddle_position(uint64_t now, uint64_t expire_time_
}
int16_t Egs51Can::get_engine_coolant_temp(uint64_t now, uint64_t expire_time_ms) {
MS_608 ms608;
MS_608EGS51 ms608;
if (this->ms51.get_MS_608(now, expire_time_ms, &ms608)) {
return ms608.get_T_MOT() - 40;
} else {
@ -285,7 +281,7 @@ int16_t Egs51Can::get_engine_coolant_temp(uint64_t now, uint64_t expire_time_ms)
}
int16_t Egs51Can::get_engine_oil_temp(uint64_t now, uint64_t expire_time_ms) { // TODO
MS_308 ms308;
MS_308EGS51 ms308;
if (this->ms51.get_MS_308(now, expire_time_ms, &ms308)) {
return ms308.get_T_OEL() - 40;
} else {
@ -294,7 +290,7 @@ int16_t Egs51Can::get_engine_oil_temp(uint64_t now, uint64_t expire_time_ms) { /
}
uint16_t Egs51Can::get_engine_rpm(uint64_t now, uint64_t expire_time_ms) {
MS_308 ms308;
MS_308EGS51 ms308;
if (this->ms51.get_MS_308(now, expire_time_ms, &ms308)) {
return ms308.get_NMOT();
} else {
@ -321,35 +317,35 @@ void Egs51Can::set_clutch_status(ClutchStatus status) {
void Egs51Can::set_actual_gear(GearboxGear actual) {
switch(actual) {
case GearboxGear::First:
this->gs218.set_GIC(GS_218h_GIC::G_D1);
this->gs218.set_GIC(GS_218h_GICEGS51::G_D1);
break;
case GearboxGear::Second:
this->gs218.set_GIC(GS_218h_GIC::G_D2);
this->gs218.set_GIC(GS_218h_GICEGS51::G_D2);
break;
case GearboxGear::Third:
this->gs218.set_GIC(GS_218h_GIC::G_D3);
this->gs218.set_GIC(GS_218h_GICEGS51::G_D3);
break;
case GearboxGear::Fourth:
this->gs218.set_GIC(GS_218h_GIC::G_D4);
this->gs218.set_GIC(GS_218h_GICEGS51::G_D4);
break;
case GearboxGear::Fifth:
this->gs218.set_GIC(GS_218h_GIC::G_D5);
this->gs218.set_GIC(GS_218h_GICEGS51::G_D5);
break;
case GearboxGear::Park:
this->gs218.set_GIC(GS_218h_GIC::G_P);
this->gs218.set_GIC(GS_218h_GICEGS51::G_P);
break;
case GearboxGear::Neutral:
this->gs218.set_GIC(GS_218h_GIC::G_N);
this->gs218.set_GIC(GS_218h_GICEGS51::G_N);
break;
case GearboxGear::Reverse_First:
this->gs218.set_GIC(GS_218h_GIC::G_R);
this->gs218.set_GIC(GS_218h_GICEGS51::G_R);
break;
case GearboxGear::Reverse_Second:
this->gs218.set_GIC(GS_218h_GIC::G_R2);
this->gs218.set_GIC(GS_218h_GICEGS51::G_R2);
break;
case GearboxGear::SignalNotAvaliable:
case GearboxGear::SignalNotAvailable:
default:
this->gs218.set_GIC(GS_218h_GIC::G_SNV);
this->gs218.set_GIC(GS_218h_GICEGS51::G_SNV);
break;
}
}
@ -357,35 +353,35 @@ void Egs51Can::set_actual_gear(GearboxGear actual) {
void Egs51Can::set_target_gear(GearboxGear target) {
switch(target) {
case GearboxGear::First:
this->gs218.set_GZC(GS_218h_GZC::G_D1);
this->gs218.set_GZC(GS_218h_GZCEGS51::G_D1);
break;
case GearboxGear::Second:
this->gs218.set_GZC(GS_218h_GZC::G_D2);
this->gs218.set_GZC(GS_218h_GZCEGS51::G_D2);
break;
case GearboxGear::Third:
this->gs218.set_GZC(GS_218h_GZC::G_D3);
this->gs218.set_GZC(GS_218h_GZCEGS51::G_D3);
break;
case GearboxGear::Fourth:
this->gs218.set_GZC(GS_218h_GZC::G_D4);
this->gs218.set_GZC(GS_218h_GZCEGS51::G_D4);
break;
case GearboxGear::Fifth:
this->gs218.set_GZC(GS_218h_GZC::G_D5);
this->gs218.set_GZC(GS_218h_GZCEGS51::G_D5);
break;
case GearboxGear::Park:
this->gs218.set_GZC(GS_218h_GZC::G_P);
this->gs218.set_GZC(GS_218h_GZCEGS51::G_P);
break;
case GearboxGear::Neutral:
this->gs218.set_GZC(GS_218h_GZC::G_N);
this->gs218.set_GZC(GS_218h_GZCEGS51::G_N);
break;
case GearboxGear::Reverse_First:
this->gs218.set_GZC(GS_218h_GZC::G_R);
this->gs218.set_GZC(GS_218h_GZCEGS51::G_R);
break;
case GearboxGear::Reverse_Second:
this->gs218.set_GZC(GS_218h_GZC::G_R2);
this->gs218.set_GZC(GS_218h_GZCEGS51::G_R2);
break;
case GearboxGear::SignalNotAvaliable:
case GearboxGear::SignalNotAvailable:
default:
this->gs218.set_GZC(GS_218h_GZC::G_SNV);
this->gs218.set_GZC(GS_218h_GZCEGS51::G_SNV);
break;
}
}
@ -469,89 +465,51 @@ inline bool calc_torque_parity(uint16_t s) {
return (p & 1) == 1;
}
inline void to_bytes(uint64_t src, uint8_t* dst) {
for(uint8_t i = 0; i < 8; i++) {
dst[7-i] = src & 0xFF;
src >>= 8;
}
}
/**
* @brief void tx_frames() override;
void on_rx_frame(uint32_t id, uint8_t dlc, uint64_t data, uint64_t timestamp) override;
void on_rx_done(uint64_t now_ts) override;
*
*/
[[noreturn]]
void Egs51Can::tx_task_loop() {
void Egs51Can::tx_frames() {
twai_message_t tx;
tx.data_length_code = 8; // Always
GS_218 gs_218tx;
uint8_t cvn_counter = 0;
bool toggle = false;
bool time_to_toggle = false;
while(true) {
// Copy current CAN frame values to here so we don't
// accidentally modify parity calculations
gs_218tx = {gs218.raw};
GS_218EGS51 gs_218tx;
// Copy current CAN frame values to here so we don't
// accidentally modify parity calculations
gs_218tx = {gs218.raw};
// Firstly we have to deal with toggled bits!
// As toggle bits need to be toggled every 40ms,
// and egs52 Tx interval is 20ms,
// we can achieve this with 2 booleans
//gs_218tx.set_MTGL_EGS(toggle);
// Now do parity calculations
//gs_218tx.set_MPAR_EGS(calc_torque_parity(gs_218tx.raw >> 48));
if (time_to_toggle) {
toggle = !toggle;
}
time_to_toggle = !time_to_toggle;
// Now set CVN Counter (Increases every frame)
gs_218tx.set_FEHLER(cvn_counter);
cvn_counter++;
// Firstly we have to deal with toggled bits!
// As toggle bits need to be toggled every 40ms,
// and egs52 Tx interval is 20ms,
// we can achieve this with 2 booleans
//gs_218tx.set_MTGL_EGS(toggle);
// Now do parity calculations
//gs_218tx.set_MPAR_EGS(calc_torque_parity(gs_218tx.raw >> 48));
if (time_to_toggle) {
toggle = !toggle;
}
time_to_toggle = !time_to_toggle;
// Now set CVN Counter (Increases every frame)
gs_218tx.set_FEHLER(cvn_counter);
cvn_counter++;
tx.identifier = GS_218EGS51_CAN_ID;
tx.data_length_code = 6;
to_bytes(gs_218tx.raw, tx.data);
twai_transmit(&tx, 5);
}
if (!this->send_messages) {
vTaskDelay(50);
continue;
}
// Now send CAN Data!
vTaskDelay(1 / portTICK_PERIOD_MS);
tx.identifier = GS_218_CAN_ID;
tx.data_length_code = 6;
to_bytes(gs_218tx.raw, tx.data);
twai_transmit(&tx, 5);
vTaskDelay(this->tx_time_ms / portTICK_PERIOD_MS);
void Egs51Can::on_rx_frame(uint32_t id, uint8_t dlc, uint64_t data, uint64_t timestamp) {
if (this->ms51.import_frames(data, id, timestamp)) {
} else if (this->esp51.import_frames(data, id, timestamp)) {
} else if (this->ewm.import_frames(data, id, timestamp)) {
}
}
[[noreturn]]
void Egs51Can::rx_task_loop() {
twai_message_t rx;
twai_status_info_t can_status;
uint64_t now = 0;
uint64_t tmp;
uint8_t i;
this->last_i2c_query_time = 0;
while(true) {
now = (esp_timer_get_time()/1000);
twai_get_status_info(&can_status);
if (can_status.msgs_to_rx == 0) {
vTaskDelay(4 / portTICK_PERIOD_MS); // Wait for buffer to have at least 1 frame
} else { // We have frames, read them
if (now > 2) {
now -= 2;
}
for(uint8_t x = 0; x < can_status.msgs_to_rx; x++) { // Read all frames
if (twai_receive(&rx, pdMS_TO_TICKS(0)) == ESP_OK && rx.data_length_code != 0) {
tmp = 0;
for(i = 0; i < rx.data_length_code; i++) {
tmp |= (uint64_t)rx.data[i] << (8*(7-i));
}
if (this->ms51.import_frames(tmp, rx.identifier, now)) {
} else if (this->esp51.import_frames(tmp, rx.identifier, now)) {
}
}
}
vTaskDelay(2 / portTICK_PERIOD_MS); // Reset watchdog here
}
if (now - this->last_i2c_query_time > 50) {
void Egs51Can::on_rx_done(uint64_t now_ts) {
if (now_ts - this->last_i2c_query_time > 50) {
// Query I2C IO Expander
uint8_t req[2] = {0,0};
esp_err_t e = i2c_master_write_read_device(I2C_NUM_0, IO_ADDR, req, 1, this->i2c_rx_bytes, 2, 5);
@ -560,7 +518,7 @@ void Egs51Can::rx_task_loop() {
ESP_LOGE("LS", "Could not query I2C: %s", esp_err_to_name(e));
} else {
//ESP_LOGI("EGS51_CAN", "I2C Reponse %02X %02X", this->i2c_rx_bytes[0], this->i2c_rx_bytes[1]);
this->last_i2c_query_time = now;
this->last_i2c_query_time = now_ts;
}
// Set RP and Start pins on IO expander to be outputs
@ -587,7 +545,4 @@ void Egs51Can::rx_task_loop() {
}
*/
}
}
}
#endif
}

34
src/canbus/can_egs51.h
View File

@ -1,18 +1,15 @@
#ifndef __EGS51_CAN_H_
#define __EGS51_CAN_H_
#include <gearbox_config.h>
#ifdef EGS51_MODE
#include "can_hal.h"
#include "GS51.h"
#include "MS51.h"
#include "ESP51.h"
#include "../../egs51_ecus/src/GS51.h"
#include "../../egs51_ecus/src/MS51.h"
#include "../../egs51_ecus/src/ESP51.h"
#include "../../egs52_ecus/src/EWM.h"
class Egs51Can: public AbstractCan {
class Egs51Can: public EgsBaseCan {
public:
explicit Egs51Can(const char* name, uint8_t tx_time_ms);
bool begin_tasks() override;
~Egs51Can();
explicit Egs51Can(const char* name, uint8_t tx_time_ms, uint32_t baud);
/**
* Getters
@ -38,6 +35,7 @@ class Egs51Can: public AbstractCan {
uint8_t get_pedal_value(uint64_t now, uint64_t expire_time_ms) override;
// Gets the current 'static' torque produced by the engine
int get_static_engine_torque(uint64_t now, uint64_t expire_time_ms) override;
int get_driver_engine_torque(uint64_t now, uint64_t expire_time_ms) override;
// Gets the maximum engine torque allowed at this moment by the engine map
int get_maximum_engine_torque(uint64_t now, uint64_t expire_time_ms) override;
// Gets the minimum engine torque allowed at this moment by the engine map
@ -95,24 +93,24 @@ class Egs51Can: public AbstractCan {
void set_display_msg(GearboxMessage msg) override;
void set_wheel_torque_multi_factor(float ratio) override;
protected:
[[noreturn]]
void tx_task_loop() override;
[[noreturn]]
void rx_task_loop() override;
void tx_frames() override;
void on_rx_frame(uint32_t id, uint8_t dlc, uint64_t data, uint64_t timestamp) override;
void on_rx_done(uint64_t now_ts) override;
private:
// CAN Frames to Tx
GS_218 gs218 = {0};
GS_218EGS51 gs218 = {0};
ECU_MS51 ms51 = ECU_MS51();
ECU_EWM ewm = ECU_EWM();
ECU_ESP51 esp51 = ECU_ESP51();
ShifterPosition last_valid_position = ShifterPosition::SignalNotAvaliable;
ShifterPosition last_valid_position = ShifterPosition::SignalNotAvailable;
uint8_t i2c_rx_bytes[2] = {0,0};
uint8_t i2c_tx_bytes[2] = {0,0};
uint64_t last_i2c_query_time = 0;
bool start_enable = false;
bool rp_lock_enage = false;
bool can_init_ok = false;
bool toggle = false;
bool time_to_toggle = false;
uint8_t cvn_counter = 0;
};
#endif // EGS53_MODE
#endif // EGS52_CAN_H_

383
src/canbus/can_egs52.cpp
View File

@ -1,37 +1,14 @@
#include "can_egs52.h"
#include "driver/twai.h"
#include "pins.h"
#include "gearbox_config.h"
#include "board_config.h"
#include "nvs/eeprom_config.h"
#ifdef EGS52_MODE
Egs52Can::Egs52Can(const char* name, uint8_t tx_time_ms)
: AbstractCan(name, tx_time_ms)
{
// Firstly try to init CAN
ESP_LOG_LEVEL(ESP_LOG_INFO, "EGS52_CAN", "CAN constructor called");
twai_general_config_t gen_config = TWAI_GENERAL_CONFIG_DEFAULT(PIN_CAN_TX, PIN_CAN_RX, TWAI_MODE_NORMAL);
gen_config.intr_flags = ESP_INTR_FLAG_IRAM; // Set TWAI interrupt to IRAM (Enabled in menuconfig)!
gen_config.rx_queue_len = 32;
gen_config.tx_queue_len = 32;
twai_timing_config_t timing_config = TWAI_TIMING_CONFIG_500KBITS();
twai_filter_config_t filter_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
esp_err_t res;
res = twai_driver_install(&gen_config, &timing_config, &filter_config);
if (res != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EGS52_CAN", "TWAI_DRIVER_INSTALL FAILED!: %s", esp_err_to_name(res));
}
res = twai_start();
if (res != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EGS52_CAN", "TWAI_START FAILED!: %s", esp_err_to_name(res));
}
// CAN is OK!
Egs52Can::Egs52Can(const char* name, uint8_t tx_time_ms, uint32_t baud) : EgsBaseCan(name, tx_time_ms, baud) {
// Set default values
this->set_target_gear(GearboxGear::SignalNotAvaliable);
this->set_actual_gear(GearboxGear::SignalNotAvaliable);
this->set_shifter_position(ShifterPosition::SignalNotAvaliable);
this->set_target_gear(GearboxGear::SignalNotAvailable);
this->set_actual_gear(GearboxGear::SignalNotAvailable);
this->set_shifter_position(ShifterPosition::SignalNotAvailable);
this->gs218.set_GIC(GS_218h_GIC::G_SNV);
gs218.set_CALID_CVN_AKT(true);
gs218.set_G_G(true);
@ -42,79 +19,44 @@ Egs52Can::Egs52Can(const char* name, uint8_t tx_time_ms)
this->gs218.set_SCHALT(true);
this->gs218.set_MKRIECH(0xFF);
// Set permanent configuration frame
#ifdef FOUR_MATIC
this->gs418.set_ALLRAD(true);
#else
this->gs418.set_ALLRAD(false);
#endif
if (VEHICLE_CONFIG.is_four_matic != 0) {
this->gs418.set_ALLRAD(true);
} else {
this->gs418.set_ALLRAD(false);
}
this->gs418.set_FRONT(false); // Primary rear wheel drive
this->gs418.set_CVT(false); // Not CVT gearbox
this->gs418.set_MECH(GS_418h_MECH::GROSS); // Small 722.6 for now! (TODO Handle 580)
this->gs418.set_CVT(false); // Not CVT gearbox]
if (VEHICLE_CONFIG.is_large_nag != 0) {
this->gs418.set_MECH(GS_418h_MECH::GROSS);
} else {
this->gs418.set_MECH(GS_418h_MECH::KLEIN);
}
this->gs218.set_ALF(true); // Fix for KG systems where cranking would stop when TCU turns on
// Covers setting NAB, a couple unknown but static values,
// and Input RPM to 0
gs338.raw = 0xFFFF1FFF00FF0000;
this->can_init_ok = true;
}
bool Egs52Can::begin_tasks() {
if (!this->can_init_ok) { // Cannot init tasks if CAN is dead!
return false;
}
// Prevent starting again
if (this->rx_task == nullptr) {
ESP_LOG_LEVEL(ESP_LOG_INFO, "EGS52_CAN", "Starting CAN Rx task");
if (xTaskCreate(this->start_rx_task_loop, "EGS52_CAN_RX", 8192, this, 5, this->rx_task) != pdPASS) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EGS52_CAN", "CAN Rx task creation failed!");
return false;
}
}
if (this->tx_task == nullptr) {
ESP_LOG_LEVEL(ESP_LOG_INFO, "EGS52_CAN", "Starting CAN Tx task");
if (xTaskCreate(this->start_tx_task_loop, "EGS52_CAN_TX", 4096, this, 5, this->tx_task) != pdPASS) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EGS52_CAN", "CAN Tx task creation failed!");
return false;
}
}
return true; // Ready!
}
Egs52Can::~Egs52Can()
{
if (this->rx_task != nullptr) {
vTaskDelete(this->rx_task);
}
if (this->tx_task != nullptr) {
vTaskDelete(this->tx_task);
}
// Delete CAN
if (this->can_init_ok) {
twai_stop();
twai_driver_uninstall();
}
gs338.raw = 0xFFFF1FFF00FF0000u;
}
WheelData Egs52Can::get_front_right_wheel(uint64_t now, uint64_t expire_time_ms) { // TODO
return WheelData {
.double_rpm = 0,
.current_dir = WheelDirection::SignalNotAvaliable
.current_dir = WheelDirection::SignalNotAvailable
};
}
WheelData Egs52Can::get_front_left_wheel(uint64_t now, uint64_t expire_time_ms) { // TODO
return WheelData {
.double_rpm = 0,
.current_dir = WheelDirection::SignalNotAvaliable
.current_dir = WheelDirection::SignalNotAvailable
};
}
WheelData Egs52Can::get_rear_right_wheel(uint64_t now, uint64_t expire_time_ms) {
BS_208 bs208;
if (this->esp_ecu.get_BS_208(now, expire_time_ms, &bs208)) {
WheelDirection d = WheelDirection::SignalNotAvaliable;
WheelDirection d = WheelDirection::SignalNotAvailable;
switch(bs208.get_DRTGHR()) {
case BS_208h_DRTGHR::FWD:
d = WheelDirection::Forward;
@ -137,7 +79,7 @@ WheelData Egs52Can::get_rear_right_wheel(uint64_t now, uint64_t expire_time_ms)
} else {
return WheelData {
.double_rpm = 0,
.current_dir = WheelDirection::SignalNotAvaliable
.current_dir = WheelDirection::SignalNotAvailable
};
}
}
@ -145,7 +87,7 @@ WheelData Egs52Can::get_rear_right_wheel(uint64_t now, uint64_t expire_time_ms)
WheelData Egs52Can::get_rear_left_wheel(uint64_t now, uint64_t expire_time_ms) {
BS_208 bs208;
if (this->esp_ecu.get_BS_208(now, expire_time_ms, &bs208)) {
WheelDirection d = WheelDirection::SignalNotAvaliable;
WheelDirection d = WheelDirection::SignalNotAvailable;
switch(bs208.get_DRTGHL()) {
case BS_208h_DRTGHL::FWD:
d = WheelDirection::Forward;
@ -168,7 +110,7 @@ WheelData Egs52Can::get_rear_left_wheel(uint64_t now, uint64_t expire_time_ms) {
} else {
return WheelData {
.double_rpm = 0,
.current_dir = WheelDirection::SignalNotAvaliable
.current_dir = WheelDirection::SignalNotAvailable
};
}
}
@ -197,10 +139,10 @@ ShifterPosition Egs52Can::get_shifter_position_ewm(uint64_t now, uint64_t expire
return ShifterPosition::P_R;
case EWM_230h_WHC::SNV:
default:
return ShifterPosition::SignalNotAvaliable;
return ShifterPosition::SignalNotAvailable;
}
} else {
return ShifterPosition::SignalNotAvaliable;
return ShifterPosition::SignalNotAvailable;
}
}
@ -247,6 +189,14 @@ int Egs52Can::get_static_engine_torque(uint64_t now, uint64_t expire_time_ms) {
return INT_MAX;
}
int Egs52Can::get_driver_engine_torque(uint64_t now, uint64_t expire_time_ms) {
MS_212 ms212;
if (this->ecu_ms.get_MS_212(now, expire_time_ms, &ms212)) {
return ((int)ms212.get_M_FV() / 4) - 500;
}
return INT_MAX;
}
int Egs52Can::get_maximum_engine_torque(uint64_t now, uint64_t expire_time_ms) { // TODO
MS_312 ms312;
if (this->ecu_ms.get_MS_312(now, expire_time_ms, &ms312)) {
@ -344,14 +294,14 @@ bool Egs52Can::get_profile_btn_press(uint64_t now, uint64_t expire_time_ms) {
}
}
TerminalStatus Egs52Can::get_terminal_15(uint64_t now, uint64_t expire_time_ms) {
EZS_240 ezs240;
if (this->ezs_ecu.get_EZS_240(now, expire_time_ms, &ezs240)) {
return ezs240.get_KL_15() ? TerminalStatus::On : TerminalStatus::Off;
} else {
return TerminalStatus::SNA;
}
}
// TerminalStatus Egs52Can::get_terminal_15(uint64_t now, uint64_t expire_time_ms) {
// EZS_240 ezs240;
// if (this->ezs_ecu.get_EZS_240(now, expire_time_ms, &ezs240)) {
// return ezs240.get_KL_15() ? TerminalStatus::On : TerminalStatus::Off;
// } else {
// return TerminalStatus::SNA;
// }
// }
uint16_t Egs52Can::get_fuel_flow_rate(uint64_t now, uint64_t expire_time_ms) {
MS_608 ms608;
@ -362,6 +312,27 @@ uint16_t Egs52Can::get_fuel_flow_rate(uint64_t now, uint64_t expire_time_ms) {
}
}
TransferCaseState Egs52Can::get_transfer_case_state(uint64_t now, uint64_t expire_time_ms) {
VG_428 vg428;
if (this->misc_ecu.get_VG_428(now, expire_time_ms, &vg428)) {
switch (vg428.get_VG_GANG()) {
case VG_428h_VG_GANG::HI:
return TransferCaseState::Hi;
case VG_428h_VG_GANG::LO:
return TransferCaseState::Low;
case VG_428h_VG_GANG::N:
return TransferCaseState::Neither;
case VG_428h_VG_GANG::SH_IPG:
return TransferCaseState::Switching;
case VG_428h_VG_GANG::SNV:
default:
return TransferCaseState::SNA;
}
} else {
return TransferCaseState::SNA;
}
}
void Egs52Can::set_clutch_status(ClutchStatus status) {
switch(status) {
case ClutchStatus::Open:
@ -422,7 +393,7 @@ void Egs52Can::set_actual_gear(GearboxGear actual) {
this->gs418.set_GIC(GS_418h_GIC::G_D5);
this->gs218.set_GIC(GS_218h_GIC::G_D5);
break;
case GearboxGear::SignalNotAvaliable:
case GearboxGear::SignalNotAvailable:
default:
this->gs418.set_GIC(GS_418h_GIC::G_SNV);
this->gs218.set_GIC(GS_218h_GIC::G_SNV);
@ -468,7 +439,7 @@ void Egs52Can::set_target_gear(GearboxGear target) {
this->gs418.set_GZC(GS_418h_GZC::G_D5);
this->gs218.set_GZC(GS_218h_GZC::G_D5);
break;
case GearboxGear::SignalNotAvaliable:
case GearboxGear::SignalNotAvailable:
default:
this->gs418.set_GZC(GS_418h_GZC::G_SNV);
this->gs218.set_GZC(GS_218h_GZC::G_SNV);
@ -517,7 +488,7 @@ void Egs52Can::set_shifter_position(ShifterPosition pos) {
case ShifterPosition::N_D:
gs418.set_WHST(GS_418h_WHST::D);
break;
case ShifterPosition::SignalNotAvaliable:
case ShifterPosition::SignalNotAvailable:
gs418.set_WHST(GS_418h_WHST::SNV);
break;
default:
@ -532,31 +503,39 @@ void Egs52Can::set_gearbox_ok(bool is_ok) {
}
void Egs52Can::set_torque_request(TorqueRequest request) {
this->current_req = request;
switch(request) {
case TorqueRequest::Decrease:
case TorqueRequest::Begin:
gs218.set_MMIN_EGS(true);
gs218.set_MMAX_EGS(false);
gs218.set_DYN0_AMR_EGS(true);
gs218.set_DYN1_EGS(false);
return;
case TorqueRequest::Increase:
break;
case TorqueRequest::FollowMe:
gs218.set_MMIN_EGS(true);
gs218.set_MMAX_EGS(false);
gs218.set_DYN0_AMR_EGS(true);
gs218.set_DYN1_EGS(false);
break;
case TorqueRequest::Restore:
gs218.set_MMIN_EGS(true);
gs218.set_MMAX_EGS(false);
gs218.set_DYN0_AMR_EGS(true);
gs218.set_DYN1_EGS(true);
return;
break;
case TorqueRequest::None:
default:
gs218.set_MMIN_EGS(false);
gs218.set_MMAX_EGS(false);
gs218.set_DYN0_AMR_EGS(false);
gs218.set_DYN1_EGS(false);
return;
gs218.set_M_EGS(0);
break;
}
}
void Egs52Can::set_requested_torque(uint16_t torque_nm) {
if (torque_nm == 0 && gs218.get_DYN1_EGS() == false) {
if (this->current_req == TorqueRequest::None) {
gs218.set_M_EGS(0);
} else {
gs218.set_M_EGS((torque_nm + 500) * 4);
@ -649,6 +628,9 @@ void Egs52Can::set_drive_profile(GearboxProfile p) {
gs418.set_FPC('M');
break;
case GearboxProfile::Individual:
gs418.set_FPC('I');
break;
case GearboxProfile::Race:
gs418.set_FPC('R');
break;
case GearboxProfile::Underscore:
@ -930,15 +912,7 @@ inline bool calc_torque_parity(uint16_t s) {
return (p & 1) == 1;
}
inline void to_bytes(uint64_t src, uint8_t* dst) {
for(uint8_t i = 0; i < 8; i++) {
dst[7-i] = src & 0xFF;
src >>= 8;
}
}
[[noreturn]]
void Egs52Can::tx_task_loop() {
void Egs52Can::tx_frames() {
twai_message_t tx;
tx.data_length_code = 8; // Always
GS_338 gs_338tx;
@ -946,143 +920,64 @@ void Egs52Can::tx_task_loop() {
GS_418 gs_418tx;
GS_CUSTOM_558 gs_558tx;
GS_CUSTOM_668 gs_668tx;
uint8_t cvn_counter = 0;
bool toggle = false;
bool time_to_toggle = false;
while(true) {
if (!this->send_messages) {
vTaskDelay(50);
cvn_counter = 0; // Reset CVN when disabling msg Tx
toggle = false;
time_to_toggle = false;
continue;
}
// Copy current CAN frame values to here so we don't
// accidentally modify parity calculations
gs_338tx = {gs338.raw};
gs_218tx = {gs218.raw};
gs_418tx = {gs418.raw};
gs_558tx = {gs558.raw};
gs_668tx = {gs668.raw};
// Firstly we have to deal with toggled bits!
// As toggle bits need to be toggled every 40ms,
// and egs52 Tx interval is 20ms,
// we can achieve this with 2 booleans
gs_218tx.set_MTGL_EGS(toggle);
gs_418tx.set_FMRADTGL(toggle);
// Now do parity calculations
gs_218tx.set_MPAR_EGS(calc_torque_parity(gs_218tx.raw >> 48));
gs_418tx.set_FMRADPAR(calc_torque_parity(gs_418tx.raw & 0xFFFF));
if (time_to_toggle) {
toggle = !toggle;
}
time_to_toggle = !time_to_toggle;
// Now set CVN Counter (Increases every frame)
gs_218tx.set_FEHLER(cvn_counter);
cvn_counter++;
// Copy current CAN frame values to here so we don't
// accidentally modify parity calculations
gs_338tx = {gs338.raw};
gs_218tx = {gs218.raw};
gs_418tx = {gs418.raw};
gs_558tx = {gs558.raw};
gs_668tx = {gs668.raw};
// Now send CAN Data!
/**
* TX order of EGS52:
* GS_338
* GS_218
* GS_418
* GS_CUSTOM_558 ;)
*/
tx.identifier = GS_338_CAN_ID;
to_bytes(gs_338tx.raw, tx.data);
twai_transmit(&tx, 5);
vTaskDelay(1 / portTICK_PERIOD_MS);
tx.identifier = GS_218_CAN_ID;
to_bytes(gs_218tx.raw, tx.data);
twai_transmit(&tx, 5);
vTaskDelay(1 / portTICK_PERIOD_MS);
tx.identifier = GS_418_CAN_ID;
to_bytes(gs_418tx.raw, tx.data);
twai_transmit(&tx, 5);
vTaskDelay(1 / portTICK_PERIOD_MS);
tx.identifier = GS_CUSTOM_558_CAN_ID;
to_bytes(gs_558tx.raw, tx.data);
twai_transmit(&tx, 5);
vTaskDelay(1 / portTICK_PERIOD_MS);
tx.identifier = GS_CUSTOM_668_CAN_ID;
to_bytes(gs_668tx.raw, tx.data);
twai_transmit(&tx, 5);
vTaskDelay(1 / portTICK_PERIOD_MS);
vTaskDelay(this->tx_time_ms / portTICK_PERIOD_MS);
// Firstly we have to deal with toggled bits!
// As toggle bits need to be toggled every 40ms,
// and egs52 Tx interval is 20ms,
// we can achieve this with 2 booleans
gs_218tx.set_MTGL_EGS(toggle);
gs_418tx.set_FMRADTGL(toggle);
// Now do parity calculations
gs_218tx.set_MPAR_EGS(calc_torque_parity(gs_218tx.raw >> 48));
gs_418tx.set_FMRADPAR(calc_torque_parity(gs_418tx.raw & 0xFFFF));
if (time_to_toggle) {
toggle = !toggle;
}
time_to_toggle = !time_to_toggle;
// Now set CVN Counter (Increases every frame)
gs_218tx.set_FEHLER(cvn_counter);
cvn_counter++;
// Now send CAN Data!
/**
* TX order of EGS52:
* GS_338
* GS_218
* GS_418
* GS_CUSTOM_558 ;)
*/
tx.identifier = GS_338_CAN_ID;
to_bytes(gs_338tx.raw, tx.data);
twai_transmit(&tx, 5);
tx.identifier = GS_218_CAN_ID;
to_bytes(gs_218tx.raw, tx.data);
twai_transmit(&tx, 5);
tx.identifier = GS_418_CAN_ID;
to_bytes(gs_418tx.raw, tx.data);
twai_transmit(&tx, 5);
tx.identifier = GS_CUSTOM_558_CAN_ID;
to_bytes(gs_558tx.raw, tx.data);
twai_transmit(&tx, 5);
tx.identifier = GS_CUSTOM_668_CAN_ID;
to_bytes(gs_668tx.raw, tx.data);
twai_transmit(&tx, 5);
}
[[noreturn]]
void Egs52Can::rx_task_loop() {
twai_message_t rx;
twai_status_info_t can_status;
uint64_t now;
uint64_t tmp;
uint8_t i;
while(true) {
twai_get_status_info(&can_status);
if (can_status.msgs_to_rx == 0) {
vTaskDelay(4 / portTICK_PERIOD_MS); // Wait for buffer to have at least 1 frame
} else { // We have frames, read them
now = (esp_timer_get_time()/1000);
if (now > 2) {
now -= 2;
}
for(uint8_t x = 0; x < can_status.msgs_to_rx; x++) { // Read all frames
if (twai_receive(&rx, pdMS_TO_TICKS(0)) == ESP_OK && rx.data_length_code != 0) {
tmp = 0;
for(i = 0; i < rx.data_length_code; i++) {
tmp |= (uint64_t)rx.data[i] << (8*(7-i));
}
if(this->ecu_ms.import_frames(tmp, rx.identifier, now)) {
} else if (this->esp_ecu.import_frames(tmp, rx.identifier, now)) {
} else if (this->ewm_ecu.import_frames(tmp, rx.identifier, now)) {
} else if (this->misc_ecu.import_frames(tmp, rx.identifier, now)) {
} else if (this->ezs_ecu.import_frames(tmp, rx.identifier, now)) {
} else if (this->diag_rx_id != 0 && rx.identifier == this->diag_rx_id) {
// ISO-TP Diag endpoint
if (this->diag_rx_queue != nullptr && rx.data_length_code == 8) {
// Send the frame
if (xQueueSend(*this->diag_rx_queue, rx.data, 0) != pdTRUE) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EGS52_CAN","Discarded ISO-TP endpoint frame. Queue send failed");
}
}
} else if (rx.identifier > 0x780) {
ESP_LOGI("CAN", "Diag msg 0x%04X [%02X %02X %02X %02X %02X %02X %02X %02X]",
rx.identifier,
rx.data[0],
rx.data[1],
rx.data[2],
rx.data[3],
rx.data[4],
rx.data[5],
rx.data[6],
rx.data[7]
);
}
/*
else if (rx.identifier == KOMBI_408_CAN_ID) {
KOMBI_408 k408;
k408.raw = tmp;
if (esp_toggle) {
ESP_LOGI("CAN", "Turning off ESP");
k408.set_RT_EIN(true);
to_bytes(k408.raw, rx.data);
twai_transmit(&rx, 5);
} else {
ESP_LOGI("CAN", "Status of ESP disable bit %d", k408.get_RT_EIN());
}
}
*/
}
}
vTaskDelay(2 / portTICK_PERIOD_MS); // Reset watchdog here
}
void Egs52Can::on_rx_frame(uint32_t id, uint8_t dlc, uint64_t data, uint64_t timestamp) {
if(this->ecu_ms.import_frames(data, id, timestamp)) {
} else if (this->esp_ecu.import_frames(data, id, timestamp)) {
} else if (this->ewm_ecu.import_frames(data, id, timestamp)) {
} else if (this->misc_ecu.import_frames(data, id, timestamp)) {
} else if (this->ezs_ecu.import_frames(data, id, timestamp)) {
}
}
#endif
}

40
src/canbus/can_egs52.h
View File

@ -2,21 +2,18 @@
#define __EGS52_CAN_H_
#include <gearbox_config.h>
#ifdef EGS52_MODE
#include "can_hal.h"
#include "ANY_ECU.h"
#include "ESP_SBC.h"
#include "EWM.h"
#include "GS.h"
#include "MS.h"
#include "EZS.h"
#include "KOMBI.h"
#include "../../egs52_ecus/src/ANY_ECU52.h"
#include "../../egs52_ecus/src/ESP_SBC.h"
#include "../../egs52_ecus/src/EWM.h"
#include "../../egs52_ecus/src/GS.h"
#include "../../egs52_ecus/src/MS.h"
#include "../../egs52_ecus/src/EZS.h"
#include "../../egs52_ecus/src/KOMBI.h"
class Egs52Can: public AbstractCan {
class Egs52Can: public EgsBaseCan {
public:
explicit Egs52Can(const char* name, uint8_t tx_time_ms);
bool begin_tasks() override;
~Egs52Can();
explicit Egs52Can(const char* name, uint8_t tx_time_ms, uint32_t baud);
/**
* Getters
@ -42,6 +39,7 @@ class Egs52Can: public AbstractCan {
uint8_t get_pedal_value(uint64_t now, uint64_t expire_time_ms) override;
// Gets the current 'static' torque produced by the engine
int get_static_engine_torque(uint64_t now, uint64_t expire_time_ms) override;
int get_driver_engine_torque(uint64_t now, uint64_t expire_time_ms) override;
// Gets the maximum engine torque allowed at this moment by the engine map
int get_maximum_engine_torque(uint64_t now, uint64_t expire_time_ms) override;
// Gets the minimum engine torque allowed at this moment by the engine map
@ -59,8 +57,9 @@ class Egs52Can: public AbstractCan {
bool get_profile_btn_press(uint64_t now, uint64_t expire_time_ms) override;
//
bool get_is_brake_pressed(uint64_t now, uint64_t expire_time_ms) override;
TerminalStatus get_terminal_15(uint64_t now, uint64_t expire_time_ms) override;
// TerminalStatus get_terminal_15(uint64_t now, uint64_t expire_time_ms) override;
uint16_t get_fuel_flow_rate(uint64_t now, uint64_t expire_time_ms) override;
TransferCaseState get_transfer_case_state(uint64_t now, uint64_t expire_time_ms) override;
/**
* Setters
@ -109,10 +108,8 @@ class Egs52Can: public AbstractCan {
void set_gear_ratio(int16_t g100) override;
void set_abort_shift(bool is_aborting) override;
protected:
[[noreturn]]
void tx_task_loop() override;
[[noreturn]]
void rx_task_loop() override;
void tx_frames() override;
void on_rx_frame(uint32_t id, uint8_t dlc, uint64_t data, uint64_t timestamp) override;
private:
GearboxProfile curr_profile_bit = GearboxProfile::Underscore;
GearboxMessage curr_message = GearboxMessage::None;
@ -128,10 +125,11 @@ class Egs52Can: public AbstractCan {
ECU_ANY_ECU misc_ecu = ECU_ANY_ECU();
ECU_EWM ewm_ecu = ECU_EWM();
ECU_MS ecu_ms = ECU_MS();
bool can_init_ok = false;
bool esp_toggle = false;
bool time_to_toggle = false;
bool toggle = false;
uint8_t cvn_counter = 0;
TorqueRequest current_req = TorqueRequest::None;
};
#endif // EGS53_MODE
#endif // EGS52_CAN_H_
#endif // EGS53_MODE

289
src/canbus/can_egs53.cpp
View File

@ -1,95 +1,32 @@
#include "can_egs53.h"
#include "driver/twai.h"
#include "pins.h"
#include "gearbox_config.h"
#ifdef EGS53_MODE
#include "board_config.h"
#include "nvs/eeprom_config.h"
uint8_t crcTable[256]; // For CRC only
Egs53Can::Egs53Can(const char* name, uint8_t tx_time_ms)
: AbstractCan(name, tx_time_ms)
{
// Firstly try to init CAN
ESP_LOG_LEVEL(ESP_LOG_INFO, "EGS52_CAN", "CAN constructor called");
twai_general_config_t gen_config = TWAI_GENERAL_CONFIG_DEFAULT(PIN_CAN_TX, PIN_CAN_RX, TWAI_MODE_NORMAL);
gen_config.intr_flags = ESP_INTR_FLAG_IRAM; // Set TWAI interrupt to IRAM (Enabled in menuconfig)!
gen_config.rx_queue_len = 32;
gen_config.tx_queue_len = 32;
twai_timing_config_t timing_config = TWAI_TIMING_CONFIG_500KBITS();
twai_filter_config_t filter_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
esp_err_t res;
res = twai_driver_install(&gen_config, &timing_config, &filter_config);
if (res != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EGS52_CAN", "TWAI_DRIVER_INSTALL FAILED!: %s", esp_err_to_name(res));
}
res = twai_start();
if (res != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EGS52_CAN", "TWAI_START FAILED!: %s", esp_err_to_name(res));
}
Egs53Can::Egs53Can(const char* name, uint8_t tx_time_ms, uint32_t baud) : EgsBaseCan(name, tx_time_ms, baud){
// Create CRC table
uint8_t _crc;
for (int i = 0; i < 0x100; i++) {
_crc = i;
uint8_t _crc = i;
for (uint8_t bit = 0; bit < 8; bit++) {
_crc = (_crc & 0x80) ? ((_crc << 1) ^ 0x1D) : (_crc << 1);
}
crcTable[i] = _crc;
}
// CAN is OK!
// Set default values
this->sbw_rs_tcm.set_SBW_MsgTxmtId(SBW_RS_TCM_SBW_MsgTxmtId::EGS52); // We are EGS53
this->sbw_rs_tcm.set_TSL_Posn_Rq(SBW_RS_TCM_TSL_Posn_Rq::IDLE); // Idle request (No SBW on EGS53)
this->sbw_rs_tcm.set_TxSelSensPosn(0); // No dialing sensor on EGS53
// Tell engine which Mech style we are
#ifdef LARGE_NAG
this->eng_rq2_tcm.set_TxMechStyle(ENG_RQ2_TCM_TxMechStyle::LARGE);
#else
this->eng_rq2_tcm.set_TxMechStyle(ENG_RQ2_TCM_TxMechStyle::SMALL);
#endif
// Tell engine which Mech style we are
if (VEHICLE_CONFIG.is_large_nag != 0) {
this->eng_rq2_tcm.set_TxMechStyle(ENG_RQ2_TCM_TxMechStyle::LARGE);
} else {
this->eng_rq2_tcm.set_TxMechStyle(ENG_RQ2_TCM_TxMechStyle::SMALL);
}
this->eng_rq2_tcm.set_TxStyle(ENG_RQ2_TCM_TxStyle::SAT); // Stepped automatic gearbox
this->eng_rq2_tcm.set_TxShiftStyle(ENG_RQ2_TCM_TxShiftStyle::MS); // Mechanical shifting (With EWM module)
this->can_init_ok = true;
}
bool Egs53Can::begin_tasks() {
if (!this->can_init_ok) { // Cannot init tasks if CAN is dead!
return false;
}
// Prevent starting again
if (this->rx_task == nullptr) {
ESP_LOG_LEVEL(ESP_LOG_INFO, "EGS53_CAN", "Starting CAN Rx task");
if (xTaskCreate(this->start_rx_task_loop, "EGS53_CAN_RX", 8192, this, 5, this->rx_task) != pdPASS) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EGS53_CAN", "CAN Rx task creation failed!");
return false;
}
}
if (this->tx_task == nullptr) {
ESP_LOG_LEVEL(ESP_LOG_INFO, "EGS53_CAN", "Starting CAN Tx task");
if (xTaskCreate(this->start_tx_task_loop, "EGS53_CAN_TX", 8192, this, 5, this->tx_task) != pdPASS) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EGS53_CAN", "CAN Tx task creation failed!");
return false;
}
}
return true; // Ready!
}
Egs53Can::~Egs53Can()
{
if (this->rx_task != nullptr) {
vTaskDelete(this->rx_task);
}
if (this->tx_task != nullptr) {
vTaskDelete(this->tx_task);
}
// Delete CAN
if (this->can_init_ok) {
twai_stop();
twai_driver_uninstall();
}
}
WheelData Egs53Can::get_front_right_wheel(uint64_t now, uint64_t expire_time_ms) { // TODO
@ -107,7 +44,7 @@ WheelData Egs53Can::get_front_right_wheel(uint64_t now, uint64_t expire_time_ms)
dir = WheelDirection::Reverse;
break;
default:
dir = WheelDirection::SignalNotAvaliable;
dir = WheelDirection::SignalNotAvailable;
break;
}
return WheelData {
@ -117,7 +54,7 @@ WheelData Egs53Can::get_front_right_wheel(uint64_t now, uint64_t expire_time_ms)
} else {
return WheelData {
.double_rpm = 0,
.current_dir = WheelDirection::SignalNotAvaliable
.current_dir = WheelDirection::SignalNotAvailable
};
}
}
@ -137,7 +74,7 @@ WheelData Egs53Can::get_front_left_wheel(uint64_t now, uint64_t expire_time_ms)
dir = WheelDirection::Reverse;
break;
default:
dir = WheelDirection::SignalNotAvaliable;
dir = WheelDirection::SignalNotAvailable;
break;
}
return WheelData {
@ -147,7 +84,7 @@ WheelData Egs53Can::get_front_left_wheel(uint64_t now, uint64_t expire_time_ms)
} else {
return WheelData {
.double_rpm = 0,
.current_dir = WheelDirection::SignalNotAvaliable
.current_dir = WheelDirection::SignalNotAvailable
};
}
}
@ -167,7 +104,7 @@ WheelData Egs53Can::get_rear_right_wheel(uint64_t now, uint64_t expire_time_ms)
dir = WheelDirection::Reverse;
break;
default:
dir = WheelDirection::SignalNotAvaliable;
dir = WheelDirection::SignalNotAvailable;
break;
}
return WheelData {
@ -177,7 +114,7 @@ WheelData Egs53Can::get_rear_right_wheel(uint64_t now, uint64_t expire_time_ms)
} else {
return WheelData {
.double_rpm = 0,
.current_dir = WheelDirection::SignalNotAvaliable
.current_dir = WheelDirection::SignalNotAvailable
};
}
}
@ -197,7 +134,7 @@ WheelData Egs53Can::get_rear_left_wheel(uint64_t now, uint64_t expire_time_ms) {
dir = WheelDirection::Reverse;
break;
default:
dir = WheelDirection::SignalNotAvaliable;
dir = WheelDirection::SignalNotAvailable;
break;
}
return WheelData {
@ -207,7 +144,7 @@ WheelData Egs53Can::get_rear_left_wheel(uint64_t now, uint64_t expire_time_ms) {
} else {
return WheelData {
.double_rpm = 0,
.current_dir = WheelDirection::SignalNotAvaliable
.current_dir = WheelDirection::SignalNotAvailable
};
}
}
@ -236,10 +173,10 @@ ShifterPosition Egs53Can::get_shifter_position_ewm(uint64_t now, uint64_t expire
return ShifterPosition::P_R;
case SBW_RS_ISM_TSL_Posn_ISM::SNA:
default:
return ShifterPosition::SignalNotAvaliable;
return ShifterPosition::SignalNotAvailable;
}
}
return ShifterPosition::SignalNotAvaliable;
return ShifterPosition::SignalNotAvailable;
}
EngineType Egs53Can::get_engine_type(uint64_t now, uint64_t expire_time_ms) {
@ -274,6 +211,10 @@ int Egs53Can::get_static_engine_torque(uint64_t now, uint64_t expire_time_ms) {
return 0;
}
int Egs53Can::get_driver_engine_torque(uint64_t now, uint64_t expire_time_ms) {
return this->get_static_engine_torque(now, expire_time_ms);
}
int Egs53Can::get_maximum_engine_torque(uint64_t now, uint64_t expire_time_ms) { // TODO
ENG_RS2_PT rs2_pt;
if (this->ecm_ecu.get_ENG_RS2_PT(now, expire_time_ms*1000, &rs2_pt)) {
@ -367,7 +308,7 @@ void Egs53Can::set_actual_gear(GearboxGear actual) {
case GearboxGear::Fifth:
this->eng_rq2_tcm.set_Gr(ENG_RQ2_TCM_Gr::D5);
break;
case GearboxGear::SignalNotAvaliable:
case GearboxGear::SignalNotAvailable:
default:
this->eng_rq2_tcm.set_Gr(ENG_RQ2_TCM_Gr::SNA);
break;
@ -403,7 +344,7 @@ void Egs53Can::set_target_gear(GearboxGear target) {
case GearboxGear::Fifth:
this->eng_rq2_tcm.set_Gr_Target(ENG_RQ2_TCM_Gr_Target::D5);
break;
case GearboxGear::SignalNotAvaliable:
case GearboxGear::SignalNotAvailable:
default:
this->eng_rq2_tcm.set_Gr_Target(ENG_RQ2_TCM_Gr_Target::SNA);
break;
@ -463,7 +404,7 @@ void Egs53Can::set_shifter_position(ShifterPosition pos) {
this->sbw_rs_tcm.set_TxSelVlvPosn(SBW_RS_TCM_TxSelVlvPosn::D);
this->tcm_a1.set_TSL_Posn_TCM(TCM_A1_TSL_Posn_TCM::D);
break;
case ShifterPosition::SignalNotAvaliable:
case ShifterPosition::SignalNotAvailable:
default:
this->sbw_rs_tcm.set_TxSelVlvPosn(SBW_RS_TCM_TxSelVlvPosn::SNA);
this->tcm_a1.set_TSL_Posn_TCM(TCM_A1_TSL_Posn_TCM::SNA);
@ -493,7 +434,7 @@ void Egs53Can::set_turbine_torque_loss(uint16_t loss_nm) {
}
uint8_t x = 0;
// uint8_t x = 0;
unsigned long last_time = 0;
void Egs53Can::set_display_gear(GearboxDisplayGear g, bool manual_mode) {
switch (g) {
@ -589,8 +530,9 @@ void Egs53Can::set_wheel_torque_multi_factor(float ratio) {
void calc_crc_in_place(uint8_t* buffer) {
// assume len = 7
unsigned long crc;
int i,bit;
int i;
int bit;
crc = 0xFF;
for ( i=0 ; i<7 ; i++ ) {
crc ^= buffer[i];
@ -616,10 +558,9 @@ inline void to_bytes(uint64_t src, uint8_t* dst) {
}
uint8_t msg_counter = 0;
[[noreturn]]
void Egs53Can::tx_task_loop() {
uint64_t start_time;
uint32_t taken;
void Egs53Can::tx_frames() {
twai_message_t tx;
tx.data_length_code = 8; // Always
@ -631,112 +572,68 @@ void Egs53Can::tx_task_loop() {
SBW_RS_TCM sbw_rs_tcm_tx = {0};
TCM_DISP_RQ tcm_disp_rq_tx = {0};
NM_TCM nm_tcm_tx = {0};
uint8_t counter = 0;
uint8_t cvn_counter = 0;
while(true) {
// Copy current CAN frame values to here so we don't
// accidentally modify parity calculations
tcm_a1_tx = {tcm_a1.raw};
tcm_a2_tx = {tcm_a2.raw};
eng_rq1_tcm_tx = {eng_rq1_tcm.raw};
eng_rq2_tcm_tx = {eng_rq2_tcm.raw};
eng_rq3_tcm_tx = {eng_rq3_tcm.raw};
sbw_rs_tcm_tx = {sbw_rs_tcm.raw};
tcm_disp_rq_tx = {tcm_disp_rq.raw};
nm_tcm_tx = {nm_tcm.raw};
tcm_a2_tx.set_TCM_CALID_CVN_ErrNum(cvn_counter);
cvn_counter++;
if (cvn_counter == 0x14) {
cvn_counter = 0;
}
// Copy current CAN frame values to here so we don't
// accidentally modify parity calculations
tcm_a1_tx = {tcm_a1.raw};
tcm_a2_tx = {tcm_a2.raw};
eng_rq1_tcm_tx = {eng_rq1_tcm.raw};
eng_rq2_tcm_tx = {eng_rq2_tcm.raw};
eng_rq3_tcm_tx = {eng_rq3_tcm.raw};
sbw_rs_tcm_tx = {sbw_rs_tcm.raw};
tcm_disp_rq_tx = {tcm_disp_rq.raw};
nm_tcm_tx = {nm_tcm.raw};
// Set message counters
eng_rq1_tcm_tx.set_MC_ENG_RQ1_TCM(msg_counter);
eng_rq2_tcm_tx.set_MC_ENG_RQ2_TCM(msg_counter);
eng_rq3_tcm_tx.set_MC_ENG_RQ3_TCM(msg_counter);
sbw_rs_tcm_tx.set_MC_SBW_RS_TCM(msg_counter);
if (this->send_messages) {
vTaskDelay(50);
continue;
}
msg_counter++; // Global for all messages out of TCM
// Now send CAN Data!
tx.identifier = ENG_RQ1_TCM_CAN_ID;
to_bytes(eng_rq1_tcm_tx.raw, tx.data);
calc_crc_in_place(tx.data);
twai_transmit(&tx, 5);
tx.identifier = ENG_RQ2_TCM_CAN_ID;
to_bytes(eng_rq2_tcm_tx.raw, tx.data);
calc_crc_in_place(tx.data);
twai_transmit(&tx, 5);
tx.identifier = TCM_A2_CAN_ID;
to_bytes(tcm_a2_tx.raw, tx.data);
calc_crc_in_place(tx.data);
twai_transmit(&tx, 5);
tx.identifier = TCM_A1_CAN_ID;
to_bytes(tcm_a1_tx.raw, tx.data);
twai_transmit(&tx, 5);
if (counter == 5) {
tx.identifier = TCM_DISP_RQ_CAN_ID;
to_bytes(tcm_disp_rq_tx.raw, tx.data);
twai_transmit(&tx, 5);
counter = 0;
}
counter++;
tx.identifier = SBW_RS_TCM_CAN_ID;
to_bytes(sbw_rs_tcm_tx.raw, tx.data);
calc_crc_in_place(tx.data);
twai_transmit(&tx, 5);
vTaskDelay(this->tx_time_ms / portTICK_PERIOD_MS);
tcm_a2_tx.set_TCM_CALID_CVN_ErrNum(cvn_counter);
cvn_counter++;
if (cvn_counter == 0x14) {
cvn_counter = 0;
}
// Set message counters
eng_rq1_tcm_tx.set_MC_ENG_RQ1_TCM(msg_counter);
eng_rq2_tcm_tx.set_MC_ENG_RQ2_TCM(msg_counter);
eng_rq3_tcm_tx.set_MC_ENG_RQ3_TCM(msg_counter);
sbw_rs_tcm_tx.set_MC_SBW_RS_TCM(msg_counter);
msg_counter++; // Global for all messages out of TCM
// Now send CAN Data!
tx.identifier = ENG_RQ1_TCM_CAN_ID;
to_bytes(eng_rq1_tcm_tx.raw, tx.data);
calc_crc_in_place(tx.data);
twai_transmit(&tx, 5);
tx.identifier = ENG_RQ2_TCM_CAN_ID;
to_bytes(eng_rq2_tcm_tx.raw, tx.data);
calc_crc_in_place(tx.data);
twai_transmit(&tx, 5);
tx.identifier = TCM_A2_CAN_ID;
to_bytes(tcm_a2_tx.raw, tx.data);
calc_crc_in_place(tx.data);
twai_transmit(&tx, 5);
tx.identifier = TCM_A1_CAN_ID;
to_bytes(tcm_a1_tx.raw, tx.data);
twai_transmit(&tx, 5);
if (counter == 5) {
tx.identifier = TCM_DISP_RQ_CAN_ID;
to_bytes(tcm_disp_rq_tx.raw, tx.data);
twai_transmit(&tx, 5);
counter = 0;
}
counter++;
tx.identifier = SBW_RS_TCM_CAN_ID;
to_bytes(sbw_rs_tcm_tx.raw, tx.data);
calc_crc_in_place(tx.data);
twai_transmit(&tx, 5);
}
[[noreturn]]
void Egs53Can::rx_task_loop() {
twai_message_t rx;
twai_status_info_t can_status;
uint64_t now;
uint64_t tmp;
uint8_t i;
while(true) {
twai_get_status_info(&can_status);
uint8_t f_count = can_status.msgs_to_rx;
if (f_count == 0) {
vTaskDelay(4 / portTICK_PERIOD_MS); // Wait for buffer to have at least 1 frame
} else { // We have frames, read them
now = esp_timer_get_time()/1000;
for(uint8_t x = 0; x < f_count; x++) { // Read all frames
if (twai_receive(&rx, pdMS_TO_TICKS(0)) == ESP_OK && rx.data_length_code != 0 && rx.flags == 0) {
tmp = 0;
for(i = 0; i < rx.data_length_code; i++) {
tmp |= (uint64_t)rx.data[i] << (8*(7-i));
}
if(this->ecm_ecu.import_frames(tmp, rx.identifier, now)) {
} else if (this->fscm_ecu.import_frames(tmp, rx.identifier, now)) {
} else if (this->tslm_ecu.import_frames(tmp, rx.identifier, now)) {
} else if (this->diag_rx_id != 0 && rx.identifier == this->diag_rx_id) {
// ISO-TP Diag endpoint
if (this->diag_rx_queue != nullptr && rx.data_length_code == 8) {
// Send the frame
if (xQueueSend(*this->diag_rx_queue, rx.data, 0) != pdTRUE) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EGS53_CAN","Discarded ISO-TP endpoint frame. Queue send failed");
}
}
}
}
}
vTaskDelay(2 / portTICK_PERIOD_MS); // Reset watchdog here
}
void Egs53Can::on_rx_frame(uint32_t id, uint8_t dlc, uint64_t data, uint64_t timestamp) {
if(this->ecm_ecu.import_frames(data, id, timestamp)) {
} else if (this->fscm_ecu.import_frames(data, id, timestamp)) {
} else if (this->tslm_ecu.import_frames(data, id, timestamp)) {
}
}
#endif
}

34
src/canbus/can_egs53.h
View File

@ -3,20 +3,16 @@
#include <gearbox_config.h>
//#define EGS53_MODE
#ifdef EGS53_MODE
#include "can_hal.h"
#include "TCM.h"
#include "TSLM.h"
#include "FSCM.h"
#include "ECM.h"
#include "ANY_ECU.h"
#include "../../egs53_ecus/src/TCM.h"
#include "../../egs53_ecus/src/TSLM.h"
#include "../../egs53_ecus/src/FSCM.h"
#include "../../egs53_ecus/src/ECM.h"
#include "../../egs53_ecus/src/ANY_ECU.h"
class Egs53Can: public AbstractCan {
class Egs53Can: public EgsBaseCan {
public:
explicit Egs53Can(const char* name, uint8_t tx_time_ms);
bool begin_tasks() override;
~Egs53Can();
explicit Egs53Can(const char* name, uint8_t tx_time_ms, uint32_t baud);
/**
* Getters
@ -46,6 +42,7 @@ class Egs53Can: public AbstractCan {
int get_maximum_engine_torque(uint64_t now, uint64_t expire_time_ms) override;
// Gets the minimum engine torque allowed at this moment by the engine map
int get_minimum_engine_torque(uint64_t now, uint64_t expire_time_ms) override;
int get_driver_engine_torque(uint64_t now, uint64_t expire_time_ms) override;
// Gets the flappy paddle position
PaddlePosition get_paddle_position(uint64_t now, uint64_t expire_time_ms) override;
// Gets engine coolant temperature
@ -101,10 +98,8 @@ class Egs53Can: public AbstractCan {
void set_solenoid_pwm(uint16_t duty, SolenoidName s) override;
void set_wheel_torque_multi_factor(float ratio) override;
protected:
[[noreturn]]
void tx_task_loop() override;
[[noreturn]]
void rx_task_loop() override;
void tx_frames() override;
void on_rx_frame(uint32_t id, uint8_t dlc, uint64_t data, uint64_t timestamp) override;
private:
// CAN Frames to Tx
TCM_A1 tcm_a1 = {0};
@ -120,9 +115,8 @@ class Egs53Can: public AbstractCan {
ECU_FSCM fscm_ecu = ECU_FSCM();
ECU_TSLM tslm_ecu = ECU_TSLM();
// ECU Data to Rx to
bool can_init_ok = false;
};
#endif
uint8_t counter = 0;
uint8_t cvn_counter = 0;
#endif // EGS53_CAN_H_
};
#endif

153
src/canbus/can_hal.cpp
View File

@ -1,3 +1,154 @@
#include "can_hal.h"
#include "board_config.h"
#include "esp_check.h"
AbstractCan* egs_can_hal = nullptr;
EgsBaseCan* egs_can_hal = nullptr;
EgsBaseCan::EgsBaseCan(const char* name, uint8_t tx_time_ms, uint32_t baud) {
this->name = name;
this->diag_rx_id = 0x07E1;
this->diag_tx_id = 0x07E9;
this->diag_rx_queue = nullptr;
this->can_init_status = ESP_OK;
this->tx_time_ms = tx_time_ms;
// Firstly try to init CAN
ESP_LOG_LEVEL(ESP_LOG_INFO, this->name, "Booting CAN Layer");
if (nullptr == pcb_gpio_matrix) {
this->can_init_status = ESP_ERR_INVALID_STATE;
ESP_LOGE(this->name, "No GPIO matrix! Cannot start CAN");
return;
}
twai_general_config_t gen_config = TWAI_GENERAL_CONFIG_DEFAULT(pcb_gpio_matrix->can_tx_pin, pcb_gpio_matrix->can_rx_pin, TWAI_MODE_NORMAL);
gen_config.intr_flags = ESP_INTR_FLAG_IRAM; // Set TWAI interrupt to IRAM (Enabled in menuconfig)!
gen_config.rx_queue_len = 32;
gen_config.tx_queue_len = 32;
twai_timing_config_t timing_config{};
switch(baud) {
case 1000000: // 1mbps
timing_config = TWAI_TIMING_CONFIG_1MBITS();
break;
case 800000: // 800kbps
timing_config = TWAI_TIMING_CONFIG_800KBITS();
break;
case 500000: // 500kbps
timing_config = TWAI_TIMING_CONFIG_500KBITS();
break;
case 250000:
timing_config = TWAI_TIMING_CONFIG_250KBITS();
break;
case 125000:
timing_config = TWAI_TIMING_CONFIG_125KBITS();
break;
case 100000:
timing_config = TWAI_TIMING_CONFIG_100KBITS();
break;
default:
ESP_LOGE(this->name, "Cannot set CAN baud to %d", baud);
this->can_init_status = ESP_ERR_INVALID_ARG;
break;
}
twai_filter_config_t filter_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
if (this->can_init_status == ESP_OK) {
// Init OK (Baud valid)
this->can_init_status = twai_driver_install(&gen_config, &timing_config, &filter_config);
if (this->can_init_status == ESP_OK) {
this->can_init_status = twai_start();
if (this->can_init_status == ESP_OK) {
ESP_LOGI(this->name, "Calling setup");
} else {
ESP_LOGE(this->name, "Failed to start TWAI");
}
} else {
ESP_LOGE(this->name, "Failed to install TWAI driver");
}
}
}
esp_err_t EgsBaseCan::init_state() const {
return this->can_init_status;
}
EgsBaseCan::~EgsBaseCan() {
if (this->rx_task != nullptr) {
vTaskDelete(this->rx_task);
}
if (this->tx_task != nullptr) {
vTaskDelete(this->tx_task);
}
// Delete CAN
if (this->can_init_status == ESP_OK) {
twai_stop();
twai_driver_uninstall();
}
}
bool EgsBaseCan::begin_tasks() {
if (this->can_init_status != ESP_OK) {
return false;
}
// Prevent starting again
if (this->rx_task == nullptr) {
ESP_LOG_LEVEL(ESP_LOG_INFO, this->name, "Starting CAN Rx task");
if (xTaskCreate(this->start_rx_task_loop, "EGS_CAN_RX", 8192, this, 5, this->rx_task) != pdPASS) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, this->name, "CAN Rx task creation failed!");
return false;
}
}
if (this->tx_task == nullptr) {
ESP_LOG_LEVEL(ESP_LOG_INFO, this->name, "Starting CAN Tx task");
if (xTaskCreate(this->start_tx_task_loop, "EGS_CAN_TX", 4096, this, 5, this->tx_task) != pdPASS) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, this->name, "CAN Tx task creation failed!");
return false;
}
}
return true; // Ready!
}
[[noreturn]]
void EgsBaseCan::tx_task_loop() {
while(true) {
if (this->send_messages) {
this->tx_frames();
}
vTaskDelay(this->tx_time_ms / portTICK_PERIOD_MS);
}
}
[[noreturn]]
void EgsBaseCan::rx_task_loop() {
twai_message_t rx;
uint8_t i;
uint64_t tmp;
uint64_t now;
while(true) {
now = esp_timer_get_time() / 1000;
twai_get_status_info(&this->can_status);
uint8_t f_count = can_status.msgs_to_rx;
if (f_count == 0) {
vTaskDelay(4 / portTICK_PERIOD_MS); // Wait for buffer to have at least 1 frame
} else { // We have frames, read them
for(uint8_t x = 0; x < f_count; x++) { // Read all frames
if (twai_receive(&rx, pdMS_TO_TICKS(0)) == ESP_OK && rx.data_length_code != 0 && rx.flags == 0) {
tmp = 0;
for(i = 0; i < rx.data_length_code; i++) {
tmp |= (uint64_t)rx.data[i] << (8*(7-i));
}
this->on_rx_frame(rx.identifier, rx.data_length_code, tmp, now);
if (this->diag_rx_id != 0 && rx.identifier == this->diag_rx_id) {
// ISO-TP Diag endpoint
if (this->diag_rx_queue != nullptr && rx.data_length_code == 8) {
// Send the frame
if (xQueueSend(*this->diag_rx_queue, rx.data, 0) != pdTRUE) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EGS_BASIC_CAN","Discarded ISO-TP endpoint frame. Queue send failed");
}
}
}
}
}
vTaskDelay(2 / portTICK_PERIOD_MS); // Reset watchdog here
}
this->on_rx_done(now);
}
}

217
src/canbus/can_hal.h
View File

@ -12,12 +12,13 @@
#include <esp_log.h>
#include <freertos/queue.h>
#include <string.h>
#include "driver/twai.h"
enum class WheelDirection: uint8_t {
Forward, // Wheel going forwards
Reverse, // Wheel going backwards
Stationary, // Stationary (Not forward or backwards)
SignalNotAvaliable = 0xFF // SNV
SignalNotAvailable = 0xFF // SNV
};
struct WheelData {
@ -25,13 +26,6 @@ struct WheelData {
WheelDirection current_dir; // Wheel direction
};
struct DiagIsoTpInfo {
uint32_t tx_canid;
uint32_t rx_canid;
uint8_t bs;
uint8_t st_min;
};
enum class SystemStatusCheck: uint8_t {
// Waiting for check to complete
Waiting,
@ -47,23 +41,40 @@ enum class EngineType: uint8_t {
Unknown = 0xFF
};
/// @brief Torque request type
enum class TorqueRequest: uint8_t {
Decrease,
Increase,
None
/// @brief No torque request specified
None,
/// @brief Begin torque request. Limit engine
Begin,
/// @brief Torque request, engine must follow EGS demand
FollowMe,
/// @brief Restore torque request back to normal
Restore,
};
/// @brief Gearbox gears for 722.6 gearbox
enum class GearboxGear: uint8_t {
/// @brief Gear D1
First = 1,
/// @brief Gear D2
Second = 2,
/// @brief Gear D3
Third = 3,
/// @brief Gear D4
Fourth = 4,
/// @brief Gear D5
Fifth = 5,
/// @brief Park
Park = 8,
/// @brief Neutral
Neutral = 9,
/// @brief Gear R1
Reverse_First = 10,
/// @brief Gear R2
Reverse_Second = 11,
SignalNotAvaliable = 0xFF,
/// @brief Implausible or signal not available
SignalNotAvailable = 0xFF
};
enum class PaddlePosition: uint8_t {
@ -80,6 +91,14 @@ enum class ClutchStatus: uint8_t {
Closed
};
enum class TransferCaseState: uint8_t {
Hi, // High range
Low, // Low range
Neither, // Neutral
Switching, // Switching in progress
SNA = 0xFF, // Error
};
enum class GearboxProfile: uint8_t {
// Comfort (C)
Comfort,
@ -93,6 +112,8 @@ enum class GearboxProfile: uint8_t {
Winter,
// Failure (F)
Failure,
// Race (R)
Race,
Individual,
// Reserved for 'no profile' (_)
Underscore
@ -112,7 +133,7 @@ enum class ShifterPosition: uint8_t {
THREE, // For TRRS only
TWO, // For TRRS only
ONE, // For TRRS only
SignalNotAvaliable = 0xFF // SNV
SignalNotAvailable = 0xFF // SNV
};
enum class SolenoidName: uint8_t {
@ -166,59 +187,100 @@ enum class TerminalStatus {
SNA
};
class AbstractCan {
public:
explicit AbstractCan(const char* name, uint8_t tx_time_ms) {
this->name = name;
this->tx_time_ms = tx_time_ms;
this->tx_task = nullptr;
this->rx_task = nullptr;
this->diag_rx_queue = nullptr;
};
virtual bool begin_tasks();
~AbstractCan();
const WheelData DEFAULT_SNV_WD {
.double_rpm = 0,
.current_dir = WheelDirection::SignalNotAvailable
};
class EgsBaseCan {
public:
EgsBaseCan(const char* name, uint8_t tx_time_ms, uint32_t baud);
~EgsBaseCan();
bool begin_tasks();
esp_err_t init_state() const;
/**
* Getters
*/
// Get the front right wheel data
virtual WheelData get_front_right_wheel(uint64_t now, uint64_t expire_time_ms);
virtual WheelData get_front_right_wheel(uint64_t now, uint64_t expire_time_ms) {
return DEFAULT_SNV_WD;
}
// Get the front left wheel data
virtual WheelData get_front_left_wheel(uint64_t now, uint64_t expire_time_ms);
virtual WheelData get_front_left_wheel(uint64_t now, uint64_t expire_time_ms) {
return DEFAULT_SNV_WD;
}
// Get the rear right wheel data
virtual WheelData get_rear_right_wheel(uint64_t now, uint64_t expire_time_ms);
virtual WheelData get_rear_right_wheel(uint64_t now, uint64_t expire_time_ms) {
return DEFAULT_SNV_WD;
}
// Get the rear left wheel data
virtual WheelData get_rear_left_wheel(uint64_t now, uint64_t expire_time_ms);
virtual WheelData get_rear_left_wheel(uint64_t now, uint64_t expire_time_ms) {
return DEFAULT_SNV_WD;
}
// Gets shifter position from EWM module
virtual ShifterPosition get_shifter_position_ewm(uint64_t now, uint64_t expire_time_ms);
virtual ShifterPosition get_shifter_position_ewm(uint64_t now, uint64_t expire_time_ms) {
return ShifterPosition::SignalNotAvailable;
}
// Gets engine type
virtual EngineType get_engine_type(uint64_t now, uint64_t expire_time_ms);
virtual EngineType get_engine_type(uint64_t now, uint64_t expire_time_ms) {
return EngineType::Unknown;
}
// Returns true if engine is in limp mode
virtual bool get_engine_is_limp(uint64_t now, uint64_t expire_time_ms);
virtual bool get_engine_is_limp(uint64_t now, uint64_t expire_time_ms) {
return false;
}
// Returns true if pedal is kickdown
virtual bool get_kickdown(uint64_t now, uint64_t expire_time_ms);
virtual bool get_kickdown(uint64_t now, uint64_t expire_time_ms) {
return 0;
}
// Returns the pedal percentage. Range 0-250
virtual uint8_t get_pedal_value(uint64_t now, uint64_t expire_time_ms);
virtual uint8_t get_pedal_value(uint64_t now, uint64_t expire_time_ms) {
return 0;
}
// Gets the current 'static' torque produced by the engine
virtual int get_static_engine_torque(uint64_t now, uint64_t expire_time_ms);
virtual int get_static_engine_torque(uint64_t now, uint64_t expire_time_ms) {
return 0;
}
// Gets driver torque demand (Default torque)
virtual int get_driver_engine_torque(uint64_t now, uint64_t expire_time_ms) {
return 0;
}
// Gets the maximum engine torque allowed at this moment by the engine map
virtual int get_maximum_engine_torque(uint64_t now, uint64_t expire_time_ms);
virtual int get_maximum_engine_torque(uint64_t now, uint64_t expire_time_ms) {
return 0;
}
// Gets the minimum engine torque allowed at this moment by the engine map
virtual int get_minimum_engine_torque(uint64_t now, uint64_t expire_time_ms);
virtual int get_minimum_engine_torque(uint64_t now, uint64_t expire_time_ms) {
return 0;
}
// Gets the flappy paddle position
virtual PaddlePosition get_paddle_position(uint64_t now, uint64_t expire_time_ms);
virtual PaddlePosition get_paddle_position(uint64_t now, uint64_t expire_time_ms) {
return PaddlePosition::SNV;
}
// Gets engine coolant temperature
virtual int16_t get_engine_coolant_temp(uint64_t now, uint64_t expire_time_ms);
virtual int16_t get_engine_coolant_temp(uint64_t now, uint64_t expire_time_ms) {
return INT16_MAX;
}
// Gets engine oil temperature
virtual int16_t get_engine_oil_temp(uint64_t now, uint64_t expire_time_ms);
virtual int16_t get_engine_oil_temp(uint64_t now, uint64_t expire_time_ms) {
return INT16_MAX;
}
// Gets engine RPM
virtual uint16_t get_engine_rpm(uint64_t now, uint64_t expire_time_ms);
virtual uint16_t get_engine_rpm(uint64_t now, uint64_t expire_time_ms) {
return UINT16_MAX;
}
// Returns true if engine is cranking
virtual bool get_is_starting(uint64_t now, uint64_t expire_time_ms);
virtual bool get_profile_btn_press(uint64_t now, uint64_t expire_time_ms);
virtual bool get_is_brake_pressed(uint64_t now, uint64_t expire_time_ms);
virtual bool get_is_starting(uint64_t now, uint64_t expire_time_ms) {
return false;
}
virtual bool get_profile_btn_press(uint64_t now, uint64_t expire_time_ms) {
return false;
}
virtual bool get_is_brake_pressed(uint64_t now, uint64_t expire_time_ms) {
return false;
}
virtual uint16_t get_fuel_flow_rate(uint64_t now, uint64_t expire_time_ms) {
return 0;
}
@ -227,49 +289,53 @@ class AbstractCan {
return TerminalStatus::On; // Enabled by default unless implemented
}
virtual TransferCaseState get_transfer_case_state(uint64_t now, uint64_t expire_time_ms) {
return TransferCaseState::SNA;
}
/**
* Setters
*/
virtual void set_race_start(bool race_start);
virtual void set_race_start(bool race_start){};
// Set solenoid PMW
virtual void set_solenoid_pwm(uint16_t duty, SolenoidName s){};
// Set the gearbox clutch position on CAN
virtual void set_clutch_status(ClutchStatus status);
virtual void set_clutch_status(ClutchStatus status){};
// Set the actual gear of the gearbox
virtual void set_actual_gear(GearboxGear actual);
virtual void set_actual_gear(GearboxGear actual){};
// Set the target gear of the gearbox
virtual void set_target_gear(GearboxGear target);
virtual void set_target_gear(GearboxGear target){};
// Sets the status bit indicating the car is safe to start
virtual void set_safe_start(bool can_start);
virtual void set_safe_start(bool can_start){};
// Sets the gerabox ATF temperature. Offset by +50C
virtual void set_gearbox_temperature(uint16_t temp);
virtual void set_gearbox_temperature(uint16_t temp){};
// Sets the RPM of the input shaft of the gearbox on CAN
virtual void set_input_shaft_speed(uint16_t rpm);
virtual void set_input_shaft_speed(uint16_t rpm){};
// Sets 4WD activated toggle bit
virtual void set_is_all_wheel_drive(bool is_4wd);
virtual void set_is_all_wheel_drive(bool is_4wd){};
// Sets wheel torque
virtual void set_wheel_torque(uint16_t t);
virtual void set_wheel_torque(uint16_t t){};
// Sets shifter position message
virtual void set_shifter_position(ShifterPosition pos);
virtual void set_shifter_position(ShifterPosition pos){};
// Sets gearbox is OK
virtual void set_gearbox_ok(bool is_ok);
virtual void set_gearbox_ok(bool is_ok){};
// Sets torque request toggle
virtual void set_torque_request(TorqueRequest request);
virtual void set_torque_request(TorqueRequest request){};
// Sets requested engine torque
virtual void set_requested_torque(uint16_t torque_nm);
virtual void set_requested_torque(uint16_t torque_nm){};
// Sets torque loss of torque converter
virtual void set_turbine_torque_loss(uint16_t loss_nm);
virtual void set_turbine_torque_loss(uint16_t loss_nm){};
// Sets torque multiplier factor from Engine all the way to wheels
virtual void set_wheel_torque_multi_factor(float ratio);
virtual void set_wheel_torque_multi_factor(float ratio){};
// Sets the status of system error check
virtual void set_error_check_status(SystemStatusCheck ssc);
virtual void set_error_check_status(SystemStatusCheck ssc){};
// Sets display profile
virtual void set_display_gear(GearboxDisplayGear g, bool manual_mode);
virtual void set_display_gear(GearboxDisplayGear g, bool manual_mode){};
// Sets drive profile
virtual void set_drive_profile(GearboxProfile p);
virtual void set_drive_profile(GearboxProfile p){};
// Sets display message
virtual void set_display_msg(GearboxMessage msg);
virtual void set_display_msg(GearboxMessage msg){};
// Set bit to signify the gearbox is aborting the shift
virtual void set_abort_shift(bool is_aborting){};
@ -298,6 +364,7 @@ class AbstractCan {
}
protected:
const char* name;
TaskHandle_t* tx_task = nullptr;
TaskHandle_t* rx_task = nullptr;
uint8_t tx_time_ms = 0;
@ -306,24 +373,36 @@ class AbstractCan {
uint16_t diag_rx_id = 0;
[[noreturn]]
virtual void tx_task_loop();
void tx_task_loop(void);
[[noreturn]]
virtual void rx_task_loop();
void rx_task_loop(void);
static void start_rx_task_loop(void *_this) {
static_cast<AbstractCan*>(_this)->rx_task_loop();
static_cast<EgsBaseCan*>(_this)->rx_task_loop();
}
static void start_tx_task_loop(void *_this) {
static_cast<AbstractCan*>(_this)->tx_task_loop();
static_cast<EgsBaseCan*>(_this)->tx_task_loop();
}
virtual void tx_frames(){};
virtual void on_rx_frame(uint32_t id, uint8_t dlc, uint64_t data, uint64_t timestamp) {};
virtual void on_rx_done(uint64_t now_ts){};
bool send_messages = true;
QueueHandle_t* diag_rx_queue;
private:
const char* name;
twai_status_info_t can_status;
esp_err_t can_init_status;
inline void to_bytes(uint64_t src, uint8_t* dst) {
for(uint8_t i = 0; i < 8; i++) {
dst[7-i] = src & 0xFF;
src >>= 8;
}
}
};
extern AbstractCan* egs_can_hal;
extern EgsBaseCan* egs_can_hal;
typedef uint8_t DiagCanMessage[8];

61
src/common_structs.h
View File

@ -1,9 +1,10 @@
/** @file */
#ifndef COMMON_STRUCTS_H__
#define COMMON_STRUCTS_H__
#ifndef COMMON_STRUCTS_H
#define COMMON_STRUCTS_H
#include <stdint.h>
#include "solenoids/solenoids.h"
#include "canbus/can_hal.h"
typedef int16_t pressure_map[11];
typedef float rpm_modifier_map[9];
@ -13,7 +14,7 @@ template<typename T, uint8_t MAX_SIZE> struct MovingAverage {
uint8_t sample_id;
uint64_t sum;
MovingAverage() {
MovingAverage(void) {
this->sample_id = 0;
this->sum = 0;
memset(this->readings, 0x00, sizeof(this->readings));
@ -26,7 +27,7 @@ template<typename T, uint8_t MAX_SIZE> struct MovingAverage {
this->sample_id = (this->sample_id+1) % MAX_SIZE;
}
T get_average() {
T get_average(void) {
return (float)sum / (float)MAX_SIZE;
}
};
@ -38,7 +39,7 @@ template<typename T, uint8_t MAX_SIZE> struct MovingAverage {
* change behaviour
*
*/
typedef struct {
struct SensorData{
/// Gearbox input RPM
uint16_t input_rpm;
/// Engine RPM
@ -55,6 +56,8 @@ typedef struct {
int16_t max_torque;
/// Engine torque limit minimum in Nm
int16_t min_torque;
/// Driver requested torque
int16_t driver_requested_torque;
/// Torque converter slip RPM (Calculated)
int16_t tcc_slip_rpm;
/// Last time the gearbox changed gear (in milliseconds)
@ -67,7 +70,11 @@ typedef struct {
int16_t d_output_rpm;
/// Current gearbox ratio
float gear_ratio;
} SensorData;
WheelData rr_wheel;
WheelData rl_wheel;
WheelData fr_wheel;
WheelData fl_wheel;
};
/**
* @brief A gearchange that a AbstractProfile can request
@ -92,7 +99,7 @@ enum class ProfileGearChange {
TWO_ONE,
};
typedef struct {
struct ShiftPhase{
/// Ramp down from current pressure to new pressure
uint16_t ramp_time;
/// Hold time at requested pressure
@ -101,13 +108,13 @@ typedef struct {
uint16_t spc_pressure;
/// How much to modify MPC pressure by in mBar
int16_t mpc_pressure;
} ShiftPhase;
};
/**
* @brief Shift data request structure
*
*/
typedef struct {
struct ShiftData{
Solenoid* shift_solenoid;
float torque_down_amount;
uint8_t targ_g;
@ -154,24 +161,23 @@ typedef struct {
* New clutches are locked into place
*/
ShiftPhase max_pressure_data;
} ShiftData;
};
typedef struct {
struct GearRatioLimit{
float max;
float min;
} GearRatioLimit;
};
typedef const GearRatioLimit GearboxRatioBounds[7];
// typedef const GearRatioLimit GearboxRatioBounds[7];
typedef const float FwdRatios[7];
typedef struct {
struct GearboxConfiguration{
uint16_t max_torque;
const GearRatioLimit* bounds;
const float* ratios; // 1-5 and R1+R2
} GearboxConfiguration;
} ;
typedef struct {
struct PressureMgrData{
pressure_map spc_1_2;
pressure_map mpc_1_2;
@ -205,23 +211,20 @@ typedef struct {
pressure_map working_mpc_5;
rpm_modifier_map ramp_speed_multiplier;
} PressureMgrData;
} ;
typedef struct {
// Target delta RPM per step (Each step ~= 40ms)
uint16_t target_d_rpm;
// Valid range = 1 - 10 (Auto clamped if value is outside this range) - Higher = faster shift
float shift_speed;
} ShiftCharacteristics;
struct ShiftCharacteristics{
uint16_t target_shift_time;
} ;
typedef struct {
struct TccLockupBounds{
int max_slip_rpm;
int min_slip_rpm;
} TccLockupBounds;
};
#define SR_REPORT_INTERVAL 50
#define MAX_POINTS_PER_SR_ARRAY 6000/SR_REPORT_INTERVAL // Every 50ms
typedef struct {
struct __attribute__ ((packed)) ShiftReport{
int16_t atf_temp_c;
// Target << 4 | current
uint8_t targ_curr;
@ -246,8 +249,8 @@ typedef struct {
// Flags for reporting shift errors
uint16_t flare_timestamp;
uint8_t shift_timeout;
} __attribute__ ((packed)) ShiftReport;
} ;
const int SD_Size = sizeof(ShiftReport);
static const int SD_Size = sizeof(ShiftReport);
#endif

62
src/diag/diag_data.cpp
View File

@ -3,25 +3,29 @@
#include "solenoids/solenoids.h"
#include "solenoids/constant_current.h"
#include "perf_mon.h"
#include <tcm_maths.h>
#include <tcu_maths.h>
#include "kwp2000.h"
#include "esp_core_dump.h"
DATA_GEARBOX_SENSORS get_gearbox_sensors(Gearbox* g) {
DATA_GEARBOX_SENSORS ret = {};
if (g == nullptr) {
memset(&ret, 0xFF, sizeof(ret));
return ret;
}
RpmReading d;
bool b = false;
if (!Sensors::read_input_rpm(&d, false)) {
ret.n2_rpm = 0xFFFF;
ret.n3_rpm = 0xFFFF;
ret.calculated_rpm = 0xFFFF;
} else {
if (Sensors::read_input_rpm(&d, false) == ESP_OK) {
ret.n2_rpm = d.n2_raw;
ret.n3_rpm = d.n3_raw;
ret.calculated_rpm = d.calc_rpm;
} else {
ret.n2_rpm = 0xFFFF;
ret.n3_rpm = 0xFFFF;
ret.calculated_rpm = 0xFFFF;
}
if (Sensors::parking_lock_engaged(&b)) {
if (Sensors::parking_lock_engaged(&b) == ESP_OK) {
ret.parking_lock = b;
ret.atf_temp_c = g->sensor_data.atf_temp;
} else {
@ -35,6 +39,10 @@ DATA_GEARBOX_SENSORS get_gearbox_sensors(Gearbox* g) {
DATA_SOLENOIDS get_solenoid_data(Gearbox* gb_ptr) {
DATA_SOLENOIDS ret = {};
if (gb_ptr == nullptr) {
memset(&ret, 0xFF, sizeof(ret));
return ret;
}
ret.mpc_current = sol_mpc->get_current_avg(); //sol_mpc->get_current_estimate();
ret.spc_current = sol_spc->get_current_avg();//sol_spc->get_current_estimate();
@ -63,6 +71,10 @@ DATA_SOLENOIDS get_solenoid_data(Gearbox* gb_ptr) {
DATA_PRESSURES get_pressure_data(Gearbox* gb_ptr) {
DATA_PRESSURES ret = {};
if (gb_ptr == nullptr) {
memset(&ret, 0xFF, sizeof(ret));
return ret;
}
ret.mpc_pwm = sol_mpc->get_pwm_compensated();
ret.spc_pwm = sol_spc->get_pwm_compensated();
ret.tcc_pwm = sol_tcc->get_pwm_compensated();
@ -78,21 +90,25 @@ DATA_PRESSURES get_pressure_data(Gearbox* gb_ptr) {
return ret;
}
DATA_DMA_BUFFER dump_i2s_dma() {
DATA_DMA_BUFFER dump_i2s_dma(void) {
DATA_DMA_BUFFER dma = {};
dma.dma = 0;
dma.adc_reading = sol_spc->diag_get_adc_peak_raw();
return dma;
}
DATA_CANBUS_RX get_rx_can_data(AbstractCan* can_layer) {
DATA_CANBUS_RX get_rx_can_data(EgsBaseCan* can_layer) {
DATA_CANBUS_RX ret = {};
if (can_layer == nullptr || gearbox == nullptr) {
memset(&ret, 0xFF, sizeof(ret));
return ret;
}
uint64_t now = esp_timer_get_time() / 1000;
WheelData t = can_layer->get_rear_left_wheel(now, 250);
ret.left_rear_rpm = t.current_dir == WheelDirection::SignalNotAvaliable ? 0xFFFF : t.double_rpm;
t = can_layer->get_rear_right_wheel(now, 250);
ret.right_rear_rpm = t.current_dir == WheelDirection::SignalNotAvaliable ? 0xFFFF : t.double_rpm;
WheelData t = gearbox->sensor_data.rl_wheel;
ret.left_rear_rpm = t.current_dir == WheelDirection::SignalNotAvailable ? 0xFFFF : t.double_rpm;
t = gearbox->sensor_data.rr_wheel;
ret.right_rear_rpm = t.current_dir == WheelDirection::SignalNotAvailable ? 0xFFFF : t.double_rpm;
ret.paddle_position = can_layer->get_paddle_position(now, 250);
ret.pedal_pos = can_layer->get_pedal_value(now, 250);
@ -102,6 +118,8 @@ DATA_CANBUS_RX get_rx_can_data(AbstractCan* can_layer) {
ret.max_torque = torque == INT_MAX ? 0xFFFF : (torque + 500)*4;
torque = can_layer->get_minimum_engine_torque(now, 250);
ret.min_torque = torque == INT_MAX ? 0xFFFF : (torque + 500)*4;
torque = can_layer->get_driver_engine_torque(now, 250);
ret.driver_torque = torque == INT_MAX ? 0xFFFF : (torque + 500)*4;
torque = can_layer->get_static_engine_torque(now, 250);
ret.static_torque = torque == INT_MAX ? 0xFFFF : (torque + 500)*4;
ret.shift_button_pressed = can_layer->get_profile_btn_press(now, 250);
@ -111,7 +129,7 @@ DATA_CANBUS_RX get_rx_can_data(AbstractCan* can_layer) {
return ret;
}
DATA_SYS_USAGE get_sys_usage() {
DATA_SYS_USAGE get_sys_usage(void) {
DATA_SYS_USAGE ret = {};
CpuStats s = get_cpu_stats();
ret.core1_usage = s.load_core_1;
@ -127,8 +145,12 @@ DATA_SYS_USAGE get_sys_usage() {
return ret;
}
SHIFT_LIVE_INFO get_shift_live_Data(AbstractCan* can_layer, Gearbox* g) {
SHIFT_LIVE_INFO get_shift_live_Data(const EgsBaseCan* can_layer, Gearbox* g) {
SHIFT_LIVE_INFO ret = {};
if (can_layer == nullptr || g == nullptr) {
memset(&ret, 0xFF, sizeof(ret));
return ret;
}
ret.spc_pressure = g->pressure_mgr->get_targ_spc_pressure();
ret.mpc_pressure = g->pressure_mgr->get_targ_mpc_pressure();
@ -179,14 +201,14 @@ SHIFT_LIVE_INFO get_shift_live_Data(AbstractCan* can_layer, Gearbox* g) {
}
TCM_CORE_CONFIG get_tcm_config() {
TCM_CORE_CONFIG get_tcm_config(void) {
TCM_CORE_CONFIG cfg;
memcpy(&cfg, &VEHICLE_CONFIG, sizeof(TCM_CORE_CONFIG));
return cfg;
}
uint8_t set_tcm_config(TCM_CORE_CONFIG cfg) {
ShifterPosition pos = egs_can_hal->get_shifter_position_ewm(esp_timer_get_time()/1000, 250);
kwp_result_t set_tcm_config(TCM_CORE_CONFIG cfg) {
ShifterPosition pos = egs_can_hal == nullptr ? ShifterPosition::SignalNotAvailable : egs_can_hal->get_shifter_position_ewm(esp_timer_get_time()/1000, 250);
if (
pos == ShifterPosition::D || pos == ShifterPosition::MINUS || pos == ShifterPosition::PLUS || pos == ShifterPosition::R || // Stationary positions
pos == ShifterPosition::N_D || pos == ShifterPosition::P_R || pos == ShifterPosition::R_N // Intermediate positions
@ -214,7 +236,7 @@ uint8_t set_tcm_config(TCM_CORE_CONFIG cfg) {
}
}
COREDUMP_INFO get_coredump_info() {
COREDUMP_INFO get_coredump_info(void) {
size_t addr = 0;
size_t size = 0;
esp_core_dump_image_get(&addr, &size);
@ -224,6 +246,6 @@ COREDUMP_INFO get_coredump_info() {
};
}
const esp_app_desc_t* get_image_header() {
const esp_app_desc_t* get_image_header(void) {
return esp_ota_get_app_description();
}

21
src/diag/diag_data.h
View File

@ -6,8 +6,9 @@
#include "gearbox.h"
#include "nvs/eeprom_config.h"
#include "common_structs.h"
#include "endpoint.h"
#include "endpoints/endpoint.h"
#include <esp_app_format.h>
#include "kwp2000_defines.h"
// Needed extra bytes for response, SID, RLI, Shift ID
static_assert(sizeof(ShiftReport) < DIAG_CAN_MAX_SIZE-3, "Shift report is too big to fit in Diag message!");
@ -30,6 +31,7 @@ static_assert(sizeof(ShiftReport) < DIAG_CAN_MAX_SIZE-3, "Shift report is too bi
#define RLI_DMA_DUMP 0x26
#define RLI_SHIFT_LIVE 0x27
#define RLI_FW_HEADER 0x28
#define RLI_EFUSE_CONFIG 0xFD // TCM Configuration (PCB Config in EFUSE)
#define RLI_TCM_CONFIG 0xFE // TCM configuration (AKA SCN)
// Gearbox sensor struct
@ -79,6 +81,7 @@ typedef struct {
uint16_t min_torque;
uint16_t max_torque;
uint16_t static_torque;
uint16_t driver_torque;
uint16_t left_rear_rpm;
uint16_t right_rear_rpm;
uint8_t shift_button_pressed;
@ -126,17 +129,17 @@ typedef struct {
DATA_GEARBOX_SENSORS get_gearbox_sensors(Gearbox* g);
DATA_SOLENOIDS get_solenoid_data(Gearbox* gb_ptr);
DATA_PRESSURES get_pressure_data(Gearbox* gb_ptr);
DATA_CANBUS_RX get_rx_can_data(AbstractCan* can_layer);
DATA_SYS_USAGE get_sys_usage();
DATA_DMA_BUFFER dump_i2s_dma();
SHIFT_LIVE_INFO get_shift_live_Data(AbstractCan* can_layer, Gearbox* g);
DATA_CANBUS_RX get_rx_can_data(EgsBaseCan* can_layer);
DATA_SYS_USAGE get_sys_usage(void);
DATA_DMA_BUFFER dump_i2s_dma(void);
SHIFT_LIVE_INFO get_shift_live_Data(const EgsBaseCan* can_layer, Gearbox* g);
// Read and write SCN config
TCM_CORE_CONFIG get_tcm_config();
uint8_t set_tcm_config(TCM_CORE_CONFIG cfg);
TCM_CORE_CONFIG get_tcm_config(void);
kwp_result_t set_tcm_config(TCM_CORE_CONFIG cfg);
COREDUMP_INFO get_coredump_info();
COREDUMP_INFO get_coredump_info(void);
const esp_app_desc_t* get_image_header();
const esp_app_desc_t* get_image_header(void);
#endif // __DIAG_DATA_H__

77
src/diag/egs_emulation.cpp
View File

@ -1,46 +1,56 @@
#include "egs_emulation.h"
#include "sensors.h"
#include "../pressure_manager.h"
RLI_31_DATA get_rli_31(AbstractCan* can_layer) {
inline uint16_t flip_uint16_t(uint16_t x) {
return ((x & 0xff) << 8) | ((x & 0xff00) >> 8);
}
RLI_30_DATA get_rli_30(EgsBaseCan* can_layer) {
RLI_30_DATA ret = {};
memset(&ret, 0x00, sizeof(RLI_30_DATA));
return ret;
}
RLI_31_DATA get_rli_31(EgsBaseCan* can_layer) {
RLI_31_DATA ret = {};
uint64_t now = esp_timer_get_time() / 1000;
RpmReading d;
if (!Sensors::read_input_rpm(&d, false)) {
ret.n2_pulse_count = 0xFFFF;
ret.n3_pulse_count = 0xFFFF;
ret.input_rpm = 0xFFFF;
} else {
if (Sensors::read_input_rpm(&d, false) == ESP_OK) {
ret.n2_pulse_count = d.n2_raw;
ret.n3_pulse_count = d.n3_raw;
ret.input_rpm = d.calc_rpm;
} else {
ret.n2_pulse_count = 0xFFFF;
ret.n3_pulse_count = 0xFFFF;
ret.input_rpm = 0xFFFF;
}
ret.vehicle_speed_rear_wheels = 0; // TODO
ret.vehicle_speed_front_wheels = 0; // TODO
WheelData wd = {};
wd = can_layer->get_front_left_wheel(now, 300);
if (wd.current_dir == WheelDirection::SignalNotAvaliable) {
WheelData wd = can_layer->get_front_left_wheel(now, 300);
if (wd.current_dir == WheelDirection::SignalNotAvailable) {
ret.front_left_wheel_speed = 0xFFFF;
} else {
ret.front_left_wheel_speed = wd.double_rpm / 2.0;
}
wd = can_layer->get_front_right_wheel(now, 300);
if (wd.current_dir == WheelDirection::SignalNotAvaliable) {
if (wd.current_dir == WheelDirection::SignalNotAvailable) {
ret.front_right_wheel_speed = 0xFFFF;
} else {
ret.front_right_wheel_speed = wd.double_rpm / 2.0;
}
wd = can_layer->get_rear_left_wheel(now, 300);
if (wd.current_dir == WheelDirection::SignalNotAvaliable) {
if (wd.current_dir == WheelDirection::SignalNotAvailable) {
ret.rear_left_wheel_speed = 0xFFFF;
} else {
ret.rear_left_wheel_speed = wd.double_rpm / 2.0;
}
wd = can_layer->get_rear_right_wheel(now, 300);
if (wd.current_dir == WheelDirection::SignalNotAvaliable) {
if (wd.current_dir == WheelDirection::SignalNotAvailable) {
ret.rear_right_wheel_speed = 0xFFFF;
} else {
ret.rear_right_wheel_speed = wd.double_rpm / 2.0;
@ -48,5 +58,48 @@ RLI_31_DATA get_rli_31(AbstractCan* can_layer) {
ret.engine_speed = can_layer->get_engine_rpm(now, 300);
return ret;
}
RLI_32_DATA get_rli_32(EgsBaseCan* can_layer) {
RLI_32_DATA ret = {};
memset(&ret, 0x00, sizeof(RLI_32_DATA));
return ret;
}
RLI_33_DATA get_rli_33(EgsBaseCan* can_layer) {
RLI_33_DATA ret = {};
memset(&ret, 0x00, sizeof(RLI_33_DATA));
ret.mpc_pressure = flip_uint16_t(pressure_manager->get_targ_mpc_pressure());
ret.spc_pressure = flip_uint16_t(pressure_manager->get_targ_spc_pressure());
ret.mpc_target_current = flip_uint16_t(pressure_manager->get_targ_mpc_current());
ret.spc_target_current = flip_uint16_t(pressure_manager->get_targ_spc_current());
ret.mpc_actual_current = flip_uint16_t(sol_mpc->get_current_avg());
ret.spc_actual_current = flip_uint16_t(sol_spc->get_current_avg());
ret.tcc_pwm_255 = (uint8_t)(sol_tcc->get_pwm_raw() >> 4);
bool _1245 = sol_y3->get_pwm_raw() > 10;
bool _23 = sol_y5->get_pwm_raw() > 10;
bool _34 = sol_y4->get_pwm_raw() > 10;
if (_1245 && !_23 && !_34) {
ret.shift_valve_state = ShiftValveStatus::_1245;
} else if (_23 && !_1245 && !_34) {
ret.shift_valve_state = ShiftValveStatus::_23;
} else if (_34 && !_1245 && !_23) {
ret.shift_valve_state = ShiftValveStatus::_34;
} else if (_1245 && _23 && !_34) {
ret.shift_valve_state = ShiftValveStatus::_1245_and_23;
} else if (_1245 && _34 && !_23) {
ret.shift_valve_state = ShiftValveStatus::_1245_and_34;
} else if (_23 && _34 && !_1245) {
ret.shift_valve_state = ShiftValveStatus::_23_and_34;
} else if (_1245 && _23 && _34) {
ret.shift_valve_state = ShiftValveStatus::_1245_and_23_and_34;
} else {
ret.shift_valve_state = ShiftValveStatus::No;
}
ret.valve_flag = _1245 || _23 || _34;
return ret;
}

127
src/diag/egs_emulation.h
View File

@ -1,14 +1,101 @@
#ifndef __EGS_EMULATION_H_
#define __EGS_EMULATION_H_
#ifndef EGS_EMULATION_H
#define EGS_EMULATION_H
/// Data structures for EGS52 emulation in DAS
#include <stdint.h>
#include "canbus/can_hal.h"
#define RLI_30 0x30
#define RLI_31 0x31 // 7
#define RLI_35 0x35 // 16
// #define RLI_30 0x30
// #define RLI_31 0x31 // 7
static const uint16_t RLI_31 = 0x31;
// #define RLI_35 0x35 // 16
enum class RecognisedGear : uint8_t {
Inactive = 0,
D1 = 1,
D2 = 2,
D3 = 3,
D4 = 4,
D5 = 5,
R = 6,
R2 = 7,
WrongGear = 23,
Calculating = 88,
SNV = 255
};
enum class TccState : uint8_t {
Open = 0,
OpenSlipping = 1,
SlippingOpen = 2,
Slipping = 3,
SlippingLocked = 4,
LockedSlipping = 5,
Locked = 6
};
enum class ShiftValveStatus : uint8_t {
No = 0,
_1245 = 1,
_23 = 2,
_1245_and_23 = 3,
_34 = 4,
_1245_and_34 = 5,
_23_and_34 = 6,
_1245_and_23_and_34 = 7
};
typedef struct {
uint16_t tcc_delta_speed; // 16..31
uint16_t tcc_speed; // 32-47
uint16_t tcc_pressure; // 48-63
TccState tcc_status: 8; // 64-71
uint8_t ewm_position; // 72-79
uint8_t program;// 80-87
RecognisedGear gear_recognised: 8; // 88-95
uint8_t actual_gear: 4; // 96..99
uint8_t target_gear: 4; // 100..103
uint8_t atf_temp: 8; // 104..111
uint16_t motor_torque: 16; // 112..127
uint16_t converter_torque: 16; // 128..143
uint16_t output_speed; // 144-159
bool kickdown: 1; // 160
bool alf: 1; // 161
bool start_lockout_reason: 1; // 162
bool rp_lock: 1; // 163
bool starter_relay: 1; // 164
bool solenoid_1245: 1; // 165
bool solenoid_23: 1; // 166
bool solenoid_34: 1; // 167
bool ewm_prg_btn: 1; // 168
bool ewm_plus: 1; // 169
bool ewm_minus: 1; // 170
bool not_switching_bit: 1; // 171
bool gear_protection: 1; // 172
bool tcc_open_request: 1; // 173
uint8_t padding_2: 2; // 174..175
bool downshift: 1; // 176
bool upshift: 1; // 177
bool dyn0_amr_egs: 1; // 178
bool dyn1_amr_egs: 1; // 179
bool release_circuit: 1; // 180
bool wheel_plus_btn: 1; // 181
bool wheel_minus_btn: 1; // 182
bool converter_clutch_en: 1; // 183
bool error_current: 1; // 184
bool emergency_mode: 1; // 185
bool asr_active: 1; // 186
bool transmission_protection_ack: 1; // 187
bool bang_start: 1; // 188
bool min_trq_egs: 1; // 189
bool circuit_break: 1; // 190
bool double_circuit: 1; // 191
uint16_t padding_9: 9; // 192..200
bool converter_ok: 1; // 201
} __attribute__ ((packed)) RLI_30_DATA;
typedef struct {
uint16_t n2_pulse_count;
@ -24,12 +111,34 @@ typedef struct {
} __attribute__ ((packed)) RLI_31_DATA;
typedef struct {
uint8_t shifter_position;
uint8_t id_608_tc_status;
uint8_t pedal_percent; // 16..23
uint16_t delta_rpm_upshift; // 24..39
uint16_t delta_rpm_downshift; // 40..55
uint8_t pedal_delta_percent; // 56..63
uint16_t pitch; // 64..79
uint8_t driving_staus_ident; // 80..87
uint8_t switching_warming_engine; // 88..95
uint8_t low_request_gear_range_restrict; // 96..103
uint8_t max_request_gear_range_restrict; // 104..111
} __attribute__ ((packed)) RLI_32_DATA;
typedef struct {
uint8_t valve_flag; // 16..23
ShiftValveStatus shift_valve_state; //24..31
uint16_t spc_pressure; // 32..47
uint16_t mpc_pressure; // 48..63
uint16_t spc_target_current; // 64..79
uint16_t spc_actual_current; // 80..95
uint16_t mpc_target_current; // 96..111
uint16_t mpc_actual_current; // 112..127
uint8_t tcc_pwm_255; // 128..135
} __attribute__ ((packed)) RLI_33_DATA;
RLI_31_DATA get_rli_31(AbstractCan* can_layer);
RLI_33_DATA get_rli_33(EgsBaseCan* can_layer);
RLI_32_DATA get_rli_32(EgsBaseCan* can_layer);
RLI_31_DATA get_rli_31(EgsBaseCan* can_layer);
RLI_30_DATA get_rli_30(EgsBaseCan* can_layer);
#endif

181
src/diag/endpoint.h
View File

@ -1,181 +0,0 @@
#ifndef _DIAG_ENDPOINT_H__
#define _DIAG_ENDPOINT_H__
#include <stdint.h>
#include <driver/uart.h>
#include "esp_log.h"
#include "string.h"
#include "canbus/can_hal.h"
#include "esp_core_dump.h"
#include "tcm_maths.h"
#include "driver/twai.h"
#define DIAG_CAN_MAX_SIZE 4095 // ISO-TP Maximum
const char HEX_DEF[17] = "0123456789ABCDEF";
typedef struct {
uint16_t id;
uint16_t data_size;
uint8_t data[DIAG_CAN_MAX_SIZE]; // 512B messages max (EGS52/3 is always max 256 bytes)
} DiagMessage;
typedef struct {
uint8_t data[DIAG_CAN_MAX_SIZE];
uint16_t curr_pos;
uint16_t max_pos;
} CanEndpointMsg;
/**
* @brief Abstract endpoint
*
*/
class AbstractEndpoint {
public:
AbstractEndpoint(){};
virtual void send_data(DiagMessage* msg); // Blocking operation
virtual bool read_data(DiagMessage* dest);
};
/**
* @brief Endpoint for USB communication with ultimate-nag52 custom USB util
*
*/
#define UART_NUM
const static size_t UART_MSG_SIZE = 6+(2*DIAG_CAN_MAX_SIZE);
class UsbEndpoint: public AbstractEndpoint {
public:
UsbEndpoint(bool can_use_spiram) : AbstractEndpoint() {
esp_err_t e;
this->allocation_psram = can_use_spiram;
e = uart_driver_install(0, UART_MSG_SIZE/2, UART_MSG_SIZE/2, 0, nullptr, 0);
if (e != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "USBEndpoint","Error installing UART driver: %s", esp_err_to_name(e));
return;
}
if (this->allocation_psram) {
this->read_buffer = (char*)heap_caps_malloc(UART_MSG_SIZE, MALLOC_CAP_SPIRAM);
this->write_buffer = (char*)heap_caps_malloc(UART_MSG_SIZE, MALLOC_CAP_SPIRAM);
} else {
this->read_buffer = (char*)malloc(UART_MSG_SIZE);
this->write_buffer = (char*)malloc(UART_MSG_SIZE);
}
if (this->read_buffer == nullptr) {
return;
}
uart_flush(0);
this->read_pos = 0;
}
void send_data(DiagMessage* msg) override {
this->write_buffer[0] = '#';
this->write_buffer[1] = HEX_DEF[(msg->id >> 12) & 0x0F];
this->write_buffer[2] = HEX_DEF[(msg->id >> 8) & 0x0F];
this->write_buffer[3] = HEX_DEF[(msg->id >> 4) & 0x0F];
this->write_buffer[4] = HEX_DEF[msg->id & 0x0F];
for (uint16_t i = 0; i < msg->data_size; i++) {
this->write_buffer[5+(i*2)] = HEX_DEF[(msg->data[i] >> 4) & 0x0F];
this->write_buffer[6+(i*2)] = HEX_DEF[msg->data[i] & 0x0F];
}
this->write_buffer[(msg->data_size*2)+5] = '\n';
uart_write_bytes(0, &this->write_buffer[0], (msg->data_size*2)+6);
}
bool read_data(DiagMessage* dest) override {
this->length = 0;
uart_get_buffered_data_len(0, &length);
if (length != 0) {
max_bytes_left = UART_MSG_SIZE - this->read_pos;
to_read = MIN(length, max_bytes_left);
uart_read_bytes(0, &this->read_buffer[this->read_pos], to_read, 0);
this->read_pos += length;
return false;
} else if (this->read_pos != 0) {
if (this->read_pos < 5) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "USBEndpoint", "Corrupt incoming msg. Less than 5 bytes");
this->read_pos = 0;
return false;
} else {
uint16_t read_size = (this->read_buffer[0] << 8) | this->read_buffer[1];
if (read_size != this->read_pos - 2) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "USBEndpoint", "Corrupt incoming msg. Msg size is %d bytes, buffer has %d bytes", read_size, this->read_pos-2);
this->read_pos = 0;
return false;
} else {
// Valid msg!
dest->id = (this->read_buffer[2] << 8) | this->read_buffer[3];
dest->data_size = read_size-2;
memcpy(dest->data, &this->read_buffer[4], dest->data_size);
this->read_pos = 0;
return true;
}
}
}
return false;
}
private:
// NOTE TO SELF
// Every USB MSG:
// {ID: 0x07E0, Data: [0x00, 0x11, 0x22, 0x33]} = '#07E100112233\n'
// Read msg size: 6 bytes: USB message size: 14 = (Read size *2) + 2
char* read_buffer;
char* write_buffer;
uint16_t read_pos;
bool clear_to_send = false;
bool allocation_psram = false;
int data_size;
int line_idx;
int max_bytes_left;
int to_read;
size_t length;
};
/**
* @brief Endpoint for ISO-TP communication with OBD readers
*
*/
class CanEndpoint: public AbstractEndpoint {
public:
CanEndpoint(AbstractCan* can_layer);
void send_data(DiagMessage* msg) override;
bool read_data(DiagMessage* dest) override;
static void start_iso_tp(void *_this) {
static_cast<CanEndpoint*>(_this)->iso_tp_server_loop();
}
private:
void process_single_frame(DiagCanMessage msg);
void process_start_frame(DiagCanMessage msg);
void process_multi_frame(DiagCanMessage msg);
void process_flow_control(DiagCanMessage msg);
[[noreturn]]
void iso_tp_server_loop();
AbstractCan* can;
QueueHandle_t rx_queue;
//QueueHandle_t tx_queue;
CanEndpointMsg tx_msg;
CanEndpointMsg rx_msg;
QueueHandle_t read_msg_queue;
QueueHandle_t send_msg_queue;
CanEndpointMsg tmp;
bool is_sending;
bool clear_to_send;
bool is_receiving;
uint8_t rx_bs;
uint8_t tx_pci = 0x20;
uint64_t last_rx_time;
uint64_t last_tx_time;
uint8_t tx_bs = 8;
uint8_t tx_stmin = 20;
uint8_t frames_received = 0;
twai_message_t tx_can;
uint8_t tx_count = 0;
bool send_to_twai(DiagCanMessage msg);
};
#endif

33
src/diag/can_endpoint.cpp → src/diag/endpoints/can_endpoint.cpp
View File

@ -1,5 +1,5 @@
#include "endpoint.h"
#include "kwp2000_defines.h"
#include "../kwp2000_defines.h"
#include "driver/twai.h"
const DiagCanMessage FLOW_CONTROL = {0x30, KWP_CAN_BS, KWP_CAN_ST_MIN, 0, 0, 0, 0, 0};
@ -8,8 +8,12 @@ const DiagCanMessage FLOW_CONTROL_OVERFLOW = {0x32, 0, 0, 0, 0, 0, 0, 0};
#define ISO_TP_TIMEOUT 2000
CanEndpoint::CanEndpoint(AbstractCan* can_layer) {
CanEndpoint::CanEndpoint(EgsBaseCan* can_layer) {
this->can = can_layer;
if (nullptr == this->can) {
this->status = ESP_ERR_INVALID_ARG;
return;
}
memset(&tx_can, 0x00, sizeof(twai_message_t));
this->tx_can.data_length_code = 8;
this->tx_can.identifier = KWP_ECU_TX_ID;
@ -23,6 +27,11 @@ CanEndpoint::CanEndpoint(AbstractCan* can_layer) {
this->last_rx_time = 0;
this->last_tx_time = 0;
can_layer->register_diag_queue(&this->rx_queue, KWP_ECU_RX_ID);
this->status = ESP_OK;
}
esp_err_t CanEndpoint::init_state() {
return this->status;
}
bool CanEndpoint::send_to_twai(DiagCanMessage msg) {
@ -53,9 +62,8 @@ void CanEndpoint::iso_tp_server_loop() {
// This loop deals with sending and receiving data from ISO-TP
DiagCanMessage rx;
esp_err_t res;
uint64_t now = esp_timer_get_time() / 1000;
while(1) {
now = esp_timer_get_time() / 1000;
uint64_t now = esp_timer_get_time() / 1000;
while (xQueueReceive(this->rx_queue, rx, 0) == pdTRUE) {
this->last_rx_time = now;
switch (rx[0] & 0xF0) { // Check incoming frame PCI
@ -110,7 +118,8 @@ void CanEndpoint::iso_tp_server_loop() {
}
}
if (is_sending && clear_to_send && (now-this->last_tx_time >= KWP_CAN_ST_MIN)) {
// if (is_sending && clear_to_send && (now-this->last_tx_time >= KWP_CAN_ST_MIN)) {
if (is_sending && clear_to_send ) {
uint8_t max_cpy = tx_msg.max_pos-tx_msg.curr_pos;
if (max_cpy > 7) {
max_cpy = 7;
@ -174,12 +183,12 @@ void CanEndpoint::process_single_frame(DiagCanMessage msg) {
void CanEndpoint::process_start_frame(DiagCanMessage msg) {
if (this->is_receiving) {
send_to_twai((uint8_t*)FLOW_CONTROL_BUSY);
send_to_twai(const_cast<uint8_t*>(FLOW_CONTROL_BUSY));
return;
}
uint16_t size = (msg[0] & 0x0F) << 8 | msg[1];
if (size > DIAG_CAN_MAX_SIZE) {
send_to_twai((uint8_t*)FLOW_CONTROL_OVERFLOW);
send_to_twai(const_cast<uint8_t*>(FLOW_CONTROL_OVERFLOW));
return;
}
// Not busy receiving and message size fits
@ -188,7 +197,7 @@ void CanEndpoint::process_start_frame(DiagCanMessage msg) {
this->rx_msg.max_pos = size;
this->frames_received = 0;
memcpy(rx_msg.data, &msg[2], 6);
if (!this->send_to_twai((uint8_t*)FLOW_CONTROL)) {
if (!this->send_to_twai(const_cast<uint8_t*>(FLOW_CONTROL))) {
this->is_receiving = false; // Set if could not send Tx Frame
}
}
@ -208,8 +217,10 @@ void CanEndpoint::process_multi_frame(DiagCanMessage msg) {
if (xQueueSend(this->read_msg_queue, &this->rx_msg, 0) != pdTRUE) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "CanEndpoint_psf", "Tx queue is full!?");
}
} else if ((this->frames_received >= KWP_CAN_BS) && KWP_CAN_BS != 0) { // Send flow control when we overflow
if (!this->send_to_twai((uint8_t*)FLOW_CONTROL)) {
// } else if ((this->frames_received >= KWP_CAN_BS) && KWP_CAN_BS != 0) { // Send flow control when we overflow
} else if (KWP_CAN_BS != 0) { // Send flow control when we overflow
// if (!this->send_to_twai((uint8_t*)FLOW_CONTROL)) {
if (!this->send_to_twai(const_cast<uint8_t*>(FLOW_CONTROL))) {
this->is_receiving = false; // Set if could not send Tx Frame
}
this->frames_received = 0;
@ -219,7 +230,7 @@ void CanEndpoint::process_multi_frame(DiagCanMessage msg) {
}
void CanEndpoint::process_flow_control(DiagCanMessage msg) {
ESP_LOGI("CAN_ENDPOINT", "FC Received! BS %d STMIN %d", msg[1], msg[2]);
//ESP_LOGI("CAN_ENDPOINT", "FC Received! BS %d STMIN %d", msg[1], msg[2]);
if (msg[0] == 0x30 && this->is_sending) {
this->clear_to_send = true;
this->tx_bs = msg[1];

121
src/diag/endpoints/endpoint.h Normal file
View File

@ -0,0 +1,121 @@
#ifndef DIAG_ENDPOINT_H
#define DIAG_ENDPOINT_H
#include <stdint.h>
#include <driver/uart.h>
#include "esp_log.h"
#include "string.h"
#include "canbus/can_hal.h"
#include "esp_core_dump.h"
#include "tcu_maths.h"
#include "driver/twai.h"
#include "esp_err.h"
// #define DIAG_CAN_MAX_SIZE 4095 // ISO-TP Maximum
static const uint16_t DIAG_CAN_MAX_SIZE = 4095u; // ISO-TP Maximum
struct DiagMessage
{
uint16_t id;
uint16_t data_size;
uint8_t data[DIAG_CAN_MAX_SIZE]; // 512B messages max (EGS52/3 is always max 256 bytes)
};
struct CanEndpointMsg
{
uint8_t data[DIAG_CAN_MAX_SIZE];
uint16_t curr_pos;
uint16_t max_pos;
};
/**
* @brief Abstract endpoint
*
*/
class AbstractEndpoint
{
public:
AbstractEndpoint(void){};
virtual void send_data(DiagMessage *msg); // Blocking operation
virtual bool read_data(DiagMessage *dest);
virtual esp_err_t init_state();
};
/**
* @brief Endpoint for USB communication with ultimate-nag52 custom USB util
*
*/
class UsbEndpoint : public AbstractEndpoint
{
public:
explicit UsbEndpoint();
void send_data(DiagMessage *msg) override;
bool read_data(DiagMessage *dest) override;
esp_err_t init_state() override;
private:
// NOTE TO SELF
// Every USB MSG:
// {ID: 0x07E0, Data: [0x00, 0x11, 0x22, 0x33]} = '#07E100112233\n'
// Read msg size: 6 bytes: USB message size: 14 = (Read size *2) + 2
char *read_buffer;
char *write_buffer;
uint16_t read_pos;
bool clear_to_send = false;
int data_size;
int line_idx;
int max_bytes_left;
int to_read;
size_t length;
esp_err_t status;
};
/**
* @brief Endpoint for ISO-TP communication with OBD readers
*
*/
class CanEndpoint : public AbstractEndpoint
{
public:
explicit CanEndpoint(EgsBaseCan *can_layer);
void send_data(DiagMessage *msg) override;
bool read_data(DiagMessage *dest) override;
esp_err_t init_state() override;
static void start_iso_tp(void *_this)
{
static_cast<CanEndpoint *>(_this)->iso_tp_server_loop();
}
private:
void process_single_frame(DiagCanMessage msg);
void process_start_frame(DiagCanMessage msg);
void process_multi_frame(DiagCanMessage msg);
void process_flow_control(DiagCanMessage msg);
[[noreturn]] void iso_tp_server_loop();
EgsBaseCan *can;
QueueHandle_t rx_queue;
// QueueHandle_t tx_queue;
CanEndpointMsg tx_msg;
CanEndpointMsg rx_msg;
QueueHandle_t read_msg_queue;
QueueHandle_t send_msg_queue;
CanEndpointMsg tmp;
bool is_sending;
bool clear_to_send;
bool is_receiving;
uint8_t rx_bs;
uint8_t tx_pci = 0x20;
uint64_t last_rx_time;
uint64_t last_tx_time;
uint8_t tx_bs = 8;
uint8_t tx_stmin = 20;
uint8_t frames_received = 0;
twai_message_t tx_can;
uint8_t tx_count = 0;
esp_err_t status;
bool send_to_twai(DiagCanMessage msg);
};
#endif

90
src/diag/endpoints/usb_endpoint.cpp Normal file
View File

@ -0,0 +1,90 @@
#include "endpoint.h"
const static char HEX_DEF[17] = "0123456789ABCDEF";
const static size_t UART_MSG_SIZE = 6 + (2 * DIAG_CAN_MAX_SIZE);
UsbEndpoint::UsbEndpoint() : AbstractEndpoint()
{
data_size = 0;
line_idx = 0;
max_bytes_left = 0;
to_read = 0;
length = 0;
this->status = uart_driver_install(0, UART_MSG_SIZE / 2u, UART_MSG_SIZE / 2u, 0, nullptr, 0);
if (this->status == ESP_OK)
{
this->read_buffer = static_cast<char *>(heap_caps_malloc(UART_MSG_SIZE, MALLOC_CAP_SPIRAM));
this->write_buffer = static_cast<char *>(heap_caps_malloc(UART_MSG_SIZE, MALLOC_CAP_SPIRAM));
if (nullptr != this->read_buffer && nullptr != this->write_buffer)
{
uart_flush(0);
this->read_pos = 0;
} else {
this->status = ESP_ERR_NO_MEM;
}
}
}
esp_err_t UsbEndpoint::init_state() {
return this->status;
}
void UsbEndpoint::send_data(DiagMessage *msg)
{
this->write_buffer[0] = '#';
this->write_buffer[1] = HEX_DEF[(msg->id >> 12) & 0x0F];
this->write_buffer[2] = HEX_DEF[(msg->id >> 8) & 0x0F];
this->write_buffer[3] = HEX_DEF[(msg->id >> 4) & 0x0F];
this->write_buffer[4] = HEX_DEF[msg->id & 0x0F];
for (uint16_t i = 0; i < msg->data_size; i++)
{
this->write_buffer[5 + (i * 2)] = HEX_DEF[(msg->data[i] >> 4) & 0x0F];
this->write_buffer[6 + (i * 2)] = HEX_DEF[msg->data[i] & 0x0F];
}
this->write_buffer[(msg->data_size * 2) + 5] = '\n';
uart_write_bytes(0, &this->write_buffer[0], (msg->data_size * 2) + 6);
}
bool UsbEndpoint::read_data(DiagMessage *dest)
{
this->length = 0;
uart_get_buffered_data_len(0, &length);
if (length != 0)
{
max_bytes_left = UART_MSG_SIZE - this->read_pos;
to_read = MIN(length, max_bytes_left);
uart_read_bytes(0, &this->read_buffer[this->read_pos], to_read, 0);
this->read_pos += length;
return false;
}
else if (this->read_pos != 0)
{
if (this->read_pos < 5)
{
ESP_LOG_LEVEL(ESP_LOG_ERROR, "USBEndpoint", "Corrupt incoming msg. Less than 5 bytes");
this->read_pos = 0;
return false;
}
else
{
uint16_t read_size = (this->read_buffer[0] << 8) | this->read_buffer[1];
if (read_size != this->read_pos - 2)
{
ESP_LOG_LEVEL(ESP_LOG_ERROR, "USBEndpoint", "Corrupt incoming msg. Msg size is %d bytes, buffer has %d bytes", read_size, this->read_pos - 2);
this->read_pos = 0;
return false;
}
else
{
// Valid msg!
dest->id = (this->read_buffer[2] << 8) | this->read_buffer[3];
dest->data_size = read_size - 2;
memcpy(dest->data, &this->read_buffer[4], dest->data_size);
this->read_pos = 0;
return true;
}
}
}
return false;
}

59
src/diag/flasher.cpp
View File

@ -2,26 +2,38 @@
#include "esp_timer.h"
#include "speaker.h"
#include "esp_partition.h"
#include "tcm_maths.h"
#include "tcu_maths.h"
Flasher::Flasher(AbstractCan *can_ref, Gearbox* gearbox) {
Flasher::Flasher(EgsBaseCan *can_ref, Gearbox* gearbox) {
this->can_ref = can_ref;
this->gearbox_ref = gearbox;
update_partition = nullptr;
read_base_addr = 0u;
read_bytes = 0u;
read_bytes_total = 0u;
}
Flasher::~Flasher() {
this->gearbox_ref->diag_regain_control(); // Re-enable engine starting
}
DiagMessage Flasher::on_request_download(uint8_t* args, uint16_t arg_len) {
/**
* "This is a function that handles a Request Download diagnostic message in a vehicle's
* onboard diagnostic system. The function checks the current state of the vehicle's shifter
* and engine, and then parses the request data to determine the destination memory address,
* data format, and uncompressed memory size. If the request is valid, it prepares the vehicle
* for data transfer by disabling the gearbox controller and setting the appropriate drive
* profile and display gear. It also sets up a block counter to track the progress of the data
* transfer. The function then returns a positive response message containing the maximum
* number of data bytes that can be transferred in a single block." - ChatGPT
*/
DiagMessage Flasher::on_request_download(const uint8_t* args, uint16_t arg_len) {
// Shifter must be Offline (SNV) or P or N
if (!is_shifter_passive(this->can_ref)) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "FLASHER", "Rejecting download request. Shifter not in valid position");
return this->make_diag_neg_msg(SID_REQ_DOWNLOAD, NRC_CONDITIONS_NOT_CORRECT_REQ_SEQ_ERROR);
return this->make_diag_neg_msg(SID_REQ_DOWNLOAD, NRC_UN52_SHIFTER_ACTIVE);
}
if (!is_engine_off(this->can_ref)) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "FLASHER", "Rejecting download request. Engine is on");
return this->make_diag_neg_msg(SID_REQ_DOWNLOAD, NRC_CONDITIONS_NOT_CORRECT_REQ_SEQ_ERROR);
return this->make_diag_neg_msg(SID_REQ_DOWNLOAD, NRC_UN52_ENGINE_ON);
}
// Format [MA, MA, MA, FF, MS, MS, MS]
// Dest memory address
@ -34,23 +46,19 @@ DiagMessage Flasher::on_request_download(uint8_t* args, uint16_t arg_len) {
uint32_t dest_mem_address = args[0] << 16 | args[1] << 8 | args[2];
uint8_t fmt = args[3];
uint32_t dest_mem_size = args[4] << 16 | args[5] << 8 | args[6];
ESP_LOG_LEVEL(ESP_LOG_INFO, "FLASHER", "DEST %08X, Format is %02X and size is %08X", dest_mem_address, fmt, dest_mem_size);
// Valid memory regions
if (dest_mem_address == MEM_REGION_OTA) {
// Init OTA system
this->update_partition = esp_ota_get_next_update_partition(NULL);
this->update_type = UPDATE_TYPE_OTA;
if (this->update_partition == nullptr) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "FLASHER", "Target update partition was NULL!");
return this->make_diag_neg_msg(SID_REQ_DOWNLOAD, NRC_GENERAL_REJECT);
return this->make_diag_neg_msg(SID_REQ_DOWNLOAD, NRC_UN52_NULL_PARTITION);
}
this->update_handle = 0;
esp_err_t err = esp_ota_begin(this->update_partition, dest_mem_size, &update_handle);
if (err != ESP_OK) {
// HUH!?
ESP_LOG_LEVEL(ESP_LOG_ERROR, "FLASHER", "esp_ota_begin failed! %s", esp_err_to_name(err));
esp_ota_abort(this->update_handle);
return this->make_diag_neg_msg(SID_REQ_DOWNLOAD, NRC_GENERAL_REJECT);
return this->make_diag_neg_msg(SID_REQ_DOWNLOAD, NRC_UN52_OTA_BEGIN_FAIL);
}
} else { // Invalid memory region
return this->make_diag_neg_msg(SID_REQ_DOWNLOAD, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT);
@ -65,35 +73,30 @@ DiagMessage Flasher::on_request_download(uint8_t* args, uint16_t arg_len) {
uint8_t resp[2] = { 0x00, 0x00 };
resp[0] = CHUNK_SIZE >> 8 & 0xFF;
resp[1] = CHUNK_SIZE & 0xFF;
spkr.send_note(1000, 100, 100);
this->written_data = 0;
this->data_dir = DATA_DIR_DOWNLOAD;
return this->make_diag_pos_msg(SID_REQ_DOWNLOAD, resp, 2);
}
DiagMessage Flasher::on_request_upload(uint8_t* args, uint16_t arg_len) {
DiagMessage Flasher::on_request_upload(const uint8_t* args, uint16_t arg_len) {
// Shifter must be Offline (SNV) or P or N
if (!is_shifter_passive(this->can_ref)) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "FLASHER", "Rejecting download request. Shifter not in valid position");
return this->make_diag_neg_msg(SID_REQ_DOWNLOAD, NRC_CONDITIONS_NOT_CORRECT_REQ_SEQ_ERROR);
return this->make_diag_neg_msg(SID_REQ_DOWNLOAD, NRC_UN52_SHIFTER_ACTIVE);
}
if (!is_engine_off(this->can_ref)) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "FLASHER", "Rejecting download request. Engine is on");
return this->make_diag_neg_msg(SID_REQ_DOWNLOAD, NRC_CONDITIONS_NOT_CORRECT_REQ_SEQ_ERROR);
return this->make_diag_neg_msg(SID_REQ_DOWNLOAD, NRC_UN52_ENGINE_ON);
}
// Format [MA, MA, MA, FF, MS, MS, MS]
// Dest memory address
// data format
// Uncompressed memory size
// For now we only support 0x00 format (Uncompressed and unencrypted)
ESP_LOG_LEVEL(ESP_LOG_INFO, "FLASHER", "Upload requested. Arg size %d", arg_len);
if (arg_len != 7) { // Request was the wrong size
return this->make_diag_neg_msg(SID_REQ_UPLOAD, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT);
}
uint32_t src_mem_address = args[0] << 16 | args[1] << 8 | args[2];
uint8_t fmt = args[3];
uint32_t src_mem_size = args[4] << 16 | args[5] << 8 | args[6];
ESP_LOG_LEVEL(ESP_LOG_INFO, "FLASHER", "DEST %08X, Format is %02X and size is %08X", src_mem_address, fmt, src_mem_size);
// Valid memory regions
if (src_mem_address == MEM_REGION_COREDUMP) {
this->update_type = UPDATE_TYPE_COREDUMP;
@ -111,7 +114,7 @@ DiagMessage Flasher::on_request_upload(uint8_t* args, uint16_t arg_len) {
uint8_t resp[2] = { 0x00, 0x00 };
resp[0] = CHUNK_SIZE >> 8 & 0xFF;
resp[1] = CHUNK_SIZE & 0xFF;
spkr.send_note(1000, 100, 100);
spkr->send_note(1000, 100, 100);
this->data_dir = DATA_DIR_UPLOAD;
this->read_base_addr = src_mem_address;
this->read_bytes = 0;
@ -135,21 +138,17 @@ DiagMessage Flasher::on_transfer_data(uint8_t* args, uint16_t arg_len) {
this->written_data += arg_len-1;
return this->make_diag_pos_msg(SID_TRANSFER_DATA, nullptr, 0);
} else {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "FLASHER", "esp_ota_write failed!");
esp_ota_abort(this->update_handle);
return this->make_diag_neg_msg(SID_TRANSFER_DATA, NRC_GENERAL_REJECT);
return this->make_diag_neg_msg(SID_TRANSFER_DATA, NRC_UN52_OTA_WRITE_FAIL);
}
} else {
ESP_LOG_LEVEL(ESP_LOG_INFO, "FLASHER", "OTA invalid type!");
return this->make_diag_neg_msg(SID_TRANSFER_DATA, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT);
return this->make_diag_neg_msg(SID_TRANSFER_DATA, NRC_UN52_OTA_INVALID_TY);
}
} else if (args[0] == this->block_counter) {
// Repeated request, KWP spec says to do nothing and just return OK!
ESP_LOG_LEVEL(ESP_LOG_INFO, "FLASHER", "Skip write!");
return this->make_diag_pos_msg(SID_TRANSFER_DATA, nullptr, 0);
} else {
// Huh
ESP_LOG_LEVEL(ESP_LOG_INFO, "FLASHER", "Wtf");
return this->make_diag_neg_msg(SID_TRANSFER_DATA, NRC_GENERAL_REJECT);
}
} else if (this->data_dir == DATA_DIR_UPLOAD) {
@ -168,7 +167,7 @@ DiagMessage Flasher::on_transfer_data(uint8_t* args, uint16_t arg_len) {
this->read_bytes += max_bytes;
return msg;
} else {
return this->make_diag_neg_msg(SID_TRANSFER_DATA, NRC_GENERAL_REJECT);
return this->make_diag_neg_msg(SID_TRANSFER_DATA, NRC_UN52_OTA_INVALID_TY);
}
} else { // Transfer mode not set!
return this->make_diag_neg_msg(SID_TRANSFER_DATA, NRC_CONDITIONS_NOT_CORRECT_REQ_SEQ_ERROR);
@ -186,7 +185,7 @@ DiagMessage Flasher::on_transfer_exit(uint8_t* args, uint16_t arg_len) {
DiagMessage Flasher::on_request_verification(uint8_t* args, uint16_t arg_len) {
if (this->update_type == UPDATE_TYPE_OTA) {
if (this->update_handle == 0) {
return this->make_diag_neg_msg(SID_START_ROUTINE_BY_LOCAL_IDENT, NRC_CONDITIONS_NOT_CORRECT_REQ_SEQ_ERROR); // Can't start verirication without it!
return this->make_diag_neg_msg(SID_START_ROUTINE_BY_LOCAL_IDENT, NRC_CONDITIONS_NOT_CORRECT_REQ_SEQ_ERROR); // Can't start verification without it!
} else {
/// Now verify and switch boot partitions!
uint8_t res[2] = {0xE1, 0x00};

10
src/diag/flasher.h
View File

@ -25,16 +25,16 @@ static_assert(CHUNK_SIZE+2 <= DIAG_CAN_MAX_SIZE); // SID, CHUNK ID
class Flasher {
public:
Flasher(AbstractCan *can_ref, Gearbox* gearbox);
Flasher(EgsBaseCan *can_ref, Gearbox* gearbox);
~Flasher();
DiagMessage on_request_download(uint8_t* args, uint16_t arg_len);
DiagMessage on_request_upload(uint8_t* args, uint16_t arg_len);
DiagMessage on_request_download(const uint8_t* args, uint16_t arg_len);
DiagMessage on_request_upload(const uint8_t* args, uint16_t arg_len);
DiagMessage on_transfer_data(uint8_t* args, uint16_t arg_len);
DiagMessage on_transfer_exit(uint8_t* args, uint16_t arg_len);
DiagMessage on_request_verification(uint8_t* args, uint16_t arg_len);
private:
Gearbox* gearbox_ref;
AbstractCan* can_ref;
EgsBaseCan* can_ref;
uint8_t block_counter = 0;
uint8_t update_type = 0;
uint32_t data_read = 0;
@ -48,7 +48,7 @@ class Flasher {
size_t read_bytes;
size_t read_bytes_total;
DiagMessage make_diag_neg_msg(uint8_t sid, uint8_t nrc){
DiagMessage make_diag_neg_msg(uint8_t sid, kwp_result_t nrc){
DiagMessage msg;
global_make_diag_neg_msg(&msg, sid, nrc);
return msg;

537
src/diag/kwp2000.cpp
View File

@ -15,6 +15,11 @@ typedef struct {
uint8_t week;
} ECU_Date;
uint8_t decToBcd(uint8_t val)
{
return ( (val/10*16) + (val%10) );
}
uint8_t bcd_to_hex(char c) {
switch (c) {
case '0':
@ -52,65 +57,83 @@ uint8_t bcd_to_hex(char c) {
}
}
ECU_Date pcb_ver_to_date(TCM_EFUSE_CONFIG* cfg) {
switch (cfg->board_ver) {
case 1:
return ECU_Date {
.day = 12,
.month = 12,
.year = 21,
.week = 49
};
case 2:
return ECU_Date {
.day = 07,
.month = 07,
.year = 22,
.week = 27
};
case 3:
return ECU_Date {
.day = 12,
.month = 12,
.year = 22,
.week = 49
};
default:
return ECU_Date {
.day = 0,
.month = 0,
.year = 0,
.week = 0
};
}
}
ECU_Date fw_date_to_bcd(char* date) {
uint8_t month = 0x01;
uint8_t month_base_10 = 0;
if (strncmp("Jan", date, 3) == 0) {
month = 0x01;
month_base_10 = 1;
} else if (strncmp("Feb", date, 3) == 0) {
month = 0x02;
month_base_10 = 2;
} else if (strncmp("Mar", date, 3) == 0) {
month = 0x03;
month_base_10 = 3;
} else if (strncmp("Apr", date, 3) == 0) {
month = 0x04;
month_base_10 = 4;
} else if (strncmp("May", date, 3) == 0) {
month = 0x05;
month_base_10 = 5;
} else if (strncmp("Jun", date, 3) == 0) {
month = 0x06;
month_base_10 = 6;
} else if (strncmp("Jul", date, 3) == 0) {
month = 0x07;
month_base_10 = 7;
} else if (strncmp("Aug", date, 3) == 0) {
month = 0x08;
month_base_10 = 8;
} else if (strncmp("Sep", date, 3) == 0) {
month = 0x09;
month_base_10 = 9;
} else if (strncmp("Oct", date, 3) == 0) {
month = 0x10;
month_base_10 = 10;
} else if (strncmp("Nov", date, 3) == 0) {
month = 0x11;
month_base_10 = 11;
} else if (strncmp("Dec", date, 3) == 0) {
month = 0x12;
month_base_10 = 12;
char* month_str = &date[3];
if (strncmp("Jan", month_str, 3) == 0) {
month = 1;
} else if (strncmp("Feb", month_str, 3) == 0) {
month = 2;
} else if (strncmp("Mar", month_str, 3) == 0) {
month = 3;
} else if (strncmp("Apr", month_str, 3) == 0) {
month = 4;
} else if (strncmp("May", month_str, 3) == 0) {
month = 5;
} else if (strncmp("Jun", month_str, 3) == 0) {
month = 6;
} else if (strncmp("Jul", month_str, 3) == 0) {
month = 7;
} else if (strncmp("Aug", month_str, 3) == 0) {
month = 8;
} else if (strncmp("Sep", month_str, 3) == 0) {
month = 9;
} else if (strncmp("Oct", month_str, 3) == 0) {
month = 10;
} else if (strncmp("Nov", month_str, 3) == 0) {
month = 11;
} else if (strncmp("Dec", month_str, 3) == 0) {
month = 12;
} else {
month_base_10 = 99;
month = 0x99; //??
month = 0x00;
}
uint8_t day_base_10 =((date[4] - '0') * 10) + (date[5]-'0'); // Hacky way
uint8_t day = ((bcd_to_hex(date[4]) & 0x0f) << 4) | bcd_to_hex(date[5]);
uint8_t year = ((bcd_to_hex(date[9]) & 0x0f) << 4) | bcd_to_hex(date[10]);
uint8_t year_base_10 =((date[9] - '0') * 10) + (date[10]-'0'); // Hacky way
uint8_t day =((date[0] - '0') * 10) + (date[1]-'0');
uint8_t year =((date[9] - '0') * 10) + (date[10]-'0');
struct tm time;
memset(&time, 0, sizeof(time));
char timebuf[4];
time.tm_mday = day_base_10;
time.tm_year = 100 + year_base_10;
time.tm_mon = month_base_10-1;
time.tm_mday = day;
time.tm_year = 100 + year;
time.tm_mon = month-1;
mktime(&time);
strftime(timebuf, 4, "%W", &time);
uint8_t week = ((bcd_to_hex(timebuf[0]) & 0x0f) << 4) | bcd_to_hex(timebuf[1]);
strftime(timebuf, 4, "%02W", &time);
uint8_t week =((timebuf[0] - '0') * 10) + (timebuf[1]-'0'); // Hacky way
return ECU_Date {
.day = day,
.month = month,
@ -119,20 +142,62 @@ ECU_Date fw_date_to_bcd(char* date) {
};
}
Kwp2000_server::Kwp2000_server(AbstractCan* can_layer, Gearbox* gearbox) {
Kwp2000_server::Kwp2000_server(EgsBaseCan* can_layer, Gearbox* gearbox) {
// Init SPIRAM (We will need this!)
this->next_tp_time = 0;
this->session_mode = SESSION_DEFAULT;
this->usb_diag_endpoint = new UsbEndpoint(true);
this->usb_diag_endpoint = new UsbEndpoint();
this->reboot_pending = false;
this->can_layer = can_layer;
this->gearbox_ptr = gearbox;
this->can_endpoint = new CanEndpoint(can_layer);
// Start ISO-TP endpoint
xTaskCreatePinnedToCore(can_endpoint->start_iso_tp, "ISO_TP_DIAG", 8192, this->can_endpoint, 5, nullptr, 0);
if (this->can_endpoint->init_state() == ESP_OK) {
// Start ISO-TP endpoint
xTaskCreatePinnedToCore(can_endpoint->start_iso_tp, "ISO_TP_DIAG", 8192, this->can_endpoint, 5, nullptr, 0);
}
this->supplier_id = 0x08;
if (can_layer == nullptr || gearbox == nullptr) {
this->diag_var_code = 0x0000;
} else {
switch (VEHICLE_CONFIG.egs_can_type) {
case 1:
this->diag_var_code = 0x0251;
break;
case 2:
this->diag_var_code = 0x0252;
break;
case 3:
this->diag_var_code = 0x0353;
break;
default:
this->diag_var_code = 0x0000;
break;
}
}
init_perfmon();
}
kwp_result_t Kwp2000_server::convert_err_result(kwp_result_t in) {
kwp_result_t out = NRC_GENERAL_REJECT;
switch(in) {
case NRC_UN52_ENGINE_OFF:
case NRC_UN52_ENGINE_ON:
case NRC_UN52_SHIFTER_ACTIVE:
case NRC_UN52_SHIFTER_PASSIVE:
out = NRC_CONDITIONS_NOT_CORRECT_REQ_SEQ_ERROR;
break;
case NRC_UN52_NO_MEM:
out = NRC_GENERAL_REJECT;
break;
default:
out = in;
break;
}
return out;
}
Kwp2000_server::~Kwp2000_server() {
if (this->flash_handler != nullptr) {
delete this->flash_handler;
@ -142,44 +207,21 @@ Kwp2000_server::~Kwp2000_server() {
}
void Kwp2000_server::make_diag_neg_msg(uint8_t sid, uint8_t nrc) {
/*
this->tx_msg.id = KWP_ECU_TX_ID;
this->tx_msg.data_size = 3;
this->tx_msg.data[0] = 0x7F;
this->tx_msg.data[1] = sid;
this->tx_msg.data[2] = nrc;
*/
global_make_diag_neg_msg(&this->tx_msg, sid, nrc);
kwp_result_t nrc_convert = nrc;
if (this->session_mode != SESSION_CUSTOM_UN52) {
nrc_convert = this->convert_err_result(nrc);
ESP_LOGI("KWP2000", "Converting %s NRC to %s", kwp_nrc_to_name(nrc), kwp_nrc_to_name(nrc_convert));
}
global_make_diag_neg_msg(&this->tx_msg, sid, nrc_convert);
this->send_resp = true;
}
void Kwp2000_server::make_diag_pos_msg(uint8_t sid, const uint8_t* resp, uint16_t len) {
/*
if (len + 2 > DIAG_CAN_MAX_SIZE) {
make_diag_neg_msg(sid, NRC_GENERAL_REJECT);
return;
}
this->tx_msg.id = KWP_ECU_TX_ID;
this->tx_msg.data_size = len+1;
this->tx_msg.data[0] = sid+0x40;
memcpy(&this->tx_msg.data[1], resp, len);
*/
global_make_diag_pos_msg(&this->tx_msg, sid, resp, len);
this->send_resp = true;
}
void Kwp2000_server::make_diag_pos_msg(uint8_t sid, uint8_t pid, const uint8_t* resp, uint16_t len) {
/*
if (len + 3 > DIAG_CAN_MAX_SIZE) {
make_diag_neg_msg(sid, NRC_GENERAL_REJECT);
return;
}
this->tx_msg.id = KWP_ECU_TX_ID;
this->tx_msg.data_size = len+2;
this->tx_msg.data[0] = sid+0x40;
this->tx_msg.data[1] = pid;
memcpy(&this->tx_msg.data[2], resp, len);
*/
global_make_diag_pos_msg(&this->tx_msg, sid, pid, resp, len);
this->send_resp = true;
}
@ -191,21 +233,19 @@ int Kwp2000_server::allocate_routine_args(uint8_t* src, uint8_t arg_len) {
void Kwp2000_server::server_loop() {
this->send_resp = false;
uint64_t timestamp;
while(1) {
timestamp = esp_timer_get_time()/1000;
uint64_t timestamp = esp_timer_get_time()/1000;
bool read_msg = false;
bool endpoint_was_usb = false;
if (this->usb_diag_endpoint->read_data(&this->rx_msg)) {
if (this->usb_diag_endpoint->init_state() == ESP_OK && this->usb_diag_endpoint->read_data(&this->rx_msg)) {
endpoint_was_usb = true;
read_msg = true;
} else if (this->can_endpoint->read_data(&this->rx_msg)) {
} else if (this->can_endpoint->init_state() == ESP_OK && this->can_endpoint->read_data(&this->rx_msg)) {
endpoint_was_usb = false;
read_msg = true;
}
if (read_msg) {
this->next_tp_time = timestamp+KWP_TP_TIMEOUT_MS;
//ESP_LOG_BUFFER_HEX_LEVEL("KWP_READ_MSG", this->rx_msg.data, this->rx_msg.data_size, esp_log_level_t::ESP_LOG_INFO);
if (this->rx_msg.data_size == 0) {
continue; // Huh?
}
@ -239,7 +279,7 @@ void Kwp2000_server::server_loop() {
this->process_start_routine_by_local_ident(args_ptr, args_size);
break;
case SID_REQUEST_ROUTINE_RESULTS_BY_LOCAL_IDENT:
this->process_request_routine_resutls_by_local_ident(args_ptr, args_size);
this->process_request_routine_results_by_local_ident(args_ptr, args_size);
break;
case SID_REQ_UPLOAD:
this->process_request_upload(args_ptr, args_size);
@ -247,6 +287,9 @@ void Kwp2000_server::server_loop() {
case SID_REQ_DOWNLOAD:
this->process_request_download(args_ptr, args_size);
break;
case SID_IOCTL_BY_LOCAL_IDENT:
this->process_ioctl_by_local_ident(args_ptr, args_size);
break;
case SID_TRANSFER_DATA:
this->process_transfer_data(args_ptr, args_size);
break;
@ -256,8 +299,15 @@ void Kwp2000_server::server_loop() {
case SID_SHIFT_MGR_OP:
this->process_shift_mgr_op(args_ptr, args_size);
break;
case SID_ENABLE_NORMAL_MSG_TRANSMISSION:
this->process_enable_msg_tx(args_ptr, args_size);
break;
case SID_DISABLE_NORMAL_MSG_TRANSMISSION:
this->process_disable_msg_tx(args_ptr, args_size);
break;
default:
ESP_LOG_LEVEL(ESP_LOG_WARN, "KWP_HANDLE_REQ", "Requested SID %02X is not supported", rx_msg.data[0]);
ESP_LOG_LEVEL(ESP_LOG_WARN, "KWP_HANDLE_REQ", "Requested SID %02X is not supported, full msg was:", rx_msg.data[0]);
ESP_LOG_BUFFER_HEX_LEVEL("KWP_HANDLE_REQ", rx_msg.data, rx_msg.data_size, ESP_LOG_WARN);
make_diag_neg_msg(rx_msg.data[0], NRC_SERVICE_NOT_SUPPORTED);
break;
}
@ -265,7 +315,7 @@ void Kwp2000_server::server_loop() {
if (this->send_resp) {
if (endpoint_was_usb) {
this->usb_diag_endpoint->send_data(&tx_msg);
} else {
} else if (this->can_endpoint != nullptr) {
this->can_endpoint->send_data(&tx_msg);
}
this->send_resp = false;
@ -276,7 +326,6 @@ void Kwp2000_server::server_loop() {
this->session_mode == SESSION_CUSTOM_UN52)
&& timestamp > this->next_tp_time
) {
ESP_LOG_LEVEL(ESP_LOG_INFO, "KWP2000", "Tester present interval has expired, returning to default mode");
this->session_mode = SESSION_DEFAULT;
}
if (this->reboot_pending) {
@ -300,7 +349,7 @@ void Kwp2000_server::server_loop() {
}
void Kwp2000_server::process_start_diag_session(uint8_t* args, uint16_t arg_len) {
void Kwp2000_server::process_start_diag_session(const uint8_t* args, uint16_t arg_len) {
if (arg_len != 1) { // Must only have 1 arg
make_diag_neg_msg(SID_START_DIAGNOSTIC_SESSION, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT);
return;
@ -324,7 +373,7 @@ void Kwp2000_server::process_start_diag_session(uint8_t* args, uint16_t arg_len)
make_diag_pos_msg(SID_START_DIAGNOSTIC_SESSION, &args[0], 1);
}
void Kwp2000_server::process_ecu_reset(uint8_t* args, uint16_t arg_len) {
void Kwp2000_server::process_ecu_reset(const uint8_t* args, uint16_t arg_len) {
if (
this->session_mode == SESSION_EXTENDED ||
this->session_mode == SESSION_REPROGRAMMING ||
@ -336,7 +385,7 @@ void Kwp2000_server::process_ecu_reset(uint8_t* args, uint16_t arg_len) {
} else {
// 1 arg, process the reset type
if (args[0] == 0x01 || args[1] == 0x82) {
if (!is_shifter_passive(this->can_layer)) {
if (this->can_layer != nullptr && !is_shifter_passive(this->can_layer)) {
// P or R, we CANNOT reset the ECU!
make_diag_neg_msg(SID_ECU_RESET, NRC_CONDITIONS_NOT_CORRECT_REQ_SEQ_ERROR);
return;
@ -360,7 +409,7 @@ void Kwp2000_server::process_read_status_of_dtcs(uint8_t* args, uint16_t arg_len
}
void Kwp2000_server::process_read_ecu_ident(uint8_t* args, uint16_t arg_len) {
void Kwp2000_server::process_read_ecu_ident(const uint8_t* args, uint16_t arg_len) {
// Any diagnostic session
if (arg_len != 1) {
make_diag_neg_msg(SID_READ_ECU_IDENT, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT);
@ -371,37 +420,39 @@ void Kwp2000_server::process_read_ecu_ident(uint8_t* args, uint16_t arg_len) {
esp_ota_get_partition_description(running, &running_info);
if (args[0] == 0x86) {
ECU_Date date = fw_date_to_bcd(running_info.date);
ECU_Date v_date = pcb_ver_to_date(&BOARD_CONFIG);
uint8_t daimler_ident_data[16];
memset(daimler_ident_data, 0x00, 16);
// Part number
daimler_ident_data[0] = 0x01;
daimler_ident_data[0] = 0x12;
daimler_ident_data[1] = 0x23;
daimler_ident_data[2] = 0x45;
daimler_ident_data[3] = 0x67;
daimler_ident_data[4] = 0x89;
// ECU Hardware date
daimler_ident_data[5] = date.week;
daimler_ident_data[6] = date.year;
daimler_ident_data[5] = decToBcd(v_date.week); //date.week;
daimler_ident_data[6] = decToBcd(v_date.year); //date.year;
// ECU Software date
daimler_ident_data[7] = date.week;
daimler_ident_data[8] = date.year;
daimler_ident_data[9] = SUPPLIER_ID;
daimler_ident_data[10] = DIAG_VARIANT_CODE >> 8;
daimler_ident_data[11] = DIAG_VARIANT_CODE & 0xFF;
daimler_ident_data[13] = date.year;
daimler_ident_data[14] = date.month;
daimler_ident_data[15] = date.day;
daimler_ident_data[7] = decToBcd(date.week);
daimler_ident_data[8] = decToBcd(date.year);
daimler_ident_data[9] = this->supplier_id;
daimler_ident_data[10] = this->diag_var_code >> 8;
daimler_ident_data[11] = this->diag_var_code & 0xFF;
daimler_ident_data[13] = decToBcd(BOARD_CONFIG.manufacture_year);
daimler_ident_data[14] = decToBcd(BOARD_CONFIG.manufacture_month);
daimler_ident_data[15] = decToBcd(BOARD_CONFIG.manufacture_day);
make_diag_pos_msg(SID_READ_ECU_IDENT, 0x86, daimler_ident_data, 16);
} else if (args[0] == 0x87) { // Daimler and Mitsubishi compatible identification
ECU_Date date = fw_date_to_bcd(running_info.date);
uint8_t ident_data[19];
memset(ident_data, 0x00, 19);
ident_data[0] = 0x00; // TODO ECU origin
ident_data[1] = SUPPLIER_ID;
ident_data[2] = DIAG_VARIANT_CODE >> 8;
ident_data[3] = DIAG_VARIANT_CODE & 0xFF;
ident_data[5] = 0x00;// HW version
ident_data[6] = 0x00;// HW version
ident_data[1] = this->supplier_id;
ident_data[2] = this->diag_var_code >> 8;
ident_data[3] = this->diag_var_code & 0xFF;
ident_data[5] = 0x01;// HW version
ident_data[6] = BOARD_CONFIG.board_ver;// HW version
ident_data[7] = date.day;// SW version
ident_data[8] = date.month;// SW version
ident_data[9] = date.year;// SW version
@ -417,14 +468,21 @@ void Kwp2000_server::process_read_ecu_ident(uint8_t* args, uint16_t arg_len) {
//ident_data[19] = '9'; // Part number to end
make_diag_pos_msg(SID_READ_ECU_IDENT, 0x87, ident_data, 19);
} else if (args[0] == 0x88) { // VIN original
make_diag_pos_msg(SID_READ_ECU_IDENT, 0x88, (const uint8_t*)"ULTIMATENAG52ESP0", 17);
make_diag_pos_msg(SID_READ_ECU_IDENT, 0x88, reinterpret_cast<const uint8_t*>("ULTIMATENAG52ESP0"), 17);
} else if (args[0] == 0x89) { // Diagnostic variant code
int d = DIAG_VARIANT_CODE;
int d = this->diag_var_code;
uint8_t b[4];
memcpy(b, &d, 4);
make_diag_pos_msg(SID_READ_ECU_IDENT, 0x89, b, 4);
} else if (args[0] == 0x8A) {
char x[4];
x[0] = 'H';
x[1] = 'E';
x[2] = 'L';
x[3] = 'P';
return make_diag_pos_msg(SID_READ_ECU_IDENT, 0x8A, (uint8_t*)&x, 4);
} else if (args[0] == 0x90) { // VIN current
make_diag_pos_msg(SID_READ_ECU_IDENT, 0x90, (const uint8_t*)"ULTIMATENAG52ESP0", 17);
make_diag_pos_msg(SID_READ_ECU_IDENT, 0x90, reinterpret_cast<const uint8_t*>("ULTIMATENAG52ESP0"), 17);
} else {
make_diag_neg_msg(SID_READ_ECU_IDENT, NRC_REQUEST_OUT_OF_RANGE);
}
@ -447,29 +505,60 @@ void Kwp2000_server::process_read_data_local_ident(uint8_t* args, uint16_t arg_l
esp_efuse_mac_get_default(mac);
char resp[13];
sprintf(resp, "%02X%02X%02X%02X%02X%02X", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, 0xE1, (const uint8_t*)resp, 12);
make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, 0xE1, reinterpret_cast<const uint8_t*>(resp), 12);
} else if (args[0] == RLI_MAP_EDITOR) {
// We need 2 args for this.
if (arg_len != 2) {
// 0 - RLI
// 1 - Map ID
// 2 - CMD
// 3-4 - Arg len
// 5..n - Data
if (arg_len < 5) {
make_diag_neg_msg(SID_READ_DATA_LOCAL_IDENT, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT);
return;
}
int16_t* read_buf = nullptr;
uint16_t dest_size = 0;
uint8_t ret = MapEditor::read_map_data(args[1], &dest_size, &read_buf);
uint8_t map_id = args[1];
uint8_t cmd = args[2];
uint16_t map_len_bytes = args[3] << 8 | args[4];
if (arg_len-5 != map_len_bytes) {
make_diag_neg_msg(SID_READ_DATA_LOCAL_IDENT, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT);
return;
}
uint8_t ret;
uint8_t* buffer = nullptr;
uint16_t read_bytes_size = 0;
if ( cmd == MAP_CMD_READ || cmd == MAP_CMD_READ_DEFAULT || cmd == MAP_CMD_READ_EEPROM) {
uint8_t c;
if (cmd == MAP_CMD_READ) {
c = MAP_READ_TYPE_MEM;
} else if (cmd == MAP_CMD_READ_DEFAULT) {
c = MAP_READ_TYPE_PRG;
} else { // MAP_CMD_READ_EEPROM
c = MAP_READ_TYPE_STO;
}
ret = MapEditor::read_map_data(map_id, c, &read_bytes_size, &buffer);
} else if (cmd == MAP_CMD_READ_META) {
ret = MapEditor::read_map_metadata(map_id, &read_bytes_size, &buffer);
} else {
ret = NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT;
}
if (ret == 0) { // OK
uint8_t* buf = new uint8_t[2+dest_size];
buf[0] = dest_size >> 8;
buf[1] = dest_size & 0xFF;
memcpy(&buf[2], read_buf, dest_size);
make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, buf, 2+dest_size);
delete read_buf; // DELETE MapEditor allocation
uint8_t* buf = static_cast<uint8_t*>(heap_caps_malloc(2+read_bytes_size, MALLOC_CAP_SPIRAM));
if (buf == nullptr) {
free(buffer); // DELETE MapEditor allocation
make_diag_neg_msg(SID_READ_DATA_LOCAL_IDENT, NRC_GENERAL_REJECT);
return;
}
buf[0] = read_bytes_size & 0xFF;
buf[1] = read_bytes_size >> 8;
memcpy(&buf[2], buffer, read_bytes_size);
make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, buf, 2+read_bytes_size);
delete[] buf;
free(buffer); // DELETE MapEditor allocation
return;
} else {
make_diag_neg_msg(SID_READ_DATA_LOCAL_IDENT, ret);
return;
}
} else if (args[0] == RLI_GEARBOX_SENSORS) {
DATA_GEARBOX_SENSORS r = get_gearbox_sensors(this->gearbox_ptr);
make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, RLI_GEARBOX_SENSORS, (uint8_t*)&r, sizeof(DATA_GEARBOX_SENSORS));
@ -494,27 +583,47 @@ void Kwp2000_server::process_read_data_local_ident(uint8_t* args, uint16_t arg_l
} else if (args[0] == RLI_TCM_CONFIG) {
TCM_CORE_CONFIG r = get_tcm_config();
make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, RLI_TCM_CONFIG, (uint8_t*)&r, sizeof(TCM_CORE_CONFIG));
} else if (args[0] == RLI_EFUSE_CONFIG) {
TCM_EFUSE_CONFIG ecfg;
EEPROM::read_efuse_config(&ecfg);
make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, RLI_EFUSE_CONFIG, (uint8_t*)&ecfg, sizeof(TCM_EFUSE_CONFIG));
} else if (args[0] == RLI_SHIFT_LIVE) {
SHIFT_LIVE_INFO r = get_shift_live_Data(egs_can_hal, gearbox);
make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, RLI_SHIFT_LIVE, (uint8_t*)&r, sizeof(SHIFT_LIVE_INFO));
} else if (args[0] == RLI_FW_HEADER) {
make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, RLI_FW_HEADER, (uint8_t*)get_image_header(), sizeof(esp_app_desc_t));
make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, RLI_FW_HEADER, reinterpret_cast<const uint8_t*>(get_image_header()), sizeof(esp_app_desc_t));
}
else {
// EGS52 emulation
#ifdef EGS52_MODE
if (args[0] == RLI_31) {
RLI_31_DATA r = get_rli_31(egs_can_hal);
return make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, RLI_31, (uint8_t*)&r, sizeof(RLI_31_DATA));
if (VEHICLE_CONFIG.egs_can_type == 2) {
if (args[0] == 0x31) {
RLI_31_DATA r = get_rli_31(egs_can_hal);
return make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, 0x31, (uint8_t*)&r, sizeof(RLI_31_DATA));
} else if (args[0] == 0x33) {
RLI_33_DATA r = get_rli_33(egs_can_hal);
return make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, 0x33, (uint8_t*)&r, sizeof(RLI_33_DATA));
} else if (args[0] == 0x32) {
RLI_32_DATA r = get_rli_32(egs_can_hal);
return make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, 0x32, (uint8_t*)&r, sizeof(RLI_32_DATA));
} else if (args[0] == 0x30) {
RLI_30_DATA r = get_rli_30(egs_can_hal);
return make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, 0x30, (uint8_t*)&r, sizeof(RLI_30_DATA));
} else if (args[0] == 0xD1) {
char x[48];
memset(&x, 0, 48);
memcpy(&x,&BOARD_CONFIG, sizeof(BOARD_CONFIG));
return make_diag_pos_msg(SID_READ_DATA_LOCAL_IDENT, 0xD1, (uint8_t*)&x, 48);
}
}
#endif
make_diag_neg_msg(SID_READ_DATA_LOCAL_IDENT, NRC_REQUEST_OUT_OF_RANGE);
}
}
void Kwp2000_server::process_read_data_ident(uint8_t* args, uint16_t arg_len) {
}
void Kwp2000_server::process_read_mem_address(uint8_t* args, uint16_t arg_len) {
if (this->session_mode != SESSION_EXTENDED && this->session_mode != SESSION_CUSTOM_UN52) {
make_diag_neg_msg(SID_READ_MEM_BY_ADDRESS, NRC_SERVICE_NOT_SUPPORTED_IN_ACTIVE_DIAG_SESSION);
@ -526,25 +635,69 @@ void Kwp2000_server::process_read_mem_address(uint8_t* args, uint16_t arg_len) {
}
uint8_t* address;
if (arg_len == 4) {
address = (uint8_t*)(0x40070000+((args[2] << 16) | (args[1] << 8) | args[0])); // Raw address to read from
address = reinterpret_cast<uint8_t*>(0x40070000+((args[2] << 16) | (args[1] << 8) | args[0])); // Raw address to read from
} else {
address = (uint8_t*)(0x40070000+((args[3] << 24) | (args[2] << 16) | (args[1] << 8) | args[0])); // Raw address to read from 4 byte
address = reinterpret_cast<uint8_t*>(0x40070000+((args[3] << 24) | (args[2] << 16) | (args[1] << 8) | args[0])); // Raw address to read from 4 byte
}
if (address + args[arg_len-1] >= (uint8_t*)0x400BFFFF) { // Address too big (Not in SRAM 0 or SRAM1)!
if (address + args[arg_len-1] >= reinterpret_cast<uint8_t*>(0x400BFFFF)) { // Address too big (Not in SRAM 0 or SRAM1)!
make_diag_neg_msg(SID_READ_MEM_BY_ADDRESS, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT);
return;
}
ESP_LOG_LEVEL(ESP_LOG_INFO, "RMA","Pointer: %p", address);
make_diag_pos_msg(SID_READ_MEM_BY_ADDRESS, address, args[arg_len-1]); // Copy args[3] len bytes from address into positive message
}
void Kwp2000_server::process_security_access(uint8_t* args, uint16_t arg_len) {
}
void Kwp2000_server::process_disable_msg_tx(uint8_t* args, uint16_t arg_len) {
if (this->session_mode != SESSION_EXTENDED && this->session_mode != SESSION_CUSTOM_UN52) {
make_diag_neg_msg(SID_DISABLE_NORMAL_MSG_TRANSMISSION, NRC_SERVICE_NOT_SUPPORTED_IN_ACTIVE_DIAG_SESSION);
return;
}
if (arg_len != 1) {
make_diag_neg_msg(SID_DISABLE_NORMAL_MSG_TRANSMISSION, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT);
return;
}
if (!egs_can_hal) {
make_diag_neg_msg(SID_DISABLE_NORMAL_MSG_TRANSMISSION, NRC_GENERAL_REJECT);
return;
}
bool response = true;
if (args[0] == 0x01) { response = true; }
else if (args[0] == 0x02) { response = false; }
else {
make_diag_neg_msg(SID_DISABLE_NORMAL_MSG_TRANSMISSION, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT);
return;
}
egs_can_hal->disable_normal_msg_transmission();
if (response) {
make_diag_pos_msg(SID_DISABLE_NORMAL_MSG_TRANSMISSION, nullptr, 0);
}
}
void Kwp2000_server::process_enable_msg_tx(uint8_t* args, uint16_t arg_len) {
void Kwp2000_server::process_enable_msg_tx(uint8_t* args, uint16_t arg_len) {
if (this->session_mode != SESSION_EXTENDED && this->session_mode != SESSION_CUSTOM_UN52) {
make_diag_neg_msg(SID_ENABLE_NORMAL_MSG_TRANSMISSION, NRC_SERVICE_NOT_SUPPORTED_IN_ACTIVE_DIAG_SESSION);
return;
}
if (arg_len != 1) {
make_diag_neg_msg(SID_ENABLE_NORMAL_MSG_TRANSMISSION, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT);
return;
}
if (!egs_can_hal) {
make_diag_neg_msg(SID_ENABLE_NORMAL_MSG_TRANSMISSION, NRC_GENERAL_REJECT);
return;
}
bool response = true;
if (args[0] == 0x01) { response = true; }
else if (args[0] == 0x02) { response = false; }
else {
make_diag_neg_msg(SID_ENABLE_NORMAL_MSG_TRANSMISSION, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT);
return;
}
egs_can_hal->enable_normal_msg_transmission();
if (response) {
make_diag_pos_msg(SID_ENABLE_NORMAL_MSG_TRANSMISSION, nullptr, 0);
}
}
void Kwp2000_server::process_dynamically_define_local_ident(uint8_t* args, uint16_t arg_len) {
@ -553,6 +706,26 @@ void Kwp2000_server::process_write_data_by_ident(uint8_t* args, uint16_t arg_len
}
void Kwp2000_server::process_ioctl_by_local_ident(uint8_t* args, uint16_t arg_len) {
if (this->session_mode != SESSION_EXTENDED && this->session_mode != SESSION_CUSTOM_UN52 && this->session_mode != SESSION_REPROGRAMMING) {
make_diag_neg_msg(SID_IOCTL_BY_LOCAL_IDENT, NRC_SERVICE_NOT_SUPPORTED_IN_ACTIVE_DIAG_SESSION);
return;
}
if (arg_len == 2) {
// 0x10 (EGS mode), 0x01 (Report current state)
if (args[0] == 0x10 && args[1] == 0x01) { // Query EGS mode (DAS EGS51 and EGS52)
// We need to return this for DAS to be happy EGS is in 'production' mode
// resp[0] - 0x10 (EGS mode)
// resp[1..2] - 0x0001 - Normal, 0x0002 - Assembly mode, 0x0004 - Role mode, 0x0008 - Slave mode
uint8_t resp[3] = {0x10, 0x00, 0x02};
if (BOARD_CONFIG.board_ver == 0) {
resp[2] = 0x02; // Assembly mode if mode is unknown
}
make_diag_pos_msg(SID_IOCTL_BY_LOCAL_IDENT, resp, 3);
return;
}
}
make_diag_neg_msg(SID_IOCTL_BY_LOCAL_IDENT, NRC_CONDITIONS_NOT_CORRECT_REQ_SEQ_ERROR);
return;
}
void Kwp2000_server::process_start_routine_by_local_ident(uint8_t* args, uint16_t arg_len) {
if (this->session_mode != SESSION_EXTENDED && this->session_mode != SESSION_CUSTOM_UN52 && this->session_mode != SESSION_REPROGRAMMING) {
@ -571,7 +744,7 @@ void Kwp2000_server::process_start_routine_by_local_ident(uint8_t* args, uint16_
gearbox_ptr->sensor_data.engine_rpm == 0 && //Engine off
gearbox_ptr->sensor_data.input_rpm == 0 && // Not moving
Solenoids::get_solenoid_voltage() > 10000 && // Enough battery voltage
Sensors::parking_lock_engaged(&pl) &&
Sensors::parking_lock_engaged(&pl) == ESP_OK &&
!pl // Parking lock off (In D/R)
) {
this->routine_running = true;
@ -614,7 +787,7 @@ void Kwp2000_server::process_start_routine_by_local_ident(uint8_t* args, uint16_
void Kwp2000_server::process_stop_routine_by_local_ident(uint8_t* args, uint16_t arg_len) {
}
void Kwp2000_server::process_request_routine_resutls_by_local_ident(uint8_t* args, uint16_t arg_len) {
void Kwp2000_server::process_request_routine_results_by_local_ident(const uint8_t* args, uint16_t arg_len) {
if (this->session_mode != SESSION_EXTENDED && this->session_mode != SESSION_CUSTOM_UN52 && this->session_mode != SESSION_REPROGRAMMING) {
make_diag_neg_msg(SID_REQUEST_ROUTINE_RESULTS_BY_LOCAL_IDENT, NRC_SERVICE_NOT_SUPPORTED_IN_ACTIVE_DIAG_SESSION);
return;
@ -707,38 +880,39 @@ void Kwp2000_server::process_write_data_by_local_ident(uint8_t* args, uint16_t a
if (args[0] == RLI_MAP_EDITOR) {
// 0 - RLI
// 1 - Map ID
// 2-3 - Map data size (Bytes)
// 4..n - Map data to write
//
// OR
//
// 0 - RLI
// 1 - 0xFF - RELOAD
// 2- Profile IDx
if (arg_len == 3 && args[1] == 0xFF) {
uint8_t ret = MapEditor::trigger_reload(args[2]);
if (ret != 0x00) {
make_diag_neg_msg(SID_WRITE_DATA_BY_LOCAL_IDENT, ret); // Error
} else {
make_diag_pos_msg(SID_WRITE_DATA_BY_LOCAL_IDENT, RLI_MAP_EDITOR, nullptr, 0); // Ok!
}
return;
}
if (arg_len < 7) {
// 2 - CMD
// 3-4 - Arg len
// 5..n - Data
if (arg_len < 5) {
make_diag_neg_msg(SID_WRITE_DATA_BY_LOCAL_IDENT, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT);
return;
}
uint8_t map_id = args[1];
uint16_t map_len_bytes = args[2] << 8 | args[3];
if (arg_len-4 != map_len_bytes) {
uint8_t cmd = args[2];
uint16_t map_len_bytes = args[4] << 8 | args[3];
if (arg_len-5 != map_len_bytes) {
make_diag_neg_msg(SID_WRITE_DATA_BY_LOCAL_IDENT, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT);
return;
}
uint8_t ret = MapEditor::write_map_data(map_id, map_len_bytes, (int16_t*)&args[4]);
if (ret != 0x00) {
make_diag_neg_msg(SID_WRITE_DATA_BY_LOCAL_IDENT, ret); // Error
uint8_t ret;
switch (cmd) {
case MAP_CMD_WRITE:
ret = MapEditor::write_map_data(map_id, map_len_bytes/2, (int16_t*)&args[5]); // len_bytes /2 = sizeof(int16)
break;
case MAP_CMD_UNDO:
ret = MapEditor::undo_changes(map_id);
break;
case MAP_CMD_BURN:
ret = MapEditor::burn_to_eeprom(map_id);
break;
default:
ret = NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT;
break;
}
if (ret == 0) {
make_diag_pos_msg(SID_WRITE_DATA_BY_LOCAL_IDENT, nullptr, 0);
} else {
make_diag_pos_msg(SID_WRITE_DATA_BY_LOCAL_IDENT, RLI_MAP_EDITOR, nullptr, 0); // Ok!
make_diag_neg_msg(SID_WRITE_DATA_BY_LOCAL_IDENT, ret);
}
} else if (args[0] == RLI_TCM_CONFIG) {
if (arg_len-1 != sizeof(TCM_CORE_CONFIG)) {
@ -748,12 +922,26 @@ void Kwp2000_server::process_write_data_by_local_ident(uint8_t* args, uint16_t a
TCM_CORE_CONFIG cfg;
memcpy(&cfg, &args[1], sizeof(TCM_CORE_CONFIG));
uint8_t res = set_tcm_config(cfg);
if (res == 0x00) {
if (res == NRC_OK) {
make_diag_pos_msg(SID_WRITE_DATA_BY_LOCAL_IDENT, RLI_TCM_CONFIG, nullptr, 0);
} else {
make_diag_neg_msg(SID_WRITE_DATA_BY_LOCAL_IDENT, res);
}
}
} else if (args[0] == RLI_EFUSE_CONFIG) {
if (arg_len-1 != sizeof(TCM_EFUSE_CONFIG)) {
make_diag_neg_msg(SID_WRITE_DATA_BY_LOCAL_IDENT, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT);
} else {
// TCM Core config size ok
TCM_EFUSE_CONFIG cfg;
memcpy(&cfg, &args[1], sizeof(TCM_EFUSE_CONFIG));
bool res = EEPROM::write_efuse_config(&cfg);
if (res == ESP_OK) {
make_diag_pos_msg(SID_WRITE_DATA_BY_LOCAL_IDENT, RLI_TCM_CONFIG, nullptr, 0);
} else {
make_diag_neg_msg(SID_WRITE_DATA_BY_LOCAL_IDENT, NRC_GENERAL_REJECT);
}
}
} else {
make_diag_neg_msg(SID_WRITE_DATA_BY_LOCAL_IDENT, NRC_REQUEST_OUT_OF_RANGE);
}
@ -764,7 +952,7 @@ void Kwp2000_server::process_write_data_by_local_ident(uint8_t* args, uint16_t a
void Kwp2000_server::process_write_mem_by_address(uint8_t* args, uint16_t arg_len) {
}
void Kwp2000_server::process_tester_present(uint8_t* args, uint16_t arg_len) {
void Kwp2000_server::process_tester_present(const uint8_t* args, uint16_t arg_len) {
if (arg_len != 1) { // Must only have 1 arg
make_diag_neg_msg(SID_TESTER_PRESENT, NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT);
return;
@ -843,13 +1031,13 @@ void Kwp2000_server::process_shift_mgr_op(uint8_t* args, uint16_t arg_len) {
void Kwp2000_server::run_solenoid_test() {
vTaskDelay(50);
ESP_LOG_LEVEL(ESP_LOG_INFO, "RT_SOL_TEST", "Starting solenoid test");
this->routine_results_len = 1 + 2 + (6*6); // ATF temp (2 byte), (current off, current on, vbatt) (x6);
memset(this->routine_result, 0, this->routine_results_len);
this->routine_result[0] = this->routine_id;
// Routine results format
int16_t atf = this->gearbox_ptr->sensor_data.atf_temp;
uint16_t batt = Solenoids::get_solenoid_voltage();
// uint16_t batt = Solenoids::get_solenoid_voltage();
uint16_t batt;
this->routine_result[1] = atf & 0xFF;
this->routine_result[2] = (atf >> 8) & 0xFF;
@ -889,7 +1077,7 @@ void Kwp2000_server::run_solenoid_test() {
this->routine_result[13] = curr & 0xFF;
this->routine_result[14] = (curr >> 8) & 0xFF;
#define NUM_CURRENT_SAMPLES 10
const int NUM_CURRENT_SAMPLES = 10;
float total_batt = 0;
float total_curr = 0;
sol_mpc->write_pwm_12_bit(4096); // 1. MPC solenoid
@ -901,8 +1089,8 @@ void Kwp2000_server::run_solenoid_test() {
total_batt += Solenoids::get_solenoid_voltage();
vTaskDelay(10);
}
curr = total_curr / NUM_CURRENT_SAMPLES;
batt = total_batt / NUM_CURRENT_SAMPLES;
curr = (uint16_t)(total_curr / (float)NUM_CURRENT_SAMPLES);
batt = (uint16_t)(total_batt / (float)NUM_CURRENT_SAMPLES);
this->routine_result[15] = batt & 0xFF;
this->routine_result[16] = (batt >> 8) & 0xFF;
this->routine_result[17] = curr & 0xFF;
@ -997,8 +1185,7 @@ void Kwp2000_server::run_solenoid_test() {
this->routine_result[37] = curr & 0xFF;
this->routine_result[38] = (curr >> 8) & 0xFF;
sol_y5->write_pwm_12_bit(0);
ESP_LOG_LEVEL(ESP_LOG_INFO, "RT_SOL_TEST", "Cleaning up");
this->routine_running = false;
mpc_cc->toggle_state(true);
spc_cc->toggle_state(true);

39
src/diag/kwp2000.h
View File

@ -3,7 +3,7 @@
#ifndef _KWP_H__
#define _KWP_H__
#include "endpoint.h"
#include "endpoints/endpoint.h"
#include <stdint.h>
#include "kwp2000_defines.h"
#include "gearbox.h"
@ -13,23 +13,6 @@
#include "esp32/spiram.h"
#include "flasher.h"
// Ident data
#ifdef EGS53_MODE
#define SUPPLIER_ID 0x08 // Simens
#define DIAG_VARIANT_CODE 0x0353 // DiagVersion53_EGS53
#endif
#ifdef EGS52_MODE
#define SUPPLIER_ID 0x08 // Simens
#define DIAG_VARIANT_CODE 0x0251 // DiagVersion51_EGS52
#endif
#ifdef EGS51_MODE
#define SUPPLIER_ID 0x08 // Simens
#define DIAG_VARIANT_CODE 0x000 // DiagVersion51_EGS52
#endif
#define PROCESSOR_TYPE
#define COMM_MATRIX_VERSION 0x0101 // 01.01
#define CAN_DRIVER_VERSION 0x0101 // 01.01
@ -45,18 +28,20 @@
class Kwp2000_server {
public:
Kwp2000_server(AbstractCan* can_layer, Gearbox* gearbox);
Kwp2000_server(EgsBaseCan* can_layer, Gearbox* gearbox);
~Kwp2000_server();
static void start_kwp_server(void *_this) {
static_cast<Kwp2000_server*>(_this)->server_loop();
}
void make_diag_neg_msg(uint8_t sid, uint8_t nrc);
kwp_result_t convert_err_result(kwp_result_t in);
void make_diag_neg_msg(uint8_t sid, kwp_result_t nrc);
void make_diag_pos_msg(uint8_t sid, const uint8_t* resp, uint16_t len);
void make_diag_pos_msg(uint8_t sid, uint8_t pid, const uint8_t* resp, uint16_t len);
Gearbox* gearbox_ptr;
AbstractCan* can_layer;
EgsBaseCan* can_layer;
private:
[[noreturn]]
void server_loop();
@ -76,11 +61,11 @@ class Kwp2000_server {
bool reboot_pending;
int allocate_routine_args(uint8_t* src, uint8_t arg_len);
void process_start_diag_session(uint8_t* args, uint16_t arg_len);
void process_ecu_reset(uint8_t* args, uint16_t arg_len);
void process_start_diag_session(const uint8_t* args, uint16_t arg_len);
void process_ecu_reset(const uint8_t* args, uint16_t arg_len);
void process_clear_diag_info(uint8_t* args, uint16_t arg_len);
void process_read_status_of_dtcs(uint8_t* args, uint16_t arg_len);
void process_read_ecu_ident(uint8_t* args, uint16_t arg_len);
void process_read_ecu_ident(const uint8_t* args, uint16_t arg_len);
void process_read_data_local_ident(uint8_t* args, uint16_t arg_len);
void process_read_data_ident(uint8_t* args, uint16_t arg_len);
void process_read_mem_address(uint8_t* args, uint16_t arg_len);
@ -92,14 +77,14 @@ class Kwp2000_server {
void process_ioctl_by_local_ident(uint8_t* args, uint16_t arg_len);
void process_start_routine_by_local_ident(uint8_t* args, uint16_t arg_len);
void process_stop_routine_by_local_ident(uint8_t* args, uint16_t arg_len);
void process_request_routine_resutls_by_local_ident(uint8_t* args, uint16_t arg_len);
void process_request_routine_results_by_local_ident(const uint8_t* args, uint16_t arg_len);
void process_request_download(uint8_t* args, uint16_t arg_len);
void process_request_upload(uint8_t* args, uint16_t arg_len);
void process_transfer_data(uint8_t* args, uint16_t arg_len);
void process_transfer_exit(uint8_t* args, uint16_t arg_len);
void process_write_data_by_local_ident(uint8_t* args, uint16_t arg_len);
void process_write_mem_by_address(uint8_t* args, uint16_t arg_len);
void process_tester_present(uint8_t* args, uint16_t arg_len);
void process_tester_present(const uint8_t* args, uint16_t arg_len);
void process_control_dtc_settings(uint8_t* args, uint16_t arg_len);
void process_response_on_event(uint8_t* args, uint16_t arg_len);
void process_shift_mgr_op(uint8_t* args, uint16_t arg_len);
@ -115,6 +100,8 @@ class Kwp2000_server {
void run_solenoid_test();
xTaskHandle* running_routine;
Flasher* flash_handler = nullptr;
uint8_t supplier_id;
uint16_t diag_var_code;
};
#endif //_KWP_H__

30
src/diag/kwp2000_defines.h
View File

@ -45,6 +45,15 @@ KWP2000 SERVICE IDENTIFIERS
/* Custom SIDs for UN52 */
#define SID_SHIFT_MGR_OP 0x88
#ifdef __cplusplus
extern "C" {
#endif
typedef unsigned char kwp_result_t;
// For completion purposes
#define NRC_OK 0x00
/*
KWP2000 Negative response codes
*/
@ -68,6 +77,27 @@ KWP2000 Negative response codes
#define NRC_DATA_DECRYPTION_FAILED 0x9B
#define NRC_ECU_NOT_RESPONDING 0xA0
#ifdef __cplusplus
}
#endif
/**
Extended Negative response codes (Only for UN52 diag mode (0x93))
This helps to return ESP_ERR_T types to the config app for better
error handling and error descriptions
*/
#define NRC_UN52_NO_MEM 0x72 // Malloc failure
#define NRC_UN52_ENGINE_ON 0x73 // Engine on
#define NRC_UN52_ENGINE_OFF 0x74 // Engine off
#define NRC_UN52_SHIFTER_PASSIVE 0x75 // Passive position (P/N/SNV)
#define NRC_UN52_SHIFTER_ACTIVE 0x76 // Active position (R/D)
// OTA errors
#define NRC_UN52_NULL_PARTITION 0x41
#define NRC_UN52_OTA_BEGIN_FAIL 0x42
#define NRC_UN52_OTA_WRITE_FAIL 0x43
#define NRC_UN52_OTA_INVALID_TY 0x44
/*
KWP Response types (Used by multiple SID parameters)
*/

87
src/diag/kwp_utils.cpp
View File

@ -13,12 +13,12 @@ void global_make_diag_neg_msg(DiagMessage *dest, uint8_t sid, uint8_t nrc) {
void global_make_diag_pos_msg(DiagMessage *dest, uint8_t sid, const uint8_t* resp, uint16_t len) {
if (len + 2 > DIAG_CAN_MAX_SIZE) {
global_make_diag_neg_msg(dest, sid, NRC_GENERAL_REJECT);
return;
} else {
dest->id = KWP_ECU_TX_ID;
dest->data_size = len+1;
dest->data[0] = sid+0x40;
memcpy(&dest->data[1], resp, len);
}
dest->id = KWP_ECU_TX_ID;
dest->data_size = len+1;
dest->data[0] = sid+0x40;
memcpy(&dest->data[1], resp, len);
}
void global_make_diag_pos_msg(DiagMessage *dest, uint8_t sid, uint8_t pid, const uint8_t* resp, uint16_t len) {
@ -33,23 +33,76 @@ void global_make_diag_pos_msg(DiagMessage *dest, uint8_t sid, uint8_t pid, const
memcpy(&dest->data[2], resp, len);
}
bool is_engine_off(AbstractCan* can) {
bool is_engine_off(EgsBaseCan* can) {
// Engine MUST be off (Ignition state)
int rpm = egs_can_hal->get_engine_rpm(esp_timer_get_time()/1000, 250);
if (rpm != 0 && rpm != UINT16_MAX) { // 0 = 0RPM, MAX = SNV (Engine ECU is offline)
return false;
}
return true;
return (rpm == 0 || rpm == UINT16_MAX); // 0 = 0RPM, MAX = SNV (Engine ECU is offline)
}
bool is_shifter_passive(AbstractCan* can) {
bool is_shifter_passive(EgsBaseCan* can) {
// Shifter must be Offline (SNV) or P or N
ShifterPosition pos = can->get_shifter_position_ewm(esp_timer_get_time()/1000, 250);
if (
pos == ShifterPosition::D || pos == ShifterPosition::MINUS || pos == ShifterPosition::PLUS || pos == ShifterPosition::R || // Stationary positions
pos == ShifterPosition::N_D || pos == ShifterPosition::P_R || pos == ShifterPosition::R_N // Intermediate positions
) {
return false;
return (pos == ShifterPosition::N || pos == ShifterPosition::P || pos == ShifterPosition::SignalNotAvailable);
}
// Error lookup
#define ERR_TABLE_ADD(err) { err, #err }
typedef struct {
kwp_result_t code;
const char* name;
} kwp_err_desc;
const char* kwp_nrc_unknown = "NRC_UNKNOWN";
static const kwp_err_desc kwp_err_table[] = {
ERR_TABLE_ADD(NRC_OK),
ERR_TABLE_ADD(NRC_GENERAL_REJECT),
ERR_TABLE_ADD(NRC_SERVICE_NOT_SUPPORTED),
ERR_TABLE_ADD(NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT),
ERR_TABLE_ADD(NRC_BUSY_REPEAT_REQUEST),
ERR_TABLE_ADD(NRC_CONDITIONS_NOT_CORRECT_REQ_SEQ_ERROR),
ERR_TABLE_ADD(NRC_ROUTINE_NOT_COMPLETE),
ERR_TABLE_ADD(NRC_REQUEST_OUT_OF_RANGE),
ERR_TABLE_ADD(NRC_SECURITY_ACCESS_DENIED),
ERR_TABLE_ADD(NRC_INVALID_KEY),
ERR_TABLE_ADD(NRC_EXCEED_NUMBER_OF_ATTEMPTS),
ERR_TABLE_ADD(NRC_REQUIRED_TIME_DELAY_NOT_EXPIRED),
ERR_TABLE_ADD(NRC_DOWNLOAD_NOT_ACCEPTED),
ERR_TABLE_ADD(NRC_UPLOAD_NOT_ACCEPTED),
ERR_TABLE_ADD(NRC_TRANSFER_SUSPENDED),
ERR_TABLE_ADD(NRC_RESPONSE_PENDING),
ERR_TABLE_ADD(NRC_SERVICE_NOT_SUPPORTED_IN_ACTIVE_DIAG_SESSION),
ERR_TABLE_ADD(NRC_DATA_DECOMPRESSION_FAILED),
ERR_TABLE_ADD(NRC_DATA_DECRYPTION_FAILED),
ERR_TABLE_ADD(NRC_ECU_NOT_RESPONDING),
ERR_TABLE_ADD(NRC_UN52_NO_MEM),
ERR_TABLE_ADD(NRC_UN52_ENGINE_ON),
ERR_TABLE_ADD(NRC_UN52_ENGINE_OFF),
ERR_TABLE_ADD(NRC_UN52_SHIFTER_PASSIVE),
ERR_TABLE_ADD(NRC_UN52_SHIFTER_ACTIVE),
ERR_TABLE_ADD(NRC_UN52_NULL_PARTITION),
ERR_TABLE_ADD(NRC_UN52_OTA_BEGIN_FAIL),
ERR_TABLE_ADD(NRC_UN52_OTA_WRITE_FAIL),
ERR_TABLE_ADD(NRC_UN52_OTA_INVALID_TY),
};
const char* kwp_nrc_to_name(kwp_result_t res) {
const char* def = nullptr;
for (int i = 0; i < sizeof(kwp_err_table) / sizeof(kwp_err_desc); i++) {
if (kwp_err_table[i].code == res) {
def = kwp_err_table->name;
break;
}
}
return true;
if (def == nullptr) {
def = kwp_nrc_unknown;
}
return def;
}

8
src/diag/kwp_utils.h
View File

@ -2,7 +2,7 @@
#ifndef __KWP_UTILS_H__
#define __KWP_UTILS_H__
#include "endpoint.h"
#include "endpoints/endpoint.h"
#include "stdint.h"
// Couple of helpful functions
@ -12,7 +12,9 @@ void global_make_diag_pos_msg(DiagMessage *dest, uint8_t sid, const uint8_t* res
void global_make_diag_pos_msg(DiagMessage *dest, uint8_t sid, uint8_t pid, const uint8_t* resp, uint16_t len);
bool is_engine_off(AbstractCan* can);
bool is_shifter_passive(AbstractCan* can);
bool is_engine_off(EgsBaseCan* can);
bool is_shifter_passive(EgsBaseCan* can);
const char* kwp_nrc_to_name(kwp_result_t res);
#endif

272
src/diag/map_editor.cpp
View File

@ -1,153 +1,159 @@
#include "map_editor.h"
#include "kwp2000_defines.h"
#include "../nvs/eeprom_config.h"
#include "nvs/eeprom_config.h"
#include "../maps.h"
#include "string.h"
#include "../speaker.h"
#include "../profiles.h"
#include "speaker.h"
#include "profiles.h"
#include "pressure_manager.h"
const char* map_id_to_name(uint8_t map_id) {
StoredTcuMap* get_map(uint8_t map_id) {
switch(map_id) {
case MAP_ID_S_DIESEL_UPSHIFT:
return MAP_NAME_S_DIESEL_UPSHIFT;
case MAP_ID_S_DIESEL_DOWNSHIFT:
return MAP_NAME_S_DIESEL_DOWNSHIFT;
case MAP_ID_S_PETROL_UPSHIFT:
return MAP_NAME_S_PETROL_UPSHIFT;
case MAP_ID_S_PETROL_DOWNSHIFT:
return MAP_NAME_S_PETROL_DOWNSHIFT;
case MAP_ID_C_DIESEL_UPSHIFT:
return MAP_NAME_C_DIESEL_UPSHIFT;
case MAP_ID_C_DIESEL_DOWNSHIFT:
return MAP_NAME_C_DIESEL_DOWNSHIFT;
case MAP_ID_C_PETROL_UPSHIFT:
return MAP_NAME_C_PETROL_UPSHIFT;
case MAP_ID_C_PETROL_DOWNSHIFT:
return MAP_NAME_C_PETROL_DOWNSHIFT;
case MAP_ID_A_DIESEL_UPSHIFT:
return MAP_NAME_A_DIESEL_UPSHIFT;
case MAP_ID_A_DIESEL_DOWNSHIFT:
return MAP_NAME_A_DIESEL_DOWNSHIFT;
case MAP_ID_A_PETROL_UPSHIFT:
return MAP_NAME_A_PETROL_UPSHIFT;
case MAP_ID_A_PETROL_DOWNSHIFT:
return MAP_NAME_A_PETROL_DOWNSHIFT;
case A_UPSHIFT_MAP_ID:
return agility->get_upshift_map();
case C_UPSHIFT_MAP_ID:
return comfort->get_upshift_map();
case S_UPSHIFT_MAP_ID:
return standard->get_upshift_map();
case A_DOWNSHIFT_MAP_ID:
return agility->get_downshift_map();
case C_DOWNSHIFT_MAP_ID:
return comfort->get_downshift_map();
case S_DOWNSHIFT_MAP_ID:
return standard->get_downshift_map();
case WORKING_PRESSURE_MAP_ID:
return pressure_manager->get_working_map();
case PCS_CURRENT_MAP_ID:
return pressure_manager->get_pcs_map();
case TCC_PWM_MAP_ID:
return pressure_manager->get_tcc_pwm_map();
case FILL_TIME_MAP_ID:
return pressure_manager->get_fill_time_map();
case FILL_PRESSURE_MAP_ID:
return pressure_manager->get_fill_pressure_map();
case A_UPTIME_MAP_ID:
return agility->get_upshift_time_map();
case A_DNTIME_MAP_ID:
return agility->get_downshift_time_map();
case S_UPTIME_MAP_ID:
return standard->get_upshift_time_map();
case S_DNTIME_MAP_ID:
return standard->get_downshift_time_map();
case C_UPTIME_MAP_ID:
return comfort->get_upshift_time_map();
case C_DNTIME_MAP_ID:
return comfort->get_downshift_time_map();
case W_UPTIME_MAP_ID:
return winter->get_upshift_time_map();
case W_DNTIME_MAP_ID:
return winter->get_downshift_time_map();
case M_UPTIME_MAP_ID:
return manual->get_upshift_time_map();
case M_DNTIME_MAP_ID:
return manual->get_downshift_time_map();
default:
return nullptr;
}
}
const int16_t* get_default_map(uint8_t map_id) {
switch(map_id) {
case MAP_ID_S_DIESEL_UPSHIFT:
return S_DIESEL_UPSHIFT_MAP;
case MAP_ID_S_DIESEL_DOWNSHIFT:
return S_DIESEL_DOWNSHIFT_MAP;
case MAP_ID_S_PETROL_UPSHIFT:
return S_PETROL_UPSHIFT_MAP;
case MAP_ID_S_PETROL_DOWNSHIFT:
return S_PETROL_DOWNSHIFT_MAP;
case MAP_ID_C_DIESEL_UPSHIFT:
return C_DIESEL_UPSHIFT_MAP;
case MAP_ID_C_DIESEL_DOWNSHIFT:
return C_DIESEL_DOWNSHIFT_MAP;
case MAP_ID_C_PETROL_UPSHIFT:
return C_PETROL_UPSHIFT_MAP;
case MAP_ID_C_PETROL_DOWNSHIFT:
return C_PETROL_DOWNSHIFT_MAP;
case MAP_ID_A_DIESEL_UPSHIFT:
return A_DIESEL_UPSHIFT_MAP;
case MAP_ID_A_DIESEL_DOWNSHIFT:
return A_DIESEL_DOWNSHIFT_MAP;
case MAP_ID_A_PETROL_UPSHIFT:
return A_PETROL_UPSHIFT_MAP;
case MAP_ID_A_PETROL_DOWNSHIFT:
return A_PETROL_DOWNSHIFT_MAP;
default:
return nullptr;
#define CHECK_MAP(map_id) \
StoredTcuMap* ptr = get_map(map_id); \
if (ptr == nullptr) { \
return NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT; \
}
}
uint8_t MapEditor::read_map_data(uint8_t map_id, uint16_t *dest_size, int16_t** buffer) {
uint8_t map_idx = map_id & 0b1111111;
bool is_default_map = (map_id & 0b10000000) != 0;
if (map_idx >= 0x01 && map_idx <= 0x0C) {
// Shift map
int16_t* b = (int16_t*)heap_caps_malloc(SHIFT_MAP_SIZE*sizeof(int16_t), MALLOC_CAP_SPIRAM);
if (b == nullptr) {
ESP_LOGE("MAP_EDITOR_R", "Could not allocate read array!");
return NRC_GENERAL_REJECT;
}
if (is_default_map) {
// Read default map from flash
const int16_t* default_map = get_default_map(map_idx);
if (default_map == nullptr) {
ESP_LOGE("MAP_EDITOR_R", "default map is invalid!?");
delete b;
return NRC_GENERAL_REJECT;
}
memcpy(b, default_map, SHIFT_MAP_SIZE*sizeof(int16_t));
*buffer = b;
*dest_size = SHIFT_MAP_SIZE*sizeof(int16_t);
return 0;
} else {
const char* name = map_id_to_name(map_idx);
if (name == nullptr) {
ESP_LOGE("MAP_EDITOR_R", "map name is null!?");
delete b;
return NRC_GENERAL_REJECT;
}
if (EEPROM::read_nvs_map_data(name, b, nullptr, SHIFT_MAP_SIZE)) {
// OK, copy the pointer
*buffer = b;
*dest_size = SHIFT_MAP_SIZE*sizeof(int16_t);
return 0;
} else {
// De-allocate
ESP_LOGE("MAP_EDITOR_R", "read_nvs_map_data failed!");
delete b;
return NRC_GENERAL_REJECT;
}
}
kwp_result_t MapEditor::read_map_data(uint8_t map_id, uint8_t read_type, uint16_t *dest_size_bytes, uint8_t** buffer) {
CHECK_MAP(map_id)
// Map valid
uint16_t size = ptr->get_map_element_count();
uint8_t* b = static_cast<uint8_t*>(heap_caps_malloc((size*sizeof(int16_t)), MALLOC_CAP_SPIRAM));
if (b == nullptr) {
ESP_LOGE("MAP_EDITOR_R", "Could not allocate read array!");
return NRC_UN52_NO_MEM;
}
if (read_type == MAP_READ_TYPE_PRG) {
memcpy(b, ptr->get_default_map_data(), size*sizeof(int16_t));
} else if (read_type == MAP_READ_TYPE_MEM) {
memcpy(b, ptr->get_current_data(), size*sizeof(int16_t));
} else if (read_type == MAP_READ_TYPE_STO) {
int16_t* eeprom_data = ptr->get_current_eeprom_map_data();
memcpy(b, eeprom_data, size*sizeof(int16_t));
free(eeprom_data);
} else {
return NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT;
free(buffer);
return NRC_GENERAL_REJECT;
}
*buffer = b;
*dest_size_bytes = size*sizeof(int16_t);
return NRC_OK;
}
kwp_result_t MapEditor::read_map_metadata(uint8_t map_id, uint16_t *dest_size_bytes, uint8_t** buffer) {
CHECK_MAP(map_id);
// X meta, Y meta, KEY_NAME
int16_t* x_ptr;
int16_t* y_ptr;
const char* k_ptr;
uint16_t x_size;
uint16_t y_size;
uint16_t k_size;
ptr->get_x_headers(&x_size, &x_ptr);
ptr->get_y_headers(&y_size, &y_ptr);
k_ptr = ptr->get_map_name();
k_size = strlen(k_ptr);
// 6 bytes for size data
uint16_t size = 6+k_size+((x_size+y_size)*sizeof(int16_t));
uint8_t* b = static_cast<uint8_t*>(heap_caps_malloc(size, MALLOC_CAP_SPIRAM));
if (b == nullptr) {
return NRC_UN52_NO_MEM;
}
b[1] = x_size >> 8;
b[0] = x_size & 0xFF;
b[3] = y_size >> 8;
b[2] = y_size & 0xFF;
b[5] = k_size >> 8;
b[4] = k_size & 0xFF;
memcpy(&b[6], x_ptr, x_size*sizeof(int16_t));
memcpy(&b[6+(x_size*sizeof(int16_t))], y_ptr, y_size*sizeof(int16_t));
memcpy(&b[6+((x_size+y_size)*sizeof(int16_t))], k_ptr, k_size);
*buffer = b;
*dest_size_bytes = size;
return NRC_OK;
}
uint8_t MapEditor::write_map_data(uint8_t map_id, uint16_t dest_size, int16_t* buffer) {
if (map_id >= 0x01 && map_id <= 0x0C) {
const char* name = map_id_to_name(map_id);
if (name == nullptr) {
ESP_LOGE("MAP_EDITOR_W", "map name is null!?");
return NRC_GENERAL_REJECT;
}
if (dest_size != SHIFT_MAP_SIZE*sizeof(int16_t)) {
ESP_LOGE("MAP_EDITOR_W", "Buffer has %d bytes. Shift map needs %d bytes", dest_size, sizeof(int16_t)*SHIFT_MAP_SIZE);
return NRC_GENERAL_REJECT;
}
if (EEPROM::write_nvs_map_data(name, buffer, SHIFT_MAP_SIZE)) {
spkr.send_note(1500, 300, 310);
return 0;
} else {
ESP_LOGE("MAP_EDITOR_W", "write_nvs_map_data failed!");
return NRC_GENERAL_REJECT;
}
} else {
return NRC_SUB_FUNC_NOT_SUPPORTED_INVALID_FORMAT;
}
}
uint8_t MapEditor::trigger_reload(uint8_t prof_id) {
uint8_t ret = 0;
if (prof_id == standard->get_profile_id()) {
standard->reload_data();
} else if (prof_id == comfort->get_profile_id()) {
comfort->reload_data();
} else if (prof_id == agility->get_profile_id()) {
agility->reload_data();
kwp_result_t MapEditor::write_map_data(uint8_t map_id, uint16_t dest_size, int16_t* buffer) {
CHECK_MAP(map_id)
if (ptr->replace_map_content(buffer, dest_size) == ESP_OK) {
return NRC_OK;
} else {
ret = NRC_REQUEST_OUT_OF_RANGE;
return NRC_GENERAL_REJECT;
}
}
kwp_result_t MapEditor::burn_to_eeprom(uint8_t map_id) {
CHECK_MAP(map_id)
if (ptr->save_to_eeprom() == ESP_OK) {
return NRC_OK;
} else {
return NRC_GENERAL_REJECT;
}
}
// uint8_t MapEditor::reset_to_program_default(uint8_t map_id) {
// CHECK_MAP(map_id)
// if (ptr->reload_from_eeprom() && ptr->save_to_eeprom()) {
// return 0;
// } else {
// return NRC_GENERAL_REJECT;
// }
// }
kwp_result_t MapEditor::undo_changes(uint8_t map_id) {
CHECK_MAP(map_id)
if (ptr->reload_from_eeprom() == ESP_OK) {
return NRC_OK;
} else {
return NRC_GENERAL_REJECT;
}
return ret;
}

56
src/diag/map_editor.h
View File

@ -2,28 +2,54 @@
#define __MAP_EDITOR_H_
#include <stdint.h>
#include "kwp2000_defines.h"
#define MAP_ID_S_DIESEL_UPSHIFT 0x01
#define MAP_ID_S_DIESEL_DOWNSHIFT 0x02
#define MAP_ID_S_PETROL_UPSHIFT 0x03
#define MAP_ID_S_PETROL_DOWNSHIFT 0x04
#define A_UPSHIFT_MAP_ID 0x01
#define C_UPSHIFT_MAP_ID 0x02
#define S_UPSHIFT_MAP_ID 0x03
#define A_DOWNSHIFT_MAP_ID 0x04
#define C_DOWNSHIFT_MAP_ID 0x05
#define S_DOWNSHIFT_MAP_ID 0x06
#define MAP_ID_C_DIESEL_UPSHIFT 0x05
#define MAP_ID_C_DIESEL_DOWNSHIFT 0x06
#define MAP_ID_C_PETROL_UPSHIFT 0x07
#define MAP_ID_C_PETROL_DOWNSHIFT 0x08
#define WORKING_PRESSURE_MAP_ID 0x07
#define PCS_CURRENT_MAP_ID 0x08
#define TCC_PWM_MAP_ID 0x09
#define FILL_TIME_MAP_ID 0x0A
#define FILL_PRESSURE_MAP_ID 0x0B
#define MAP_ID_A_DIESEL_UPSHIFT 0x09
#define MAP_ID_A_DIESEL_DOWNSHIFT 0x0A
#define MAP_ID_A_PETROL_UPSHIFT 0x0B
#define MAP_ID_A_PETROL_DOWNSHIFT 0x0C
#define A_UPTIME_MAP_ID 0x10
#define A_DNTIME_MAP_ID 0x11
#define S_UPTIME_MAP_ID 0x12
#define S_DNTIME_MAP_ID 0x13
#define C_UPTIME_MAP_ID 0x14
#define C_DNTIME_MAP_ID 0x15
#define W_UPTIME_MAP_ID 0x16
#define W_DNTIME_MAP_ID 0x17
#define M_UPTIME_MAP_ID 0x18
#define M_DNTIME_MAP_ID 0x19
// MAP COMMAND IDs
#define MAP_CMD_READ 0x01
#define MAP_CMD_READ_DEFAULT 0x02
#define MAP_CMD_WRITE 0x03
#define MAP_CMD_BURN 0x04
#define MAP_CMD_RESET_TO_FLASH 0x05
#define MAP_CMD_UNDO 0x06
#define MAP_CMD_READ_META 0x07
#define MAP_CMD_READ_EEPROM 0x08
#define MAP_READ_TYPE_MEM 0x01
#define MAP_READ_TYPE_PRG 0x02
#define MAP_READ_TYPE_STO 0x03
namespace MapEditor {
uint8_t read_map_data(uint8_t map_id, uint16_t *dest_size, int16_t** buffer);
uint8_t write_map_data(uint8_t map_id, uint16_t dest_size, int16_t* buffer);
uint8_t trigger_reload(uint8_t prof_id);
kwp_result_t read_map_metadata(uint8_t map_id, uint16_t *dest_size_bytes, uint8_t** buffer);
kwp_result_t read_map_data(uint8_t map_id, uint8_t read_type, uint16_t *dest_size_bytes, uint8_t** buffer);
kwp_result_t write_map_data(uint8_t map_id, uint16_t dest_size, int16_t* buffer);
kwp_result_t burn_to_eeprom(uint8_t map_id);
kwp_result_t reset_to_program_default(uint8_t map_id);
kwp_result_t undo_changes(uint8_t map_id);
}
#endif

95
src/diag/perf_mon.cpp
View File

@ -4,35 +4,30 @@
#include <esp_freertos_hooks.h>
#include <esp_timer.h>
#include <driver/timer.h>
#include "macros.h"
#define LOAD_FETCH_INTERVAL_MS 100
#ifdef LOG_TAG
#undef LOG_TAG
#endif
#define LOG_TAG "PERFMON"
#include "esp_log.h"
#include "esp_check.h"
// 346834 per second
volatile uint64_t _idle_ticks_c1 = 0;
volatile uint64_t _idle_ticks_c2 = 0;
#define TICKS_PER_MS 371000.0 / 1000.0
const uint64_t LOAD_FETCH_INTERVAL_MS = 100u;
const uint64_t TICKS_PER_MS = 371000u / 1000u;
const uint64_t MAX_TICKS = TICKS_PER_MS * LOAD_FETCH_INTERVAL_MS;
CpuStats dest;
bool perfmon_running = false;
static bool idle_hook_c1() {
static bool idle_hook_c1(void)
{
_idle_ticks_c1++;
return false;
}
static bool idle_hook_c2() {
static bool idle_hook_c2(void)
{
_idle_ticks_c2++;
return false;
}
@ -42,57 +37,61 @@ uint64_t i1 = 0;
// Guaranteed to run every 100ms
static intr_handle_t load_timer_handle;
static void IRAM_ATTR cpu_load_interrupt(void* arg) {
static void IRAM_ATTR cpu_load_interrupt(void *arg)
{
TIMERG0.int_clr_timers.t1 = 1;
TIMERG0.hw_timer[1].config.alarm_en = 1;
i0 = _idle_ticks_c1;
i1 = _idle_ticks_c2;
_idle_ticks_c1 = 0;
_idle_ticks_c2 = 0;
if (i0 <= MAX_TICKS) {
dest.load_core_1 = 1000 - ((1000 * i0)/MAX_TICKS);
if (i0 <= MAX_TICKS)
{
dest.load_core_1 = 1000 - ((1000 * i0) / MAX_TICKS);
}
if (i1 <= MAX_TICKS) {
dest.load_core_2 = 1000 - ((1000 * i1)/MAX_TICKS);
if (i1 <= MAX_TICKS)
{
dest.load_core_2 = 1000 - ((1000 * i1) / MAX_TICKS);
}
}
bool init_perfmon() {
if (perfmon_running) {
return true;
const static timer_config_t PERFORM_T_CONFIG = {
.alarm_en = timer_alarm_t::TIMER_ALARM_EN,
.counter_en = timer_start_t::TIMER_START,
.intr_type = TIMER_INTR_LEVEL,
.counter_dir = TIMER_COUNT_UP,
.auto_reload = timer_autoreload_t::TIMER_AUTORELOAD_EN,
.divider = 80 /* 1 us per tick */
};
esp_err_t init_perfmon(void)
{
if (!perfmon_running)
{
dest.load_core_1 = 0;
dest.load_core_2 = 0;
ESP_RETURN_ON_ERROR(timer_init(TIMER_GROUP_0, TIMER_1, &PERFORM_T_CONFIG), "PERFMON", "Timer init failed");
ESP_RETURN_ON_ERROR(timer_set_counter_value(TIMER_GROUP_0, TIMER_1, 0), "PERFMON", "Set counter value failed");
ESP_RETURN_ON_ERROR(timer_set_alarm_value(TIMER_GROUP_0, TIMER_1, LOAD_FETCH_INTERVAL_MS * 1000), "PERFMON", "Set alarm value failed");
ESP_RETURN_ON_ERROR(timer_enable_intr(TIMER_GROUP_0, TIMER_1), "PERFMON", "Enable intr failed");
ESP_RETURN_ON_ERROR(timer_isr_register(TIMER_GROUP_0, TIMER_1, &cpu_load_interrupt, NULL, 0, &load_timer_handle), "PERFMON", "ISR Register failed");
ESP_RETURN_ON_ERROR(timer_start(TIMER_GROUP_0, TIMER_1), "PERFMON", "Timer start failed");
ESP_RETURN_ON_ERROR(esp_register_freertos_idle_hook_for_cpu(idle_hook_c1, 0), "PERFMON", "Failed to set idle hook for Core 0");
ESP_RETURN_ON_ERROR(esp_register_freertos_idle_hook_for_cpu(idle_hook_c2, 1), "PERFMON", "Failed to set idle hook for Core 1");
perfmon_running = true;
ESP_LOG_LEVEL(ESP_LOG_INFO, "PERFMON", "Init OK!");
}
timer_config_t config = {
.alarm_en = timer_alarm_t::TIMER_ALARM_EN,
.counter_en = timer_start_t::TIMER_START,
.intr_type = TIMER_INTR_LEVEL,
.counter_dir = TIMER_COUNT_UP,
.auto_reload = timer_autoreload_t::TIMER_AUTORELOAD_EN,
.divider = 80 /* 1 us per tick */
};
dest.load_core_1 = 0;
dest.load_core_2 = 0;
esp_err_t res;
CHECK_ESP_FUNC(timer_init(TIMER_GROUP_0, TIMER_1, &config), "Timer init failed: %s", esp_err_to_name(res));
CHECK_ESP_FUNC(timer_set_counter_value(TIMER_GROUP_0, TIMER_1, 0), "Set counter value failed: %s", esp_err_to_name(res));
CHECK_ESP_FUNC(timer_set_alarm_value(TIMER_GROUP_0, TIMER_1, LOAD_FETCH_INTERVAL_MS*1000), "Set alarm value failed: %s", esp_err_to_name(res));
CHECK_ESP_FUNC(timer_enable_intr(TIMER_GROUP_0, TIMER_1), "Enable intr failed: %s", esp_err_to_name(res));
CHECK_ESP_FUNC(timer_isr_register(TIMER_GROUP_0, TIMER_1, &cpu_load_interrupt, NULL, 0, &load_timer_handle), "ISR Register failed: %s", esp_err_to_name(res));
CHECK_ESP_FUNC(timer_start(TIMER_GROUP_0, TIMER_1), "Timer start failed: %s", esp_err_to_name(res));
CHECK_ESP_FUNC(esp_register_freertos_idle_hook_for_cpu(idle_hook_c1, 0), "Failed to set idle hook for Core 0: %s", esp_err_to_name(res));
CHECK_ESP_FUNC(esp_register_freertos_idle_hook_for_cpu(idle_hook_c2, 1), "Failed to set idle hook for Core 1: %s", esp_err_to_name(res));
perfmon_running = true;
ESP_LOG_LEVEL(ESP_LOG_INFO, LOG_TAG, "Init OK!");
return true;
return perfmon_running;
}
void remove_perfmon() {
void remove_perfmon(void)
{
timer_deinit(TIMER_GROUP_0, TIMER_1);
perfmon_running = false;
}
CpuStats get_cpu_stats() {
CpuStats get_cpu_stats(void)
{
return dest;
}

17
src/diag/perf_mon.h
View File

@ -1,17 +1,18 @@
#ifndef __PERF_MON_H_
#define __PERF_MON_H_
#ifndef PERF_MON_H
#define PERF_MON_H
#include <stdint.h>
#include <esp_err.h>
typedef struct {
struct CpuStats{
volatile uint16_t load_core_1;
volatile uint16_t load_core_2;
} CpuStats;
} ;
bool init_perfmon();
void remove_perfmon();
esp_err_t init_perfmon(void);
void remove_perfmon(void);
CpuStats get_cpu_stats();
CpuStats get_cpu_stats(void);
#endif // __PERF_MON_H_
#endif // PERF_MON_H

72
src/efuse/efuse.c Normal file
View File

@ -0,0 +1,72 @@
/*
* SPDX-FileCopyrightText: 2017-2022 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: Apache-2.0
*/
#include "sdkconfig.h"
#include "esp_efuse.h"
#include <assert.h>
#include "efuse.h"
// md5_digest_table d6c1172944e02bf3f301a89c299ce6c7
// This file was generated from the file efuse.csv. DO NOT CHANGE THIS FILE MANUALLY.
// If you want to change some fields, you need to change efuse.csv file
// then run `efuse_common_table` or `efuse_custom_table` command it will generate this file.
// To show efuse_table run the command 'show_efuse_table'.
#define MAX_BLK_LEN CONFIG_EFUSE_MAX_BLK_LEN
// The last free bit in the block is counted over the entire file.
#define LAST_FREE_BIT_BLK3 40
_Static_assert(LAST_FREE_BIT_BLK3 <= MAX_BLK_LEN, "The eFuse table does not match the coding scheme. Edit the table and restart the efuse_common_table or efuse_custom_table command to regenerate the new files.");
static const esp_efuse_desc_t BOARD_VER[] = {
{EFUSE_BLK3, 0, 8}, // PCB version,
};
static const esp_efuse_desc_t M_DAY[] = {
{EFUSE_BLK3, 8, 8}, // Manufacture day of month,
};
static const esp_efuse_desc_t M_WEEK[] = {
{EFUSE_BLK3, 16, 8}, // Manufacture week of year,
};
static const esp_efuse_desc_t M_MONTH[] = {
{EFUSE_BLK3, 24, 8}, // Manufacture month of year,
};
static const esp_efuse_desc_t M_YEAR[] = {
{EFUSE_BLK3, 32, 8}, // Manufacture year,
};
const esp_efuse_desc_t* ESP_EFUSE_BOARD_VER[] = {
&BOARD_VER[0], // PCB version
NULL
};
const esp_efuse_desc_t* ESP_EFUSE_M_DAY[] = {
&M_DAY[0], // Manufacture day of month
NULL
};
const esp_efuse_desc_t* ESP_EFUSE_M_WEEK[] = {
&M_WEEK[0], // Manufacture week of year
NULL
};
const esp_efuse_desc_t* ESP_EFUSE_M_MONTH[] = {
&M_MONTH[0], // Manufacture month of year
NULL
};
const esp_efuse_desc_t* ESP_EFUSE_M_YEAR[] = {
&M_YEAR[0], // Manufacture year
NULL
};

28
src/efuse/efuse.h Normal file
View File

@ -0,0 +1,28 @@
/*
* SPDX-FileCopyrightText: 2017-2022 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: Apache-2.0
*/
#ifdef __cplusplus
extern "C" {
#endif
#include "esp_efuse.h"
// md5_digest_table d6c1172944e02bf3f301a89c299ce6c7
// This file was generated from the file efuse.csv. DO NOT CHANGE THIS FILE MANUALLY.
// If you want to change some fields, you need to change efuse.csv file
// then run `efuse_common_table` or `efuse_custom_table` command it will generate this file.
// To show efuse_table run the command 'show_efuse_table'.
extern const esp_efuse_desc_t* ESP_EFUSE_BOARD_VER[];
extern const esp_efuse_desc_t* ESP_EFUSE_M_DAY[];
extern const esp_efuse_desc_t* ESP_EFUSE_M_WEEK[];
extern const esp_efuse_desc_t* ESP_EFUSE_M_MONTH[];
extern const esp_efuse_desc_t* ESP_EFUSE_M_YEAR[];
#ifdef __cplusplus
}
#endif

1521
src/gearbox.cpp
View File

File diff suppressed because it is too large Load Diff

47
src/gearbox.h
View File

@ -1,7 +1,7 @@
// Here we go, gearbox controller code! Lets go!
#ifndef __GEARBOX_H_
#define __GEARBOX_H_
#ifndef GEARBOX_H
#define GEARBOX_H
#include <stdint.h>
#include "canbus/can_hal.h"
@ -19,12 +19,9 @@
// TODO Auto-set these based on CAN data about engine type
// 4000 is safe for now as it stops us over-revving diesel!
#define STALL_RPM 700
#define MIN_WORKING_RPM 1000
static const int MIN_WORKING_RPM = 1000;
#define OVERSPEED_RPM 15000
#define ATF_TEMP_SAMPLES 20
static const uint16_t ATF_TEMP_SAMPLES = 20;
struct TempSampleData {
int samples[ATF_TEMP_SAMPLES];
uint64_t total;
@ -38,7 +35,7 @@ const static GearRatioLimit GEAR_RATIO_LIMITS_SMALL[7] {
GearRatioLimit { .max = RAT_2_SMALL*(1.0+MAX_LIMIT), .min = RAT_2_SMALL*(1.0-MAX_LIMIT) }, // 2
GearRatioLimit { .max = RAT_3_SMALL*(1.0+MAX_LIMIT), .min = RAT_3_SMALL*(1.0-MAX_LIMIT) }, // 3
GearRatioLimit { .max = RAT_4_SMALL*(1.0+MAX_LIMIT), .min = RAT_4_SMALL*(1.0-MAX_LIMIT) }, // 4
GearRatioLimit { .max = RAT_5_SMALL*(1.0+MAX_LIMIT/2), .min = RAT_5_SMALL*(1.0-MAX_LIMIT) }, // 5
GearRatioLimit { .max = RAT_5_SMALL*(1.0+(MAX_LIMIT/2)), .min = RAT_5_SMALL*(1.0-MAX_LIMIT) }, // 5
GearRatioLimit { .max = RAT_R1_SMALL*(1.0-MAX_LIMIT), .min = RAT_R1_SMALL*(1.0+MAX_LIMIT) }, // R1
GearRatioLimit { .max = RAT_R2_SMALL*(1.0-MAX_LIMIT), .min = RAT_R2_SMALL*(1.0+MAX_LIMIT) }, // R2
};
@ -48,7 +45,7 @@ const static GearRatioLimit GEAR_RATIO_LIMITS_LARGE[7] {
GearRatioLimit { .max = RAT_2_LARGE*(1.0+MAX_LIMIT), .min = RAT_2_LARGE*(1.0-MAX_LIMIT) }, // 2
GearRatioLimit { .max = RAT_3_LARGE*(1.0+MAX_LIMIT), .min = RAT_3_LARGE*(1.0-MAX_LIMIT) }, // 3
GearRatioLimit { .max = RAT_4_LARGE*(1.0+MAX_LIMIT), .min = RAT_4_LARGE*(1.0-MAX_LIMIT) }, // 4
GearRatioLimit { .max = RAT_5_LARGE*(1.0+MAX_LIMIT/2), .min = RAT_5_LARGE*(1.0-MAX_LIMIT) }, // 5
GearRatioLimit { .max = RAT_5_LARGE*(1.0+(MAX_LIMIT/2)), .min = RAT_5_LARGE*(1.0-MAX_LIMIT) }, // 5
GearRatioLimit { .max = RAT_R1_LARGE*(1.0-MAX_LIMIT), .min = RAT_R1_LARGE*(1.0+MAX_LIMIT) }, // R1
GearRatioLimit { .max = RAT_R2_LARGE*(1.0-MAX_LIMIT), .min = RAT_R2_LARGE*(1.0+MAX_LIMIT) }, // R2
};
@ -61,7 +58,7 @@ const static FwdRatios RATIOS_LARGE {
RAT_5_LARGE,
RAT_R1_LARGE,
RAT_R2_LARGE
};
};
const static FwdRatios RATIOS_SMALL {
RAT_1_SMALL,
@ -75,25 +72,25 @@ const static FwdRatios RATIOS_SMALL {
class Gearbox {
public:
Gearbox();
Gearbox(void);
void set_profile(AbstractProfile* prof);
void inc_subprofile();
bool start_controller();
void inc_gear_request();
void dec_gear_request();
void diag_inhibit_control() { this->diag_stop_control = true; }
void diag_regain_control() { this->diag_stop_control = false; }
void inc_subprofile(void);
esp_err_t start_controller(void);
void inc_gear_request(void);
void dec_gear_request(void);
void diag_inhibit_control(void) { this->diag_stop_control = true; }
void diag_regain_control(void) { this->diag_stop_control = false; }
SensorData sensor_data;
uint16_t get_gear_ratio() {
return this->sensor_data.gear_ratio * 100;
uint16_t get_gear_ratio(void) {
return this->sensor_data.gear_ratio * 100.0F;
}
static uint16_t redline_rpm;
ShiftReporter* shift_reporter;
bool shifting = false;
PressureManager* pressure_mgr = nullptr;
bool isShifting() { return this->shifting; }
ProfileGearChange get_curr_gear_change() { return this->shift_idx; }
bool isShifting(void) { return this->shifting; }
ProfileGearChange get_curr_gear_change(void) { return this->shift_idx; }
private:
bool elapse_shift(ProfileGearChange req_lookup, AbstractProfile* profile, bool is_upshift);
bool calcGearFromRatio(bool is_reverse);
@ -106,9 +103,9 @@ private:
bool calc_input_rpm(uint16_t* dest);
bool calc_output_rpm(uint16_t* dest, uint64_t now);
[[noreturn]]
void controller_loop();
void controller_loop(void);
void shift_thread();
void shift_thread(void);
bool start_second = true; // By default
static void start_shift_thread(void *_this) {
static_cast<Gearbox*>(_this)->shift_thread();
@ -122,7 +119,7 @@ private:
TaskHandle_t shift_task = nullptr;
bool ask_upshift = false;
bool ask_downshift = false;
float tcc_percent = 0;
float tcc_percent = 0.F;
uint8_t est_gear_idx = 0;
uint16_t curr_hold_pressure = 0;
bool show_upshift = false;
@ -134,7 +131,7 @@ private:
TorqueConverter* tcc = nullptr;
TempSampleData temp_data;
bool diag_stop_control = false;
ShifterPosition shifter_pos = ShifterPosition::SignalNotAvaliable;
ShifterPosition shifter_pos = ShifterPosition::SignalNotAvailable;
GearboxConfiguration gearboxConfig;
float diff_ratio_f;
bool is_ramp = false;

202
src/main.cpp
View File

@ -1,6 +1,5 @@
#ifndef UNIT_TEST
#include "scn.h"
#include "solenoids/solenoids.h"
#include "esp_log.h"
#include <freertos/FreeRTOS.h>
@ -16,104 +15,129 @@
#include "adaptation/adapt_map.h"
#include "solenoids/constant_current.h"
#ifdef EGS51_MODE
#include "canbus/can_egs51.h"
#endif
#ifdef EGS52_MODE
#include "canbus/can_egs52.h"
#endif
#ifdef EGS53_MODE
#include "canbus/can_egs53.h"
#endif
#if defined(EGS51_MODE) && !defined(BOARD_V2)
#error "EGS51 mode can ONLY be supported on V2 boards!"
#endif
// Sanity check
#if !defined(EGS51_MODE) && !defined(EGS52_MODE) && !defined(EGS53_MODE)
#error "No CAN definition (EGS51/EGS52/EGS53)"
#endif
#if (defined(EGS52_MODE) && defined(EGS53_MODE)) || (defined(EGS51_MODE) && defined(EGS52_MODE)) || (defined(EGS51_MODE) && defined(EGS53_MODE))
#error "Multiple EGS CAN layers CANNOT be enabled at the same time!"
#endif
// CAN LAYERS
#include "canbus/can_egs51.h"
#include "canbus/can_egs52.h"
#include "canbus/can_egs53.h"
#include "board_config.h"
Kwp2000_server* diag_server;
uint8_t profile_id = 0;
#define NUM_PROFILES 5 // A, C, W, M, S
#define NUM_PROFILES 6 // A, C, W, M, S, R
AbstractProfile* profiles[NUM_PROFILES];
SPEAKER_POST_CODE setup_tcm()
{
#ifdef EGS51_MODE
#pragma message("Building with EGS51 CAN support")
egs_can_hal = new Egs51Can("EGS51", 20); // EGS51 CAN Abstraction layer
#endif
#ifdef EGS52_MODE
#pragma message("Building with EGS52 CAN support")
egs_can_hal = new Egs52Can("EGS52", 20); // EGS52 CAN Abstraction layer
#endif
#ifdef EGS53_MODE
#pragma message("Building with EGS53 CAN support")
egs_can_hal = new Egs53Can("EGS53", 20); // EGS53 CAN Abstraction layer
#endif
if (!egs_can_hal->begin_tasks()) {
return SPEAKER_POST_CODE::CAN_FAIL;
}
if (!Sensors::init_sensors()) {
return SPEAKER_POST_CODE::SENSOR_FAIL;
}
if(!Solenoids::init_all_solenoids()) {
return SPEAKER_POST_CODE::SOLENOID_FAIL;
}
if(!CurrentDriver::init_current_driver()) {
return SPEAKER_POST_CODE::SOLENOID_FAIL;
}
if (!EEPROM::init_eeprom()) {
return SPEAKER_POST_CODE::EEPROM_FAIL;
}
Speaker* spkr2 = nullptr;
standard = new StandardProfile(VEHICLE_CONFIG.engine_type == 0);
comfort = new ComfortProfile(VEHICLE_CONFIG.engine_type == 0);
winter = new WinterProfile(VEHICLE_CONFIG.engine_type == 0);
agility = new AgilityProfile(VEHICLE_CONFIG.engine_type == 0);
manual = new ManualProfile(VEHICLE_CONFIG.engine_type == 0);
profiles[0] = standard;
profiles[1] = comfort;
profiles[2] = winter;
profiles[3] = agility;
profiles[4] = manual;
// Read profile ID on startup based on TCM config
profile_id = VEHICLE_CONFIG.default_profile;
if (profile_id > 4) {
profile_id = 0;
SPEAKER_POST_CODE setup_tcm() {
SPEAKER_POST_CODE ret = SPEAKER_POST_CODE::INIT_OK; // OK by default
if (EEPROM::read_efuse_config(&BOARD_CONFIG) != ESP_OK) {
ret = SPEAKER_POST_CODE::EFUSE_NOT_SET;
} else {
// First thing to do, Configure the GPIO Pin matrix!
switch (BOARD_CONFIG.board_ver) {
case 1:
pcb_gpio_matrix = new BoardV11GpioMatrix();
break;
case 2:
pcb_gpio_matrix = new BoardV12GpioMatrix();
break;
case 3:
pcb_gpio_matrix = new BoardV13GpioMatrix();
break;
default:
pcb_gpio_matrix = nullptr;
egs_can_hal = nullptr;
spkr = new Speaker(gpio_num_t::GPIO_NUM_4); // Assume legacy when this fails!
spkr2 = new Speaker(gpio_num_t::GPIO_NUM_0); // For new PCBs
ret = SPEAKER_POST_CODE::EFUSE_NOT_SET;
}
if (ret == SPEAKER_POST_CODE::INIT_OK) {
spkr = new Speaker(pcb_gpio_matrix->spkr_pin);
if (EEPROM::init_eeprom() != ESP_OK) {
ret = SPEAKER_POST_CODE::EEPROM_FAIL;
} else {
switch (VEHICLE_CONFIG.egs_can_type) {
case 1:
egs_can_hal = new Egs51Can("EGS51", 20, 500000); // EGS51 CAN Abstraction layer
break;
case 2:
egs_can_hal = new Egs52Can("EGS52", 20, 500000); // EGS52 CAN Abstraction layer
break;
case 3:
egs_can_hal = new Egs53Can("EGS53", 20, 500000); // EGS53 CAN Abstraction layer
break;
default:
// Unknown (Fallback to basic CAN)
ESP_LOGE("INIT", "ERROR. CAN Mode not set, falling back to basic CAN (Diag only!)");
egs_can_hal = new EgsBaseCan("EGSBASIC", 20, 500000);
break;
}
}
if (!egs_can_hal->begin_tasks()) {
ret = SPEAKER_POST_CODE::CAN_FAIL;
} else {
if (Sensors::init_sensors() != ESP_OK) {
ret = SPEAKER_POST_CODE::SENSOR_FAIL;
} else {
if(Solenoids::init_all_solenoids() != ESP_OK) {
ret = SPEAKER_POST_CODE::SOLENOID_FAIL;
}
}
}
}
}
if (ret == SPEAKER_POST_CODE::INIT_OK) {
CurrentDriver::init_current_driver();
gearbox = new Gearbox();
if (!gearbox->start_controller()) {
return SPEAKER_POST_CODE::CONTROLLER_FAIL;
standard = new StandardProfile(VEHICLE_CONFIG.engine_type == 0);
comfort = new ComfortProfile(VEHICLE_CONFIG.engine_type == 0);
winter = new WinterProfile(VEHICLE_CONFIG.engine_type == 0);
agility = new AgilityProfile(VEHICLE_CONFIG.engine_type == 0);
manual = new ManualProfile(VEHICLE_CONFIG.engine_type == 0);
race = new RaceProfile(VEHICLE_CONFIG.engine_type == 0);
profiles[0] = standard;
profiles[1] = comfort;
profiles[2] = winter;
profiles[3] = agility;
profiles[4] = manual;
profiles[5] = race;
// Read profile ID on startup based on TCM config
profile_id = VEHICLE_CONFIG.default_profile;
if (profile_id > 4) {
profile_id = 0;
}
gearbox = new Gearbox();
if (gearbox->start_controller() != ESP_OK) {
ret = SPEAKER_POST_CODE::CONTROLLER_FAIL;
} else {
gearbox->set_profile(profiles[profile_id]);
}
}
gearbox->set_profile(profiles[profile_id]);
return SPEAKER_POST_CODE::INIT_OK;
return ret;
}
void err_beep_loop(void* a) {
SPEAKER_POST_CODE p = (SPEAKER_POST_CODE)(int)a;
if (p == SPEAKER_POST_CODE::INIT_OK) {
spkr.post(p); // All good, return
spkr->post(p); // All good, return
vTaskDelete(NULL);
} else {
// An error has occurred
// Set gearbox to F mode
egs_can_hal->set_drive_profile(GearboxProfile::Failure);
egs_can_hal->set_display_msg(GearboxMessage::VisitWorkshop);
egs_can_hal->set_gearbox_ok(false);
if (egs_can_hal != nullptr) {
// An error has occurred
// Set gearbox to F mode
egs_can_hal->set_drive_profile(GearboxProfile::Failure);
egs_can_hal->set_display_msg(GearboxMessage::VisitWorkshop);
egs_can_hal->set_gearbox_ok(false);
}
while(1) {
spkr.post(p);
spkr->post(p);
if (spkr2 != nullptr) {
spkr2->post(p);
}
vTaskDelay(2000/portTICK_PERIOD_MS);
}
vTaskDelete(NULL);
@ -123,7 +147,7 @@ void err_beep_loop(void* a) {
void input_manager(void*) {
bool pressed = false;
PaddlePosition last_pos = PaddlePosition::None;
ShifterPosition slast_pos = ShifterPosition::SignalNotAvaliable;
ShifterPosition slast_pos = ShifterPosition::SignalNotAvailable;
while(1) {
uint64_t now = esp_timer_get_time()/1000;
bool down = egs_can_hal->get_profile_btn_press(now, 100);
@ -183,6 +207,8 @@ const char* post_code_to_str(SPEAKER_POST_CODE s) {
return "SENSOR_INIT_FAIL";
case SPEAKER_POST_CODE::SOLENOID_FAIL:
return "SOLENOID_INIT_FAIL";
case SPEAKER_POST_CODE::EFUSE_NOT_SET:
return "EFUSE_CONFIG_NOT_SET";
default:
return nullptr;
}
@ -190,8 +216,15 @@ const char* post_code_to_str(SPEAKER_POST_CODE s) {
extern "C" void app_main(void)
{
// Set all pointers
gearbox = nullptr;
egs_can_hal = nullptr;
pressure_manager = nullptr;
SPEAKER_POST_CODE s = setup_tcm();
xTaskCreate(err_beep_loop, "PCSPKR", 2048, (void*)s, 2, nullptr);
xTaskCreate(err_beep_loop, "PCSPKR", 2048, reinterpret_cast<void*>(s), 2, nullptr);
// Now spin up the KWP2000 server (last thing)
diag_server = new Kwp2000_server(egs_can_hal, gearbox);
xTaskCreatePinnedToCore(Kwp2000_server::start_kwp_server, "KWP2000", 32*1024, diag_server, 5, nullptr, 0);
if (s != SPEAKER_POST_CODE::INIT_OK) {
while(true) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "INIT", "TCM INIT ERROR (%s)! CANNOT START TCM!", post_code_to_str(s));
@ -200,9 +233,6 @@ extern "C" void app_main(void)
} else { // INIT OK!
xTaskCreate(input_manager, "INPUT_MANAGER", 8192, nullptr, 5, nullptr);
}
// Now spin up the KWP2000 server (last thing)
diag_server = new Kwp2000_server(egs_can_hal, gearbox);
xTaskCreatePinnedToCore(Kwp2000_server::start_kwp_server, "KWP2000", 32*1024, diag_server, 5, nullptr, 0);
}
#endif // UNIT_TEST

246
src/maps.cpp
View File

@ -3,22 +3,21 @@
int16_t S_DIESEL_UPSHIFT_MAP[SHIFT_MAP_SIZE] = {
/* Pedal position */
/*0% 10% 20% 30% 40% 50% 60% 70% 80% 90% 100% */
1500, 1750, 2000, 2250, 2500, 2750, 3000, 3250, 3500, 4000, 4500,/* 1 -> 2 */
1500, 1550, 1600, 1650, 1700, 1750, 1800, 1850, 1900, 1950, 4500,/* 2 -> 3 */
1500, 1550, 1600, 1650, 1700, 1750, 1800, 1850, 1900, 1950, 4500,/* 3 -> 4 */
1500, 1550, 1600, 1650, 1700, 1750, 1800, 1850, 1900, 1950, 4500 /* 4 -> 5 */
1400, 1550, 1800, 2000, 2200, 2450, 2600, 2850, 3200, 4000, 4500,/* 1 -> 2 */
1400, 1550, 1700, 1850, 2000, 2200, 2350, 2600, 2900, 3550, 4500,/* 2 -> 3 */
1400, 1550, 1700, 1850, 1950, 2050, 2200, 2450, 2700, 3450, 4500,/* 3 -> 4 */
1500, 1550, 1700, 1800, 1950, 2050, 2200, 2450, 2650, 3400, 4500 /* 4 -> 5 */
};
int16_t S_DIESEL_DOWNSHIFT_MAP[SHIFT_MAP_SIZE] = {
/* Pedal position */
/*0% 10% 20% 30% 40% 50% 60% 70% 80% 90% 100% */
900, 1100, 1200, 1300, 1400, 1500, 1600, 1700, 1800, 1900, 2000,/* 2 -> 1 */
900, 1100, 1200, 1300, 1400, 1500, 1600, 1700, 1800, 1900, 2000,/* 3 -> 2 */
900, 1100, 1200, 1300, 1400, 1500, 1600, 1700, 1800, 1900, 2000,/* 4 -> 3 */
1000, 1100, 1300, 1500, 1700, 1900, 2100, 2400, 2600, 2800, 3000 /* 5 -> 4 */
};
100, 500, 850, 1050, 1200, 1400, 1500, 1700, 1800, 1900, 2000,/* 2 -> 1 */
150, 600, 900, 1100, 1250, 1400, 1500, 1700, 1800, 2000, 2000,/* 3 -> 2 */
700, 850, 1000, 1150, 1300, 1400, 1550, 1700, 1800, 2100, 2000,/* 4 -> 3 */
900, 1000, 1100, 1200, 1350, 1450, 1500, 1750, 1900, 2200, 2500 /* 5 -> 4 */
};
int16_t S_PETROL_UPSHIFT_MAP[SHIFT_MAP_SIZE] = {
/* Pedal position */
@ -48,19 +47,19 @@ int16_t S_PETROL_DOWNSHIFT_MAP[SHIFT_MAP_SIZE] = {
int16_t C_DIESEL_UPSHIFT_MAP[SHIFT_MAP_SIZE] = {
/* Pedal position */
/*0% 10% 20% 30% 40% 50% 60% 70% 80% 90% 100% */
1700, 1800, 2000, 2250, 2500, 2750, 3000, 3250, 3500, 4000, 4500,/* 1 -> 2 */
1300, 1300, 1400, 1500, 1700, 2000, 2300, 2450, 3200, 3750, 4500,/* 2 -> 3 */
1400, 1400, 1500, 1600, 1800, 2100, 2400, 2550, 3400, 3800, 4500,/* 3 -> 4 */
1500, 1550, 1600, 1700, 1900, 2200, 2500, 2550, 3800, 4000, 4500 /* 4 -> 5 */
1500, 1750, 1900, 2100, 2200, 2450, 2600, 3000, 3500, 4000, 4500,/* 1 -> 2 */
1500, 1500, 1400, 1500, 1700, 2000, 2300, 2450, 3200, 3750, 4500,/* 2 -> 3 */
1450, 1500, 1500, 1600, 1800, 2100, 2400, 2550, 3400, 3800, 4500,/* 3 -> 4 */
1450, 1500, 1600, 1650, 1900, 2200, 2500, 3000, 3700, 4200, 4500 /* 4 -> 5 */
};
int16_t C_DIESEL_DOWNSHIFT_MAP[SHIFT_MAP_SIZE] = {
/* Pedal position */
/*0% 10% 20% 30% 40% 50% 60% 70% 80% 90% 100% */
800, 900, 900, 900, 900, 900, 1000, 1200, 1300, 1400, 1500,/* 2 -> 1 */
800, 900, 900, 1200, 1200, 1300, 1400, 1500, 1600, 1700, 1800,/* 3 -> 2 */
900, 900, 900, 1200, 1200, 1300, 1400, 1500, 1600, 1700, 1800,/* 4 -> 3 */
900, 900, 900, 1200, 1200, 1300, 1400, 1500, 1600, 1700, 1800 /* 5 -> 4 */
100, 400, 600, 800, 900, 900, 1000, 1200, 1300, 1400, 1500,/* 2 -> 1 */
150, 500, 850, 1050, 1200, 1300, 1400, 1500, 1600, 1700, 1800,/* 3 -> 2 */
900, 900, 900, 1050, 1200, 1300, 1400, 1500, 1600, 1700, 1800,/* 4 -> 3 */
900, 900, 1000, 1100, 1200, 1300, 1400, 1500, 1600, 1700, 1800 /* 5 -> 4 */
};
int16_t C_PETROL_UPSHIFT_MAP[SHIFT_MAP_SIZE] = {
@ -90,19 +89,19 @@ int16_t C_PETROL_DOWNSHIFT_MAP[SHIFT_MAP_SIZE] = {
int16_t A_DIESEL_UPSHIFT_MAP[SHIFT_MAP_SIZE] = {
/* Pedal position */
/*0% 10% 20% 30% 40% 50% 60% 70% 80% 90% 100% */
1500, 1750, 2000, 2250, 2500, 2750, 3000, 3250, 3500, 4000, 4500,/* 1 -> 2 */
1500, 1550, 1600, 1650, 1700, 1750, 1800, 1850, 1900, 1950, 4500,/* 2 -> 3 */
1500, 1550, 1600, 1650, 1700, 1750, 1800, 1850, 1900, 1950, 4500,/* 3 -> 4 */
1500, 1550, 1600, 1650, 1700, 1750, 1800, 1850, 1900, 1950, 4500 /* 4 -> 5 */
1400, 1550, 1700, 2050, 2300, 2600, 2900, 3150, 3500, 4000, 4500,/* 1 -> 2 */
1400, 1550, 1750, 1950, 2150, 2400, 2650, 2850, 3250, 3850, 4500,/* 2 -> 3 */
1400, 1500, 1650, 1850, 2050, 2300, 2550, 2750, 3150, 3750, 4500,/* 3 -> 4 */
1450, 1550, 1650, 1850, 2050, 2250, 2500, 2700, 3100, 3700, 4500 /* 4 -> 5 */
};
int16_t A_DIESEL_DOWNSHIFT_MAP[SHIFT_MAP_SIZE] = {
/* Pedal position */
/*0% 10% 20% 30% 40% 50% 60% 70% 80% 90% 100% */
900, 1100, 1200, 1300, 1400, 1500, 1600, 1700, 1800, 1900, 2000,/* 2 -> 1 */
900, 1100, 1200, 1300, 1400, 1500, 1600, 1700, 1800, 1900, 2000,/* 3 -> 2 */
900, 1100, 1200, 1300, 1400, 1500, 1600, 1700, 1800, 1900, 2000,/* 4 -> 3 */
1000, 1100, 1300, 1500, 1700, 1900, 2100, 2400, 2600, 2800, 3000 /* 5 -> 4 */
100, 500, 900, 1100, 1200, 1400, 1600, 1700, 1800, 2000, 2300,/* 2 -> 1 */
150, 600, 1000, 1050, 1200, 1400, 1600, 1700, 1800, 2000, 2300,/* 3 -> 2 */
900, 900, 1000, 1100, 1200, 1300, 1500, 1600, 1700, 2000, 2300,/* 4 -> 3 */
900, 900, 1000, 1100, 1200, 1300, 1400, 1500, 1600, 1900, 2200 /* 5 -> 4 */
};
int16_t A_PETROL_UPSHIFT_MAP[SHIFT_MAP_SIZE] = {
@ -172,3 +171,198 @@ int16_t M_PETROL_DOWNSHIFT_MAP[SHIFT_MAP_SIZE] = {
500, 550, 600, 650, 700, 750, 800, 850, 900, 950, 1000,/* 4 -> 3 */
600, 650, 700, 750, 800, 850, 900, 950, 1000, 1050, 1100 /* 5 -> 4 */
};
int16_t SMALL_NAG_WORKING_MAP[WORKING_PRESSURE_MAP_SIZE] = {
/* Engine torque (0-100%) of 330Nm */
/*0%, 10% 20% 30% 40% 50% 60% 70% 80% 90% 100% */
500, 600, 700, 800, 900, 1000, 1100, 1200, 1300, 1400, 1500, // P/N
750, 800, 1000, 1250, 1500, 1750, 2000, 2250, 2500, 2750, 3000, // R1/R2
750, 1000, 1250, 1500, 1750, 2000, 2250, 2500, 2750, 3000, 3250, // 1
750, 1000, 1250, 1500, 1750, 2000, 2250, 2500, 2750, 3000, 3250, // 2
750, 1000, 1250, 1500, 1750, 2000, 2250, 2500, 2750, 3000, 3250, // 3
750, 1000, 1250, 1500, 1750, 2000, 2250, 2500, 2750, 3000, 3250, // 4
750, 1000, 1250, 1500, 1750, 2000, 2250, 2500, 2750, 3000, 3250, // 5
};
int16_t LARGE_NAG_WORKING_MAP[WORKING_PRESSURE_MAP_SIZE] = {
/* Engine torque (0-100%) of 580Nm */
/*0%, 10% 20% 30% 40% 50% 60% 70% 80% 90% 100% */
750, 1000, 1300, 1600, 1900, 2100, 2400, 2700, 3000, 3300, 3600, // P/N
750, 1000, 1300, 1600, 1900, 2100, 2400, 2700, 3000, 3300, 3600, // R1 or R2
900, 1100, 1200, 1500, 1800, 2100, 2400, 2700, 3000, 3300, 3600, // 1
900, 1100, 1200, 1500, 1800, 2300, 2600, 2900, 3200, 3500, 3800, // 2
900, 1100, 1200, 1500, 1800, 2300, 2600, 2900, 3200, 3500, 3800, // 3
900, 1100, 1200, 1500, 1800, 2300, 2600, 2900, 3200, 3500, 3800, // 4
900, 1100, 1200, 1500, 1800, 2300, 2600, 2900, 3200, 3500, 3800, // 5
};
int16_t SMALL_NAG_FILL_TIME_MAP[FILL_TIME_MAP_SIZE] = {
/* ATF TEMP */
/* -20C, 5C, 25C, 60C */
600, 300, 180, 160, // K1 clutch
1620, 600, 240, 140, // K2 clutch
860, 440, 180, 160, // K3 clutch
600, 380, 220, 180, // B1 brake
820, 400, 180, 120 // B2 brake
};
int16_t LARGE_NAG_FILL_TIME_MAP[FILL_TIME_MAP_SIZE] = {
/* ATF TEMP */
/* -20C, 5C, 25C, 60C */
600, 300, 180, 160, // K1 clutch
1620, 600, 240, 140, // K2 clutch
860, 440, 180, 160, // K3 clutch
600, 380, 220, 180, // B1 brake
820, 400, 180, 120 // B2 brake
};
int16_t TCC_PWM_MAP[TCC_PWM_MAP_SIZE] = { // values are in /4096
/* TCC PRESSURE (mBar) */
/* 0 2000 4000 5000 7500 10000 15000 */
0, 480, 960, 1280, 1920, 2560, 4096, // 0C
0, 560, 1040, 1280, 1920, 2560, 4096, // 30C
0, 640, 1120, 1280, 1920, 2560, 4096, // 60C
0, 640, 1120, 1280, 1920, 2560, 4096, // 90C
0, 640, 1120, 1280, 1920, 2560, 4096, // 120C
};
int16_t BROWN_PCS_CURRENT_MAP[PCS_CURRENT_MAP_SIZE] = { // values are in mA
/* mBar */
/* 0 50 600 1000 2350 5600 6600 7700 */
1300, 1100, 1085, 954, 700, 450, 350, 200, // -25C
1277, 1077, 925, 830, 675, 415, 320, 0, // 20C
1200, 1000, 835, 780, 650, 400, 288, 0, // 60C
1175, 975, 795, 745, 625, 370, 260, 0 // 150C
};
int16_t BLUE_PCS_CURRENT_MAP[PCS_CURRENT_MAP_SIZE] = {
/* 0 50 600 1000 2350 5600 6600 7700 <- mBar */
/* -25C */ 1300, 1100, 1085, 954, 700, 450, 350, 200,
/* 20C */ 1277, 1077, 925, 830, 675, 415, 320, 0,
/* 60C */ 1200, 1000, 835, 780, 650, 400, 288, 0,
/* 150C */ 1175, 975, 795, 745, 625, 370, 260, 0
};
int16_t SMALL_NAG_FILL_PRESSURE_MAP[FILL_PRESSURE_MAP_SIZE] = {
/* Clutch */
/* K1 K2 K3 B1 B2 */
1200, 1400, 1300, 1200, 1400
};
int16_t LARGE_NAG_FILL_PRESSURE_MAP[FILL_PRESSURE_MAP_SIZE] = {
/* Clutch */
/* K1 K2 K3 B1 B2 */
1200, 1400, 1300, 1200, 1400
};
int16_t M_UPSHIFT_TIME_MAP[] = { // Value = Target time in ms to shift (overlap duration)
/* 0 20 40 60 80 100 <- Pedal % */
/* < 1000 RPM (0%) */ 2000, 1500, 1000, 750, 500, 500,
/* 25% Redline */ 1000, 900, 800, 600, 475, 450,
/* 50% Redline */ 700, 650, 600, 500, 450, 400,
/* 75% Redline */ 500, 475, 450, 425, 400, 375,
/* Redline (100%) */ 450, 425, 400, 375, 350, 350
};
int16_t M_DOWNSHIFT_TIME_MAP[] = { // Value = Target time in ms to shift (overlap duration)
/* 0 20 40 60 80 100 <- Pedal % */
/* < 1000 RPM (0%) */ 2000, 1750, 1500, 1000, 750, 500,
/* 25% Redline */ 1750, 1500, 1200, 800, 650, 450,
/* 50% Redline */ 1500, 1250, 900, 600, 600, 400,
/* 75% Redline */ 1000, 1000, 600, 550, 550, 350,
/* Redline (100%) */ 800, 750, 500, 500, 400, 300
};
int16_t S_UPSHIFT_TIME_MAP[] = { // Value = Target time in ms to shift (overlap duration)
/* 0 20 40 60 80 100 <- Pedal % */
/* < 1000 RPM (0%) */ 2000, 1500, 1000, 750, 500, 500,
/* 25% Redline */ 1000, 900, 800, 600, 475, 450,
/* 50% Redline */ 700, 650, 600, 500, 450, 400,
/* 75% Redline */ 500, 475, 450, 425, 400, 375,
/* Redline (100%) */ 450, 425, 400, 375, 350, 350
};
int16_t S_DOWNSHIFT_TIME_MAP[] = { // Value = Target time in ms to shift (overlap duration)
/* 0 20 40 60 80 100 <- Pedal % */
/* < 1000 RPM (0%) */ 2000, 1750, 1500, 1000, 750, 500,
/* 25% Redline */ 1750, 1500, 1200, 800, 650, 450,
/* 50% Redline */ 1500, 1250, 900, 600, 600, 400,
/* 75% Redline */ 1000, 1000, 600, 550, 550, 350,
/* Redline (100%) */ 800, 750, 500, 500, 400, 300
};
int16_t A_UPSHIFT_TIME_MAP[] = { // Value = Target time in ms to shift (overlap duration)
/* 0 20 40 60 80 100 <- Pedal % */
/* < 1000 RPM (0%) */ 2000, 1500, 1000, 750, 500, 500,
/* 25% Redline */ 1000, 900, 800, 600, 475, 450,
/* 50% Redline */ 700, 650, 600, 500, 450, 400,
/* 75% Redline */ 500, 475, 450, 425, 400, 375,
/* Redline (100%) */ 450, 425, 400, 375, 350, 350
};
int16_t A_DOWNSHIFT_TIME_MAP[] = { // Value = Target time in ms to shift (overlap duration)
/* 0 20 40 60 80 100 <- Pedal % */
/* < 1000 RPM (0%) */ 2000, 1750, 1500, 1000, 750, 500,
/* 25% Redline */ 1750, 1500, 1200, 800, 650, 450,
/* 50% Redline */ 1500, 1250, 900, 600, 600, 400,
/* 75% Redline */ 1000, 1000, 600, 550, 550, 350,
/* Redline (100%) */ 800, 750, 500, 500, 400, 300
};
int16_t C_UPSHIFT_TIME_MAP[] = { // Value = Target time in ms to shift (overlap duration)
/* 0 20 40 60 80 100 <- Pedal % */
/* < 1000 RPM (0%) */ 2000, 1750, 1500, 1250, 1000, 750,
/* 25% Redline */ 1000, 900, 805, 800, 750, 700,
/* 50% Redline */ 900, 850, 800, 750, 700, 650,
/* 75% Redline */ 800, 750, 700, 650, 625, 600,
/* Redline (100%) */ 700, 675, 650, 625, 600, 550
};
int16_t C_DOWNSHIFT_TIME_MAP[] = { // Value = Target time in ms to shift (overlap duration)
/* 0 20 40 60 80 100 <- Pedal % */
/* < 1000 RPM (0%) */ 2000, 1750, 1500, 1250, 1000, 800,
/* 25% Redline */ 1800, 1600, 1400, 1250, 950, 750,
/* 50% Redline */ 1600, 1500, 1400, 1200, 900, 700,
/* 75% Redline */ 1400, 1300, 1200, 1100, 800, 650,
/* Redline (100%) */ 1200, 1000, 900, 800, 700, 600
};
int16_t W_UPSHIFT_TIME_MAP[] = { // Value = Target time in ms to shift (overlap duration)
/* 0 20 40 60 80 100 <- Pedal % */
/* < 1000 RPM (0%) */ 2000, 1750, 1500, 1250, 1000, 750,
/* 25% Redline */ 1000, 900, 805, 800, 750, 700,
/* 50% Redline */ 900, 850, 800, 750, 700, 650,
/* 75% Redline */ 800, 750, 700, 650, 625, 600,
/* Redline (100%) */ 700, 675, 650, 625, 600, 550
};
int16_t W_DOWNSHIFT_TIME_MAP[] = { // Value = Target time in ms to shift (overlap duration)
/* 0 20 40 60 80 100 <- Pedal % */
/* < 1000 RPM (0%) */ 2000, 1750, 1500, 1250, 1000, 800,
/* 25% Redline */ 1800, 1600, 1400, 1250, 950, 750,
/* 50% Redline */ 1600, 1500, 1400, 1200, 900, 700,
/* 75% Redline */ 1400, 1300, 1200, 1100, 800, 650,
/* Redline (100%) */ 1200, 1000, 900, 800, 700, 600
};
int16_t R_UPSHIFT_TIME_MAP[] = { // Value = Target time in ms to shift (overlap duration)
/* 0 20 40 60 80 100 <- Pedal % */
/* < 1000 RPM (0%) */ 100, 100, 100, 100, 100, 100,
/* 25% Redline */ 100, 100, 100, 100, 100, 100,
/* 50% Redline */ 100, 100, 100, 100, 100, 100,
/* 75% Redline */ 100, 100, 100, 100, 100, 100,
/* Redline (100%) */ 100, 100, 100, 100, 100, 100
};
int16_t R_DOWNSHIFT_TIME_MAP[] = { // Value = Target time in ms to shift (overlap duration)
/* 0 20 40 60 80 100 <- Pedal % */
/* < 1000 RPM (0%) */ 100, 100, 100, 100, 100, 100,
/* 25% Redline */ 100, 100, 100, 100, 100, 100,
/* 50% Redline */ 100, 100, 100, 100, 100, 100,
/* 75% Redline */ 100, 100, 100, 100, 100, 100,
/* Redline (100%) */ 100, 100, 100, 100, 100, 100
};

90
src/maps.h
View File

@ -1,6 +1,5 @@
#ifndef __MAP_H_
#define __MAP_H_
#ifndef MAP_H_
#define MAP_H_
/*
This file contains default map data for all profiles
@ -13,7 +12,6 @@
*/
#include <stdint.h>
#define SHIFT_MAP_SIZE 44
/** All values for the following maps are in input shaft RPM **/
@ -99,4 +97,88 @@ extern int16_t M_PETROL_UPSHIFT_MAP[SHIFT_MAP_SIZE];
#define MAP_NAME_M_PETROL_DOWNSHIFT "M_PETROL_DOWN"
extern int16_t M_PETROL_DOWNSHIFT_MAP[SHIFT_MAP_SIZE];
/*
================================================================================
PRESSURE MAPS
================================================================================
*/
// -- MPC working pressure --
#define WORKING_PRESSURE_MAP_SIZE 11*7 // 11 positions, 7 gears (P/N, R1/R2, 1, 2, 3, 4, 5)
#define MAP_NAME_WORKING_SMALL "WORK_SMALL"
extern int16_t SMALL_NAG_WORKING_MAP[WORKING_PRESSURE_MAP_SIZE];
#define MAP_NAME_WORKING_LARGE "WORK_LARGE"
extern int16_t LARGE_NAG_WORKING_MAP[WORKING_PRESSURE_MAP_SIZE];
// -- FILL TIME --
#define FILL_TIME_MAP_SIZE 5*4 // 4 temp positions, 5 clutch groups
#define MAP_NAME_FILL_TIME_SMALL "FILL_T_SMALL"
extern int16_t SMALL_NAG_FILL_TIME_MAP[FILL_TIME_MAP_SIZE];
#define MAP_NAME_FILL_TIME_LARGE "FILL_T_LARGE"
extern int16_t LARGE_NAG_FILL_TIME_MAP[FILL_TIME_MAP_SIZE];
// -- PWM maps --
#define TCC_PWM_MAP_SIZE 7*5 // 5 temp positions, 7 pressure readings
#define MAP_NAME_TCC_PWM "TCC_PWM"
extern int16_t TCC_PWM_MAP[TCC_PWM_MAP_SIZE];
#define PCS_CURRENT_MAP_SIZE 8*4
#define MAP_NAME_PCS_BROWN "PCS_BROWN"
extern int16_t BROWN_PCS_CURRENT_MAP[PCS_CURRENT_MAP_SIZE];
#define MAP_NAME_PCS_BLUE "PCS_BLUE"
extern int16_t BLUE_PCS_CURRENT_MAP[PCS_CURRENT_MAP_SIZE];
// -- Hold phase pressures --
#define FILL_PRESSURE_MAP_SIZE 5 // 5 clutches
#define MAP_NAME_FILL_PRESSURE_LARGE "FILL_PRESS_L"
extern int16_t LARGE_NAG_FILL_PRESSURE_MAP[FILL_PRESSURE_MAP_SIZE];
#define MAP_NAME_FILL_PRESSURE_SMALL "FILL_PRESS_S"
extern int16_t SMALL_NAG_FILL_PRESSURE_MAP[FILL_PRESSURE_MAP_SIZE];
// -- Target Shift time maps --
#define SHIFT_TIME_MAP_SIZE 30
#define MAP_NAME_M_UPSHIFT_TIME "M_UPSHIFT_TIME"
extern int16_t M_UPSHIFT_TIME_MAP[SHIFT_TIME_MAP_SIZE];
#define MAP_NAME_M_DOWNSHIFT_TIME "M_DNSHIFT_TIME"
extern int16_t M_DOWNSHIFT_TIME_MAP[SHIFT_TIME_MAP_SIZE];
#define MAP_NAME_C_UPSHIFT_TIME "C_UPSHIFT_TIME"
extern int16_t C_UPSHIFT_TIME_MAP[SHIFT_TIME_MAP_SIZE];
#define MAP_NAME_C_DOWNSHIFT_TIME "C_DNSHIFT_TIME"
extern int16_t C_DOWNSHIFT_TIME_MAP[SHIFT_TIME_MAP_SIZE];
#define MAP_NAME_S_UPSHIFT_TIME "S_UPSHIFT_TIME"
extern int16_t S_UPSHIFT_TIME_MAP[SHIFT_TIME_MAP_SIZE];
#define MAP_NAME_S_DOWNSHIFT_TIME "S_DNSHIFT_TIME"
extern int16_t S_DOWNSHIFT_TIME_MAP[SHIFT_TIME_MAP_SIZE];
#define MAP_NAME_A_UPSHIFT_TIME "A_UPSHIFT_TIME"
extern int16_t A_UPSHIFT_TIME_MAP[SHIFT_TIME_MAP_SIZE];
#define MAP_NAME_A_DOWNSHIFT_TIME "A_DNSHIFT_TIME"
extern int16_t A_DOWNSHIFT_TIME_MAP[SHIFT_TIME_MAP_SIZE];
#define MAP_NAME_W_UPSHIFT_TIME "W_UPSHIFT_TIME"
extern int16_t W_UPSHIFT_TIME_MAP[SHIFT_TIME_MAP_SIZE];
#define MAP_NAME_W_DOWNSHIFT_TIME "W_DNSHIFT_TIME"
extern int16_t W_DOWNSHIFT_TIME_MAP[SHIFT_TIME_MAP_SIZE];
#define MAP_NAME_R_UPSHIFT_TIME "R_UPSHIFT_TIME"
extern int16_t R_UPSHIFT_TIME_MAP[SHIFT_TIME_MAP_SIZE];
#define MAP_NAME_R_DOWNSHIFT_TIME "R_DNSHIFT_TIME"
extern int16_t R_DOWNSHIFT_TIME_MAP[SHIFT_TIME_MAP_SIZE];
#endif

256
src/nvs/eeprom_config.cpp
View File

@ -1,172 +1,131 @@
#include "eeprom_config.h"
#include "esp_log.h"
#include "scn.h"
#include "speaker.h"
#include <string.h>
#include "profiles.h"
#include "efuse/efuse.h"
#include "esp_efuse.h"
#include "esp_check.h"
/// If default is UINT16_MAX, it is ignored
bool read_nvs_u16(nvs_handle_t handle, const char* key, uint16_t* ptr, uint16_t default_value) {
esp_err_t e = nvs_get_u16(handle, key, ptr);
if (e == ESP_ERR_NVS_NOT_FOUND) {
if (default_value == UINT16_MAX) {
ESP_LOG_LEVEL(ESP_LOG_WARN, "EEPROM", "Value for %s not found, no defaults specified. CANNOT CONTINUE!", key);
return false;
} else {
// Try to init the default value
ESP_LOG_LEVEL(ESP_LOG_WARN, "EEPROM", "Value for %s not found, initializing to default of %u", key, default_value);
e = nvs_set_u16(handle, key, default_value);
if (e != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "Error setting default value for %s (%s)", key, esp_err_to_name(e));
return false;
}
e = nvs_commit(handle);
if (e != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "Error calling nvs_commit: %s", esp_err_to_name(e));
return false;
}
// OK
*ptr = default_value;
return true;
}
}
return (e == ESP_OK);
}
bool EEPROM::read_nvs_map_data(const char* map_name, int16_t* dest, const int16_t* default_map, size_t map_element_count) {
esp_err_t EEPROM::read_nvs_map_data(const char* map_name, int16_t* dest, const int16_t* default_map, size_t map_element_count) {
size_t byte_count = map_element_count*sizeof(int16_t);
esp_err_t e = nvs_get_blob(MAP_NVS_HANDLE, map_name, dest, &byte_count);
if (e == ESP_ERR_NVS_NOT_FOUND && default_map != nullptr) {
ESP_LOG_LEVEL(ESP_LOG_WARN, "EEPROM", "Map %s not found in NVS. Setting to default map from prog flash", map_name);
// Set default map data
if (write_nvs_map_data(map_name, default_map, map_element_count)) {
ESP_LOG_LEVEL(ESP_LOG_INFO, "EEPROM", "Map %s copied OK to NVS!", map_name);
} else {
ESP_LOG_LEVEL(ESP_LOG_WARN, "EEPROM", "Map %s not copied to NVS! Using flash default map", map_name);
}
e = write_nvs_map_data(map_name, default_map, map_element_count);
}
if(e != ESP_OK) {
if (default_map != nullptr) {
memcpy(dest, default_map, byte_count);
e = ESP_OK;
} else {
return false;
e = ESP_ERR_INVALID_ARG;
}
} else {
ESP_LOG_LEVEL(ESP_LOG_INFO, "EEPROM", "Map %s loaded OK from NVS!", map_name);
}
return true;
return e;
}
bool EEPROM::write_nvs_map_data(const char* map_name, const int16_t* to_write, size_t map_element_count) {
esp_err_t EEPROM::write_nvs_map_data(const char* map_name, const int16_t* to_write, size_t map_element_count) {
esp_err_t e = nvs_set_blob(MAP_NVS_HANDLE, map_name, to_write, map_element_count*sizeof(int16_t));
if (e != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "Error setting value for %s (%s)", map_name, esp_err_to_name(e));
return false;
}
e = nvs_commit(MAP_NVS_HANDLE);
if (e != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "Error calling nvs_commit: %s", esp_err_to_name(e));
return false;
}
return true;
}
bool read_nvs_gear_adaptation(nvs_handle_t handle, const char* key, pressure_map* map, size_t store_size) {
esp_err_t e = nvs_get_blob(handle, key, map, &store_size);
if (e == ESP_ERR_NVS_NOT_FOUND) {
ESP_LOG_LEVEL(ESP_LOG_WARN, "EEPROM", "Adaptation %s map not found. Creating a new one", key);
pressure_map new_map = {0,0,0,0,0,0,0,0,0,0,0};
e = nvs_set_blob(handle, key, &new_map, sizeof(new_map));
if (e != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "Error initializing default adaptation map map data (%s)", esp_err_to_name(e));
return false;
}
e = nvs_commit(handle);
} else {
e = nvs_commit(MAP_NVS_HANDLE);
if (e != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "Error calling nvs_commit: %s", esp_err_to_name(e));
return false;
}
ESP_LOG_LEVEL(ESP_LOG_INFO, "EEPROM", "New TCC map creation OK!");
memcpy(map, new_map, sizeof(new_map));
return true;
}
return (e == ESP_OK);
return e;
}
bool EEPROM::init_eeprom() {
// bool read_nvs_gear_adaptation(nvs_handle_t handle, const char* key, pressure_map* map, size_t store_size) {
// esp_err_t e = nvs_get_blob(handle, key, map, &store_size);
// if (e == ESP_ERR_NVS_NOT_FOUND) {
// ESP_LOG_LEVEL(ESP_LOG_WARN, "EEPROM", "Adaptation %s map not found. Creating a new one", key);
// pressure_map new_map = {0,0,0,0,0,0,0,0,0,0,0};
// e = nvs_set_blob(handle, key, &new_map, sizeof(new_map));
// if (e != ESP_OK) {
// ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "Error initializing default adaptation map map data (%s)", esp_err_to_name(e));
// return false;
// }
// e = nvs_commit(handle);
// if (e != ESP_OK) {
// ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "Error calling nvs_commit: %s", esp_err_to_name(e));
// return false;
// }
// ESP_LOG_LEVEL(ESP_LOG_INFO, "EEPROM", "New TCC map creation OK!");
// memcpy(map, new_map, sizeof(new_map));
// return true;
// }
// return (e == ESP_OK);
// }
esp_err_t EEPROM::init_eeprom() {
// Called on startup
esp_err_t err = nvs_flash_init();
if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "EEPROM init failed! %s", esp_err_to_name(err));
// Don't erase flash. User has to do this manually. Just POST beep!
return false;
esp_err_t result = nvs_flash_init();
if (result == ESP_ERR_NVS_NO_FREE_PAGES || result == ESP_ERR_NVS_NEW_VERSION_FOUND) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "EEPROM init failed! %s", esp_err_to_name(result));
} else {
// Flash init OK
nvs_handle_t config_handle;
result = nvs_open(NVS_PARTITION_USER_CFG, NVS_READWRITE, &config_handle);
if (result != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "EEPROM NVS handle failed! %s", esp_err_to_name(result));
}
MAP_NVS_HANDLE = config_handle;
result = read_core_config(&VEHICLE_CONFIG);
}
nvs_handle_t config_handle;
err = nvs_open(NVS_PARTITION_USER_CFG, NVS_READWRITE, &config_handle);
if (err != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "EEPROM NVS handle failed! %s", esp_err_to_name(err));
return false;
}
MAP_NVS_HANDLE = config_handle;
bool res = read_core_config(&VEHICLE_CONFIG);
if (!res) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "EEPROM SCN config read failed! %s", esp_err_to_name(err));
return false;
}
return true;
return result;
}
bool EEPROM::read_core_config(TCM_CORE_CONFIG* dest) {
esp_err_t EEPROM::read_core_config(TCM_CORE_CONFIG* dest) {
nvs_handle_t handle;
nvs_open(NVS_PARTITION_USER_CFG, NVS_READWRITE, &handle); // Must succeed as we have already opened it!
size_t s = sizeof(TCM_CORE_CONFIG);
esp_err_t e = nvs_get_blob(handle, NVS_KEY_SCN_CONFIG, dest, &s);
if (e == ESP_ERR_NVS_NOT_FOUND) {
esp_err_t result = nvs_get_blob(handle, NVS_KEY_SCN_CONFIG, dest, &s);
if (result == ESP_ERR_NVS_NOT_FOUND) {
ESP_LOG_LEVEL(ESP_LOG_WARN, "EEPROM", "SCN Config not found. Creating a new one");
TCM_CORE_CONFIG s = {
#ifdef LARGE_NAG
.is_large_nag = 1,
#else
TCM_CORE_CONFIG c = {
.is_large_nag = 0,
#endif
.diff_ratio = DIFF_RATIO,
.wheel_circumference = TYRE_SIZE_MM,
#ifdef FOUR_MATIC
.is_four_matic = 1,
.transfer_case_high_ratio = TC_RATIO_HIGH,
.transfer_case_low_ratio = TC_RATIO_LOW,
#else
.diff_ratio = 1000,
.wheel_circumference = 2850,
.is_four_matic = 0,
.transfer_case_high_ratio = 1000,
.transfer_case_low_ratio = 1000,
#endif
.default_profile = 0, // Standard
.red_line_rpm_diesel = 4500, // Safe for diesels, petrol-heads can change this!
.red_line_rpm_petrol = 6000,
.engine_type = 0 // Diesel by default - TODO We should read this via CAN
.engine_type = 0,
.egs_can_type = 0,
.shifter_style = 0,
.io_0_usage = 0,
.input_sensor_pulses_per_rev = 1,
.output_pulse_width_per_kmh = 1,
.gen_mosfet_purpose = 0,
};
e = nvs_set_blob(handle, NVS_KEY_SCN_CONFIG, &s, sizeof(s));
if (e != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "Error initializing default SCN config (%s)", esp_err_to_name(e));
return false;
result = nvs_set_blob(handle, NVS_KEY_SCN_CONFIG, &c, sizeof(c));
if (result != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "Error initializing default SCN config (%s)", esp_err_to_name(result));
} else {
// set blob OK
result = nvs_commit(handle);
if (result != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "Error calling nvs_commit: %s", esp_err_to_name(result));
} else {
ESP_LOG_LEVEL(ESP_LOG_INFO, "EEPROM", "New SCN creation OK!");
memcpy(dest, &s, sizeof(s));
result = ESP_OK;
}
}
e = nvs_commit(handle);
if (e != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "Error calling nvs_commit: %s", esp_err_to_name(e));
return false;
}
ESP_LOG_LEVEL(ESP_LOG_INFO, "EEPROM", "New SCN creation OK!");
memcpy(dest, &s, sizeof(s));
return true;
} else if (e != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "Could not read SCN config: %s", esp_err_to_name(e));
}
return (e == ESP_OK);
return result;
}
bool EEPROM::save_core_config(TCM_CORE_CONFIG* write) {
esp_err_t EEPROM::save_core_config(TCM_CORE_CONFIG* write) {
nvs_handle_t handle;
esp_err_t e;
size_t s = sizeof(TCM_CORE_CONFIG);
@ -174,15 +133,66 @@ bool EEPROM::save_core_config(TCM_CORE_CONFIG* write) {
e = nvs_set_blob(handle, NVS_KEY_SCN_CONFIG, write, s);
if (e != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "Error Saving SCN config (%s)", esp_err_to_name(e));
return false;
} else {
e = nvs_commit(handle);
if (e != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "Error calling nvs_commit: %s", esp_err_to_name(e));
}
}
e = nvs_commit(handle);
if (e != ESP_OK) {
ESP_LOG_LEVEL(ESP_LOG_ERROR, "EEPROM", "Error calling nvs_commit: %s", esp_err_to_name(e));
return false;
return e;
}
esp_err_t EEPROM::read_efuse_config(TCM_EFUSE_CONFIG* dest) {
if (dest == nullptr) {
return ESP_ERR_INVALID_ARG;
}
return true;
ESP_RETURN_ON_ERROR(esp_efuse_read_field_blob(ESP_EFUSE_BOARD_VER, &dest->board_ver, 8), "EFUSE_CFG", "Could not read board ver");
ESP_RETURN_ON_ERROR(esp_efuse_read_field_blob(ESP_EFUSE_M_DAY, &dest->manufacture_day, 8), "EFUSE_CFG", "Could not read manf. day");
ESP_RETURN_ON_ERROR(esp_efuse_read_field_blob(ESP_EFUSE_M_WEEK, &dest->manufacture_week, 8), "EFUSE_CFG", "Could not read manf. week");
ESP_RETURN_ON_ERROR(esp_efuse_read_field_blob(ESP_EFUSE_M_MONTH, &dest->manufacture_month, 8), "EFUSE_CFG", "Could not read manf. month");
ESP_RETURN_ON_ERROR(esp_efuse_read_field_blob(ESP_EFUSE_M_YEAR, &dest->manufacture_year, 8), "EFUSE_CFG", "Could not read manf. year");
ESP_LOG_LEVEL(ESP_LOG_INFO, "EFUSE_CFG", "CONFIG:Board Ver: V1.%d, Week %d (%02d/%02d/%02d)", dest->board_ver, dest->manufacture_week, dest->manufacture_day, dest->manufacture_month, dest->manufacture_year);
return ESP_OK;
}
esp_err_t EEPROM::write_efuse_config(TCM_EFUSE_CONFIG* dest) {
if (dest == nullptr) {
return ESP_ERR_INVALID_ARG;
}
if (dest->manufacture_month > 12 || dest->manufacture_month == 0) {
return ESP_ERR_INVALID_ARG;
}
if (dest->manufacture_day > 31 || dest->manufacture_day == 0) {
return ESP_ERR_INVALID_ARG;
}
if (dest->manufacture_week > 52 || dest->manufacture_week == 0) {
return ESP_ERR_INVALID_ARG;
}
if (dest->manufacture_year < 22) {
return ESP_ERR_INVALID_ARG;
}
if (dest->board_ver == 0 || dest->board_ver > 3) {
return ESP_ERR_INVALID_ARG;
}
if (esp_efuse_write_field_blob(ESP_EFUSE_BOARD_VER, &dest->board_ver, 8) != ESP_OK) {
return ESP_ERR_INVALID_ARG;
}
if (esp_efuse_write_field_blob(ESP_EFUSE_M_DAY, &dest->manufacture_day, 8) != ESP_OK) {
return ESP_ERR_INVALID_ARG;
}
if (esp_efuse_write_field_blob(ESP_EFUSE_M_WEEK, &dest->manufacture_week, 8) != ESP_OK) {
return ESP_ERR_INVALID_ARG;
}
if (esp_efuse_write_field_blob(ESP_EFUSE_M_MONTH, &dest->manufacture_month, 8) != ESP_OK) {
return ESP_ERR_INVALID_ARG;
}
if (esp_efuse_write_field_blob(ESP_EFUSE_M_YEAR, &dest->manufacture_year, 8) != ESP_OK) {
return ESP_ERR_INVALID_ARG;
}
return ESP_OK;
}
TCM_CORE_CONFIG VEHICLE_CONFIG = {};
TCM_EFUSE_CONFIG BOARD_CONFIG = {};
nvs_handle_t MAP_NVS_HANDLE = {};

66
src/nvs/eeprom_config.h
View File

@ -1,24 +1,28 @@
/** @file */
#ifndef __EEPROM_CONFIG_H_
#define __EEPROM_CONFIG_H_
#ifndef EEPROM_CONFIG_H
#define EEPROM_CONFIG_H
#include "esp_err.h"
#include "esp_log.h"
#include "common_structs.h"
#include "nvs.h"
#include "nvs_flash.h"
#include "esp_err.h"
#define NVS_KEY_EEPROM_INIT "EEPROM_INIT"
// #define NVS_KEY_EEPROM_INIT "EEPROM_INIT"
// Core SCN config (Needed for a ton of important calculations!)
#define NVS_KEY_SCN_CONFIG "CORE_SCN"
static const char NVS_KEY_SCN_CONFIG[9] = "CORE_SCN";
static const char NVS_PARTITION_USER_CFG[16] = "tcm_user_config";
static const char NVS_UCFG_KEY_PROFILE[13] = "LAST_PROFILE";
static const char NVS_KEY_GEAR_ADAPTATION[16] = "GEAR_ADAPTATION";
#define NVS_PARTITION_USER_CFG "tcm_user_config"
#define NVS_UCFG_KEY_PROFILE "LAST_PROFILE"
#define NVS_KEY_GEAR_ADAPTATION "GEAR_ADAPTATION"
#define SHIFTER_STYLE_EWM 0
#define SHIFTER_STYLE_TRRS 1
#define SHIFTER_STYLE_SLR 2
typedef struct {
struct __attribute__ ((packed)) TCM_CORE_CONFIG{
uint8_t is_large_nag;
uint16_t diff_ratio;
uint16_t wheel_circumference;
@ -29,21 +33,51 @@ typedef struct {
uint16_t red_line_rpm_diesel;
uint16_t red_line_rpm_petrol;
uint8_t engine_type; // 0 for diesel, 1 for petrol
} __attribute__ ((packed)) TCM_CORE_CONFIG;
// 0 - EWM
// 1 - TRRS
// 2 - SLR
uint8_t egs_can_type;
uint8_t shifter_style;
uint8_t io_0_usage;
uint8_t input_sensor_pulses_per_rev;
uint8_t output_pulse_width_per_kmh;
uint8_t gen_mosfet_purpose;
};
// -- EFuse layout --
// Key Block start bit length bit
// PRODUCT.BOARD_VER, EFUSE_BLK3, 0, 8
// PRODUCT.M_DAY, EFUSE_BLK3, 8, 8
// PRODUCT.M_WEEK, EFUSE_BLK3, 16, 8
// PRODUCT.M_MONTH, EFUSE_BLK3, 24, 8
// PRODUCT.M_YEAR, EFUSE_BLK3, 32, 8
struct __attribute__ ((packed)) TCM_EFUSE_CONFIG {
uint8_t board_ver; // 1 - Red PCB, 2 - Black PCB, 3 - Black PCB with GPIO (WIP)
uint8_t manufacture_day;
uint8_t manufacture_week;
uint8_t manufacture_month;
uint8_t manufacture_year;
};
namespace EEPROM {
bool init_eeprom();
uint8_t get_last_profile();
bool read_core_config(TCM_CORE_CONFIG* dest);
bool save_core_config(TCM_CORE_CONFIG* write);
esp_err_t init_eeprom(void);
uint8_t get_last_profile(void);
esp_err_t read_core_config(TCM_CORE_CONFIG* dest);
esp_err_t save_core_config(TCM_CORE_CONFIG* write);
esp_err_t read_efuse_config(TCM_EFUSE_CONFIG* dest);
esp_err_t write_efuse_config(TCM_EFUSE_CONFIG* dest);
bool read_nvs_map_data(const char* map_name, int16_t* dest, const int16_t* default_map, size_t map_element_count);
bool write_nvs_map_data(const char* map_name, const int16_t* to_write, size_t map_element_count);
esp_err_t read_nvs_map_data(const char* map_name, int16_t* dest, const int16_t* default_map, size_t map_element_count);
esp_err_t write_nvs_map_data(const char* map_name, const int16_t* to_write, size_t map_element_count);
};
#define NUM_GEARS 5
extern TCM_CORE_CONFIG VEHICLE_CONFIG;
extern TCM_EFUSE_CONFIG BOARD_CONFIG;
extern nvs_handle_t MAP_NVS_HANDLE;
#endif

4
src/nvs/scn_eeprom.cpp
View File

@ -1,4 +0,0 @@
#include "scn_eeprom.h"
ScnConfigClass::ScnConfigClass() {
}

14
src/nvs/scn_eeprom.h
View File

@ -1,14 +0,0 @@
#ifndef __SCN_EEPROM_H_
#define __SCN_EEPROM_H_
#include "nvs_flash.h"
#define SCN_ID "SCN_ID"
class ScnConfigClass {
public:
ScnConfigClass();
};
#endif

359
src/pressure_manager.cpp
View File

@ -1,127 +1,98 @@
#include "pressure_manager.h"
#include <tcm_maths.h>
#include "macros.h"
#include <tcu_maths.h>
#include "solenoids/constant_current.h"
#include "macros.h"
inline uint16_t locate_pressure_map_value(const pressure_map map, int percent) {
if (percent <= 0) { return map[0]; }
else if (percent >= 100) { return map[10]; }
else {
int min = percent/10;
int max = min+1;
float dy = map[max] - map[min];
float dx = (max-min)*10;
return (map[min] + ((dy/dx)) * (percent-(min*10)));
}
}
#include "maps.h"
// inline uint16_t locate_pressure_map_value(const pressure_map map, int percent) {
// if (percent <= 0) { return map[0]; }
// else if (percent >= 100) { return map[10]; }
// else {
// int min = percent/10;
// int max = min+1;
// float dy = map[max] - map[min];
// float dx = (max-min)*10;
// return (map[min] + ((dy/dx)) * (percent-(min*10)));
// }
// }
PressureManager::PressureManager(SensorData* sensor_ptr, uint16_t max_torque) {
this->sensor_data = sensor_ptr;
this->adapt_map = new AdaptationMap();
this->req_tcc_pressure = 0;
this->req_mpc_pressure = 0;
this->req_mpc_pressure = 0;
this->req_spc_pressure = 0;
this->req_current_mpc = 0;
this->req_current_spc = 0;
this->gb_max_torque = max_torque;
// For loading maps
const char* key_name;
const int16_t* default_data;
/** Pressure PWM map **/
int16_t pwm_x_headers[8] = {0, 50, 600, 1000, 2350, 5600, 6600, 7700};
int16_t pwm_y_headers[4] = {-25, 20, 60, 150};
pressure_pwm_map = new TcmMap(8, 4, pwm_x_headers, pwm_y_headers);
if (!this->pressure_pwm_map->allocate_ok()) {
this->pressure_pwm_map = nullptr;
ESP_LOG_LEVEL(ESP_LOG_ERROR, "PM", "Allocation of pressure pwm map failed!");
return;
const int16_t pwm_x_headers[8] = {0, 50, 600, 1000, 2350, 5600, 6600, 7700};
const int16_t pwm_y_headers[4] = {-25, 20, 60, 150};
key_name = MAP_NAME_PCS_BROWN;
default_data = BROWN_PCS_CURRENT_MAP;
this->pressure_pwm_map = new StoredTcuMap(key_name, PCS_CURRENT_MAP_SIZE, pwm_x_headers, pwm_y_headers, 8, 4, default_data);
if (this->pressure_pwm_map->init_status() != ESP_OK) {
delete[] this->pressure_pwm_map;
}
// Allocation OK, add the data to the map
// Values are current (mA) for SPC and MPC solenoid
int16_t pcs_current_map[8*4] = {
/* 0 50 600 1000 2350 5600 6600 7700 <- mBar */
/* -25C */ 1300, 1100, 1085, 954, 700, 450, 350, 200,
/* 20C */ 1277, 1077, 925, 830, 675, 415, 320, 0,
/* 60C */ 1200, 1000, 835, 780, 650, 400, 288, 0,
/* 150C */ 1175, 975, 795, 745, 625, 370, 260, 0
};
pressure_pwm_map->add_data(pcs_current_map, 8*4);
/** Pressure PWM map (TCC) **/
int16_t pwm_tcc_x_headers[7] = {0, 2000, 4000, 5000, 7500, 10000, 15000};
int16_t pwm_tcc_y_headers[5] = {-25, 20, 60, 150};
tcc_pwm_map = new TcmMap(7, 5, pwm_tcc_x_headers, pwm_tcc_y_headers);
if (!this->tcc_pwm_map->allocate_ok()) {
this->tcc_pwm_map = nullptr;
ESP_LOG_LEVEL(ESP_LOG_ERROR, "PM", "Allocation of TCC pressure pwm map failed!");
return;
const int16_t pwm_tcc_x_headers[7] = {0, 2000, 4000, 5000, 7500, 10000, 15000};
const int16_t pwm_tcc_y_headers[5] = {0, 30, 60, 90, 120};
key_name = MAP_NAME_TCC_PWM;
default_data = TCC_PWM_MAP;
tcc_pwm_map = new StoredTcuMap(key_name, TCC_PWM_MAP_SIZE, pwm_tcc_x_headers, pwm_tcc_y_headers, 7, 5, default_data);
if (this->tcc_pwm_map->init_status() != ESP_OK) {
delete[] this->tcc_pwm_map;
}
// Allocation OK, add the data to the map
// Values are PWM 12bit (0-1000) for TCC solenoid
int16_t tc_pwm_map[7*5] = {
/* 0 2000 4000 5000 7500 10000 15000 <- mBar */
/* 0C */ 0, 480, 960, 1280, 1920, 2560, 4096,
/* 30C */ 0, 560, 1040, 1280, 1920, 2560, 4096,
/* 60C */ 0, 640, 1120, 1280, 1920, 2560, 4096,
/* 90C */ 0, 640, 1120, 1280, 1920, 2560, 4096,
/* 120C */ 0, 640, 1120, 1280, 1920, 2560, 4096,
};
tcc_pwm_map->add_data(tc_pwm_map, 7*5);
/** Pressure Hold 2 time map **/
int16_t hold2_x_headers[5] = {1, 2, 3, 4, 5};
int16_t hold2_y_headers[4] = {-20, 5, 25, 60};
hold2_time_map = new TcmMap(5, 4, hold2_x_headers, hold2_y_headers);
if (!this->hold2_time_map->allocate_ok()) {
this->hold2_time_map = nullptr;
ESP_LOG_LEVEL(ESP_LOG_ERROR, "PM", "Allocation of pressure pwm map failed!");
return;
const int16_t hold2_x_headers[4] = {-20, 5, 25, 60};
const int16_t hold2_y_headers[5] = {1,2,3,4,5};
if (VEHICLE_CONFIG.is_large_nag) { // Large
key_name = MAP_NAME_FILL_TIME_LARGE;
default_data = LARGE_NAG_FILL_TIME_MAP;
} else { // Small
key_name = MAP_NAME_FILL_TIME_SMALL;
default_data = SMALL_NAG_FILL_TIME_MAP;
}
hold2_time_map = new StoredTcuMap(key_name, FILL_TIME_MAP_SIZE, hold2_x_headers, hold2_y_headers, 4, 5, default_data);
if (this->hold2_time_map->init_status() != ESP_OK) {
delete[] this->hold2_time_map;
}
// Allocation OK, add the data to the map
// Values are in millsecond for SPC to hold at Hold2 phase
// Derived from this table
/* K1 K2 K3 B1 B2 */
/* -20C 600, 1620, 860, 600, 820, */
/* 5C 300, 600, 440, 380, 400, */
/* 25C 180, 240, 160, 220, 180, */
/* 60C 160, 140, 140, 180, 120 */
/**
* 1 - K3, B1, B2
* 2 - K1, K3, B2
* 3 - K1, K2, B2
* 4 - K1. K2, K3
* 5 - K2, K3, B1
*
*
* 1-2 prefill K1 release B1
* 2-1 prefill B1 release K1
*
* 2-3 prefill K2 release K3
* 3-2 prefill K3 release K2
*
* 3-4 prefill K3 release B2
* 4-3 prefill B2 release K3
*
* 4-5 prefill B1 release K1
* 5-4 prefill K1 release B1
*/
int16_t hold2_map[5*4] = {
/* Clutch grp K1 K2 K3 B1 B2 */
/* -20C */ 600, 1620, 860, 600, 820,
/* 5C */ 300, 600, 440, 380, 400,
/* 25C */ 180, 240, 180, 220, 180,
/* 60C */ 160, 140, 160, 180, 120
};
hold2_time_map->add_data(hold2_map, 5*4);
this->gb_max_torque = max_torque;
/** Pressure Hold 2 pressure map **/
const int16_t hold2p_x_headers[1] = {1};
const int16_t hold2p_y_headers[5] = {1,2,3,4,5};
if (VEHICLE_CONFIG.is_large_nag) { // Large
key_name = MAP_NAME_FILL_PRESSURE_LARGE;
default_data = LARGE_NAG_FILL_PRESSURE_MAP;
} else { // Small
key_name = MAP_NAME_FILL_PRESSURE_SMALL;
default_data = SMALL_NAG_FILL_PRESSURE_MAP;
}
hold2_pressure_map = new StoredTcuMap(key_name, FILL_PRESSURE_MAP_SIZE, hold2p_x_headers, hold2p_y_headers, 1, 5, default_data);
if (this->hold2_pressure_map->init_status() != ESP_OK) {
delete[] this->hold2_pressure_map;
}
/** Working pressure map **/
const int16_t wp_x_headers[11] = {0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100};
const int16_t wp_y_headers[7] = {0, 1, 2, 3, 4, 5, 6};
if (VEHICLE_CONFIG.is_large_nag) { // Large
key_name = MAP_NAME_WORKING_LARGE;
default_data = LARGE_NAG_WORKING_MAP;
} else { // Small
key_name = MAP_NAME_WORKING_SMALL;
default_data = SMALL_NAG_WORKING_MAP;
}
this->mpc_working_pressure = new StoredTcuMap(key_name, WORKING_PRESSURE_MAP_SIZE, wp_x_headers, wp_y_headers, 11, 7, default_data);
if (this->mpc_working_pressure->init_status() != ESP_OK) {
delete[] this->mpc_working_pressure;
}
}
Clutch PressureManager::get_clutch_to_apply(ProfileGearChange change) {
@ -162,90 +133,82 @@ Clutch PressureManager::get_clutch_to_release(ProfileGearChange change) {
}
}
uint16_t PressureManager::find_working_mpc_pressure(GearboxGear curr_g, int max_rated_torque) {
const int16_t* targ_map = working_norm_pressure_p_small;
uint16_t PressureManager::find_working_mpc_pressure(GearboxGear curr_g) {
if (this->mpc_working_pressure == nullptr) {
return 7000; // Failsafe!
}
uint8_t gear_idx = 0;
switch(curr_g) {
case GearboxGear::First:
targ_map = VEHICLE_CONFIG.is_large_nag ? working_norm_pressure_1_large : working_norm_pressure_1_small;
gear_idx = 2;
break;
case GearboxGear::Second:
targ_map = VEHICLE_CONFIG.is_large_nag ? working_norm_pressure_2_large : working_norm_pressure_2_small;
gear_idx = 3;
break;
case GearboxGear::Third:
targ_map = VEHICLE_CONFIG.is_large_nag ? working_norm_pressure_3_large : working_norm_pressure_3_small;
gear_idx = 4;
break;
case GearboxGear::Fourth:
targ_map = VEHICLE_CONFIG.is_large_nag ? working_norm_pressure_4_large : working_norm_pressure_4_small;
gear_idx = 5;
break;
case GearboxGear::Fifth:
targ_map = VEHICLE_CONFIG.is_large_nag ? working_norm_pressure_5_large : working_norm_pressure_5_small;
gear_idx = 6;
break;
case GearboxGear::Reverse_First:
case GearboxGear::Reverse_Second:
targ_map = VEHICLE_CONFIG.is_large_nag ? working_norm_pressure_r_large : working_norm_pressure_r_small;
gear_idx = 1;
break;
case GearboxGear::Park:
case GearboxGear::Neutral:
case GearboxGear::SignalNotAvaliable:
case GearboxGear::SignalNotAvailable:
default: // Already set
targ_map = VEHICLE_CONFIG.is_large_nag ? working_norm_pressure_p_large : working_norm_pressure_p_small;
gear_idx = 0;
break;
}
//int pedal_pos = ((int)sensor_data->pedal_pos*100/250);
int trq_percent = (sensor_data->static_torque*100)/max_rated_torque;
return locate_pressure_map_value(targ_map, trq_percent);
float trq_percent = (float)(MAX(sensor_data->driver_requested_torque, sensor_data->static_torque)*100)/(float)this->gb_max_torque;
return this->mpc_working_pressure->get_value(trq_percent, gear_idx);
}
float PressureManager::get_tcc_temp_multiplier(int atf_temp) {
if (atf_temp < 40) {
return 1.0;
} else if (atf_temp < 60) {
return 1.2;
} else if (atf_temp < 80) {
return 1.4;
} else if (atf_temp < 100) {
return 1.6;
} else {
return 1.8;
}
return (float)scale_number(sensor_data->atf_temp, 100, 200, 40, 100) / 100.0;
}
ShiftData PressureManager::get_shift_data(ProfileGearChange shift_request, ShiftCharacteristics chars, uint16_t curr_mpc) {
ShiftData sd;
uint8_t map_gear = 1;
CLAMP(chars.shift_speed, 1, 10);
ShiftData sd;
switch (shift_request) {
case ProfileGearChange::ONE_TWO:
sd.targ_g = 2; sd.curr_g = 1; map_gear = 1;
sd.targ_g = 2; sd.curr_g = 1;
sd.shift_solenoid = sol_y3;
break;
case ProfileGearChange::TWO_THREE:
sd.targ_g = 3; sd.curr_g = 2; map_gear = 2;
sd.targ_g = 3; sd.curr_g = 2;
sd.shift_solenoid = sol_y5;
break;
case ProfileGearChange::THREE_FOUR:
sd.targ_g = 4; sd.curr_g = 3; map_gear = 3;
sd.targ_g = 4; sd.curr_g = 3;
sd.shift_solenoid = sol_y4;
break;
case ProfileGearChange::FOUR_FIVE:
sd.targ_g = 5; sd.curr_g = 4; map_gear = 4;
sd.targ_g = 5; sd.curr_g = 4;
sd.shift_solenoid = sol_y3;
break;
case ProfileGearChange::FIVE_FOUR:
sd.targ_g = 4; sd.curr_g = 5; map_gear = 5;
sd.targ_g = 4; sd.curr_g = 5;
sd.shift_solenoid = sol_y3;
break;
case ProfileGearChange::FOUR_THREE:
sd.targ_g = 3; sd.curr_g = 4; map_gear = 6;
sd.targ_g = 3; sd.curr_g = 4;
sd.shift_solenoid = sol_y4;
break;
case ProfileGearChange::THREE_TWO:
sd.targ_g = 2; sd.curr_g = 3; map_gear = 7;
sd.targ_g = 2; sd.curr_g = 3;
sd.shift_solenoid = sol_y5;
break;
case ProfileGearChange::TWO_ONE:
sd.targ_g = 1; sd.curr_g = 2; map_gear = 8;
sd.targ_g = 1; sd.curr_g = 2;
sd.shift_solenoid = sol_y3;
break;
}
@ -256,94 +219,60 @@ ShiftData PressureManager::get_shift_data(ProfileGearChange shift_request, Shift
sd.bleed_data.spc_pressure = 650;
sd.bleed_data.mpc_pressure = curr_mpc;
// Make fill phase data (AKA Hold 3)
// Make fill phase data
this->make_fill_data(&sd.fill_data, chars, shift_request, curr_mpc);
// Recalculated at the time of the gear change
this->make_torque_data(&sd.torque_data, &sd.fill_data, chars, shift_request, curr_mpc);
this->make_overlap_data(&sd.overlap_data, &sd.torque_data, chars, shift_request, curr_mpc);
this->make_torque_and_overlap_data(&sd.overlap_data, &sd.torque_data, &sd.overlap_data, chars, shift_request, curr_mpc);
this->make_max_p_data(&sd.max_pressure_data, &sd.overlap_data, chars, shift_request, curr_mpc);
float profile_td_amount = ((float)scale_number(chars.shift_speed, 10, 1, 1, 10) / 10.0);
//float profile_td_amount = ((float)scale_number(chars.shift_speed, 10, 1, 1, 10) / 10.0);
// <= 500 RPM - 0% profile_td_amount
// >= 4500 RPM - 100% profile_td_amount
//float rpm_td_amount = ((float)scale_number(sensor_data->input_rpm, 10, 0, 500, 4500) / 10.0);
sd.torque_down_amount = profile_td_amount;
sd.torque_down_amount = 0;
sd.bleed_data.mpc_pressure = sd.fill_data.mpc_pressure;
return sd;
}
uint16_t get_clutch_fill_pressure(Clutch c) {
switch(c) {
case Clutch::K1:
return 1400;
case Clutch::K2:
return 1400;
case Clutch::K3:
return 1300;
case Clutch::B1:
return 1200;
case Clutch::B2:
default:
return 1400;
}
}
void PressureManager::make_fill_data(ShiftPhase* dest, ShiftCharacteristics chars, ProfileGearChange change, uint16_t curr_mpc) {
if (this->hold2_time_map == nullptr) {
dest->hold_time = 500;
dest->spc_pressure = 1500;
dest->mpc_pressure = curr_mpc;
} else {
Clutch to_change = get_clutch_to_apply(change);
Clutch to_release = get_clutch_to_release(change);
dest->hold_time = hold2_time_map->get_value(this->sensor_data->atf_temp, (uint8_t)to_change);
dest->spc_pressure = hold2_pressure_map->get_value(1, (uint8_t)to_change);
dest->mpc_pressure = MAX(hold2_pressure_map->get_value(1, (uint8_t)to_release), curr_mpc);
}
dest->ramp_time = 100;
switch (get_clutch_to_apply(change)) {
case Clutch::K1:
dest->hold_time = hold2_time_map->get_value(1, this->sensor_data->atf_temp);
dest->spc_pressure = 1200;
break;
case Clutch::K2:
dest->hold_time = hold2_time_map->get_value(2, this->sensor_data->atf_temp);
dest->spc_pressure = 1400;
break;
case Clutch::K3:
dest->hold_time = hold2_time_map->get_value(3, this->sensor_data->atf_temp);
dest->spc_pressure = 1300;
break;
case Clutch::B1:
dest->hold_time = hold2_time_map->get_value(4, this->sensor_data->atf_temp);
dest->spc_pressure = 1200;
break;
case Clutch::B2:
default:
dest->hold_time = hold2_time_map->get_value(5, this->sensor_data->atf_temp);
dest->spc_pressure = 1400;
break;
}
const AdaptationCell* cell = this->adapt_map->get_adapt_cell(sensor_data, change, this->gb_max_torque);
dest->hold_time += cell->fill_time_adder;
dest->hold_time -= dest->ramp_time;
dest->mpc_pressure = curr_mpc + scale_number(abs(sensor_data->static_torque), 200, get_clutch_fill_pressure(get_clutch_to_release(change)), 0, gb_max_torque);
if (dest->mpc_pressure < dest->spc_pressure+200) {
dest->mpc_pressure = dest->spc_pressure+200;
}
//const AdaptationCell* cell = this->adapt_map->get_adapt_cell(sensor_data, change, this->gb_max_torque);
}
void PressureManager::make_torque_data(ShiftPhase* dest, ShiftPhase* prev, ShiftCharacteristics chars, ProfileGearChange change, uint16_t curr_mpc) {
dest->hold_time = dest->ramp_time = scale_number(abs(sensor_data->static_torque), 150, 50, gb_max_torque/3, gb_max_torque);
dest->spc_pressure = (prev->spc_pressure+prev->mpc_pressure)/2;
dest->mpc_pressure = prev->mpc_pressure;
}
void PressureManager::make_overlap_data(ShiftPhase* dest, ShiftPhase* prev, ShiftCharacteristics chars, ProfileGearChange change, uint16_t curr_mpc) {
void PressureManager::make_torque_and_overlap_data(ShiftPhase* dest_torque, ShiftPhase* dest_overlap, ShiftPhase* prev, ShiftCharacteristics chars, ProfileGearChange change, uint16_t curr_mpc) {
// Maybe we tweak this?
float torque_ratio = 0.25; // 1.4 shift time in torque phase, 3/4 shift time in overlap phase
float overlap_ratio = 1.0-torque_ratio;
dest->ramp_time = dest->hold_time = scale_number(abs(sensor_data->static_torque), 300, 150, gb_max_torque/3, gb_max_torque);
//dest->hold_time = 0;
if (change == ProfileGearChange::ONE_TWO) {
dest->ramp_time += scale_number(abs(sensor_data->static_torque), 20, 0, gb_max_torque/3, (gb_max_torque/3)*2);
dest_torque->hold_time = 0;
dest_overlap->hold_time = (float)chars.target_shift_time*overlap_ratio*0.5;
dest_torque->ramp_time = (float)chars.target_shift_time*torque_ratio;
dest_overlap->ramp_time = dest_overlap->hold_time/4;
dest_torque->mpc_pressure = prev->mpc_pressure; // Torque MPC stays same
dest_overlap->mpc_pressure = prev->mpc_pressure;
uint16_t spc_addr = MAX(100, MAX(sensor_data->driver_requested_torque, abs(sensor_data->static_torque))*2.5); // 2mBar per Nm
dest_overlap->mpc_pressure += spc_addr/2;
if (sensor_data->static_torque < 0) {
spc_addr += 250; // For coast shifts
}
dest->ramp_time *= (float)scale_number(chars.shift_speed*10, 1200, 800, 0, 100)/1000.0;
dest->hold_time = 0;
dest->ramp_time -= prev->hold_time; // Offset by torque data
uint16_t spc_addr = scale_number(abs(sensor_data->static_torque), 10, 500, 0, gb_max_torque);
spc_addr *= (float)scale_number(chars.shift_speed*10, 1000, 1500, 0, 100)/1000.0;
dest->spc_pressure = prev->spc_pressure+spc_addr;
dest->mpc_pressure = get_clutch_fill_pressure(get_clutch_to_release(change));
dest_torque->spc_pressure = MAX(prev->mpc_pressure, prev->spc_pressure); // Same as MPC (Begin torque transfer)
dest_overlap->spc_pressure = dest_torque->spc_pressure + spc_addr; // SPC lock into place clutch for overlap phase
}
void PressureManager::make_max_p_data(ShiftPhase* dest, ShiftPhase* prev, ShiftCharacteristics chars, ProfileGearChange change, uint16_t curr_mpc) {
@ -390,7 +319,7 @@ void PressureManager::set_target_spc_pressure(uint16_t targ) {
}
void PressureManager::disable_spc() {
this->req_spc_pressure = 7000;
this->req_spc_pressure = 0;
this->req_current_spc = 0;
egs_can_hal->set_spc_pressure(7000);
spc_cc->set_target_current(0);
@ -406,4 +335,12 @@ uint16_t PressureManager::get_targ_mpc_pressure(){ return this->req_mpc_pressure
uint16_t PressureManager::get_targ_spc_pressure(){ return this->req_spc_pressure; }
uint16_t PressureManager::get_targ_tcc_pressure(){ return this->req_tcc_pressure; }
uint16_t PressureManager::get_targ_spc_current(){ return this->req_current_spc; }
uint16_t PressureManager::get_targ_mpc_current(){ return this->req_current_mpc; }
uint16_t PressureManager::get_targ_mpc_current(){ return this->req_current_mpc; }
StoredTcuMap* PressureManager::get_pcs_map() { return this->pressure_pwm_map; }
StoredTcuMap* PressureManager::get_tcc_pwm_map() { return this->tcc_pwm_map; }
StoredTcuMap* PressureManager::get_working_map() { return this->mpc_working_pressure; }
StoredTcuMap* PressureManager::get_fill_time_map() { return this->hold2_time_map; }
StoredTcuMap* PressureManager::get_fill_pressure_map() { return this->hold2_pressure_map; }
PressureManager* pressure_manager = nullptr;

107
src/pressure_manager.h
View File

@ -1,49 +1,38 @@
#ifndef __PRESSURE_MANAGER_H_
#define __PRESSURE_MANAGER_H_
#ifndef PRESSURE_MANAGER_H
#define PRESSURE_MANAGER_H
#include <common_structs.h>
#include "tcm_maths.h"
#include "tcu_maths.h"
#include "profiles.h"
#include "adaptation/adapt_map.h"
#include <gearbox_config.h>
#include "nvs/eeprom_config.h"
#include "stored_map.h"
// Shift phase IDs
#define SHIFT_PHASE_BLEED 1
#define SHIFT_PHASE_FILL 2
#define SHIFT_PHASE_TORQUE 3
#define SHIFT_PHASE_OVERLAP 4
#define SHIFT_PHASE_MAX_P 5
static const uint16_t SHIFT_PHASE_BLEED = 1u;
static const uint16_t SHIFT_PHASE_FILL = 2u;
static const uint16_t SHIFT_PHASE_TORQUE = 3u;
static const uint16_t SHIFT_PHASE_OVERLAP = 4u;
static const uint16_t SHIFT_PHASE_MAX_P = 5u;
enum class Clutch {
K1,
K2,
K3,
B1,
B2,
B3 // Reverse ONLY
enum ShiftPhases : uint8_t{
BLEED = 1u,
FILL = 2u,
TORQUE = 3u,
OVERLAP = 4u,
MAX_P = 5u
};
// 0 -> 100% rated torque (In mbar) (0Nm - 330Nm)
const pressure_map working_norm_pressure_p_small = { 500, 600, 700, 800, 900, 1000, 1100, 1200, 1300, 1400, 1500}; // Working pressure table Park or N
const pressure_map working_norm_pressure_r_small = { 750, 800, 1000, 1250, 1500, 1750, 2000, 2250, 2500, 2750, 3000}; // Working pressure table R1 or R2
const pressure_map working_norm_pressure_1_small = { 750, 1000, 1250, 1500, 1750, 2000, 2250, 2500, 2750, 3000, 3250}; // Working pressure table 1
const pressure_map working_norm_pressure_2_small = { 750, 1000, 1250, 1500, 1750, 2000, 2250, 2500, 2750, 3000, 3250}; // Working pressure table 2
const pressure_map working_norm_pressure_3_small = { 750, 1000, 1250, 1500, 1750, 2000, 2250, 2500, 2750, 3000, 3250}; // Working pressure table 3
const pressure_map working_norm_pressure_4_small = { 750, 1000, 1250, 1500, 1750, 2000, 2250, 2500, 2750, 3000, 3250}; // Working pressure table 4
const pressure_map working_norm_pressure_5_small = { 750, 1000, 1250, 1500, 1750, 2000, 2250, 2500, 2750, 3000, 3250}; // Working pressure table 5
// 0 -> 100% rated torque (In mbar) (0Nm - 580Nm)
const pressure_map working_norm_pressure_p_large = { 750, 1000, 1300, 1600, 1900, 2100, 2400, 2700, 3000, 3300, 3600}; // Working pressure table R1 or R2
const pressure_map working_norm_pressure_r_large = { 750, 1000, 1300, 1600, 1900, 2100, 2400, 2700, 3000, 3300, 3600}; // Working pressure table R1 or R2
const pressure_map working_norm_pressure_1_large = { 900, 1100, 1200, 1500, 1800, 2100, 2400, 2700, 3000, 3300, 3600}; // Working pressure table 1
const pressure_map working_norm_pressure_2_large = { 900, 1100, 1200, 1500, 1800, 2100, 2400, 2700, 3000, 3300, 3600}; // Working pressure table 2
const pressure_map working_norm_pressure_3_large = { 900, 1100, 1200, 1500, 1800, 2300, 2600, 2900, 3200, 3500, 3800}; // Working pressure table 3
const pressure_map working_norm_pressure_4_large = { 900, 1100, 1200, 1500, 1800, 2300, 2600, 2900, 3200, 3500, 3800}; // Working pressure table 4
const pressure_map working_norm_pressure_5_large = { 900, 1100, 1200, 1500, 1800, 2300, 2600, 2900, 3200, 3500, 3800}; // Working pressure table 5
typedef void (*P_RAMP_FUNC)(float, float);
enum class Clutch {
K1 = 1,
K2 = 2,
K3 = 3,
B1 = 4,
B2 = 5,
B3 = 6 // Reverse ONLY
};
class PressureManager {
@ -69,12 +58,12 @@ public:
* @param targ Target TCC pressure to achieve in mBar
*/
void set_target_tcc_pressure(uint16_t targ);
uint16_t get_targ_mpc_pressure();
uint16_t get_targ_spc_pressure();
uint16_t get_targ_tcc_pressure();
uint16_t get_targ_spc_current();
uint16_t get_targ_mpc_current();
void disable_spc();
uint16_t get_targ_mpc_pressure(void);
uint16_t get_targ_spc_pressure(void);
uint16_t get_targ_tcc_pressure(void);
uint16_t get_targ_spc_current(void);
uint16_t get_targ_mpc_current(void);
void disable_spc(void);
PressureManager(SensorData* sensor_ptr, uint16_t max_torque);
@ -94,13 +83,13 @@ public:
* @return true if adaptation reset was OK
* @return false if adaptation reset failed
*/
bool diag_reset_adaptation() {
esp_err_t diag_reset_adaptation(void) {
bool result = false;
if (this->adapt_map != nullptr) {
this->adapt_map->reset();
return true;
} else {
return false;
result = true;
}
return result;
}
/**
@ -121,20 +110,29 @@ public:
* @brief Save adaptation data to NVS EEPROM
*
*/
void save() {
void save(void) {
if (this->adapt_map != nullptr) {
this->adapt_map->save();
}
}
uint16_t find_working_mpc_pressure(GearboxGear curr_g, int max_rated_torque);
uint16_t find_working_mpc_pressure(GearboxGear curr_g);
float get_tcc_temp_multiplier(int atf_temp);
void make_fill_data(ShiftPhase* dest, ShiftCharacteristics chars, ProfileGearChange change, uint16_t curr_mpc);
void make_torque_data(ShiftPhase* dest, ShiftPhase* prev, ShiftCharacteristics chars, ProfileGearChange change, uint16_t curr_mpc);
void make_overlap_data(ShiftPhase* dest, ShiftPhase* prev, ShiftCharacteristics chars, ProfileGearChange change, uint16_t curr_mpc);
void make_torque_and_overlap_data(ShiftPhase* dest_torque, ShiftPhase* dest_overlap, ShiftPhase* prev, ShiftCharacteristics chars, ProfileGearChange change, uint16_t curr_mpc);
void make_max_p_data(ShiftPhase* dest, ShiftPhase* prev, ShiftCharacteristics chars, ProfileGearChange change, uint16_t curr_mpc);
StoredTcuMap* get_pcs_map(void);
StoredTcuMap* get_tcc_pwm_map(void);
StoredTcuMap* get_working_map(void);
StoredTcuMap* get_fill_time_map(void);
StoredTcuMap* get_fill_pressure_map(void);
uint16_t get_max_rated_torque() {
return this->gb_max_torque;
}
private:
/**
* Returns the estimated PWM to send to either SPC or MPC solenoid
@ -156,15 +154,16 @@ private:
uint16_t req_mpc_pressure;
uint16_t req_current_spc;
uint16_t req_current_mpc;
TcmMap* pressure_pwm_map;
TcmMap* tcc_pwm_map;
TcmMap* hold2_time_map;
StoredTcuMap* pressure_pwm_map;
StoredTcuMap* tcc_pwm_map;
StoredTcuMap* mpc_working_pressure;
StoredTcuMap* hold2_time_map;
StoredTcuMap* hold2_pressure_map;
uint16_t gb_max_torque;
Clutch get_clutch_to_release(ProfileGearChange change);
Clutch get_clutch_to_apply(ProfileGearChange change);
};
extern PressureManager* pressure_manager;
#endif

242
src/profiles.cpp
View File

@ -1,7 +1,7 @@
#include "profiles.h"
#include <gearbox_config.h>
#include "adv_opts.h"
#include "tcm_maths.h"
#include "tcu_maths.h"
#include "gearbox.h"
#include "maps.h"
@ -13,66 +13,81 @@ AbstractProfile::AbstractProfile(bool is_diesel,
const char* downshift_map_name_diesel,
const char* upshift_map_name_petrol,
const char* downshift_map_name_petrol,
const char* upshift_time_map_name,
const char* downshift_time_map_name,
const int16_t* def_upshift_data_diesel,
const int16_t* def_downshift_data_diesel,
const int16_t* def_upshift_data_petrol,
const int16_t* def_downshift_data_petrol
const int16_t* def_downshift_data_petrol,
const int16_t* def_upshift_time_data,
const int16_t* def_downshift_time_data
) {
this->is_diesel = is_diesel;
this->tag_id = tag_id;
this->upshift_map_name_diesel = upshift_map_name_diesel;
this->downshift_map_name_diesel = downshift_map_name_diesel;
this->upshift_map_name_petrol = upshift_map_name_petrol;
this->downshift_map_name_petrol = downshift_map_name_petrol;
this->def_upshift_data_diesel = def_upshift_data_diesel;
this->def_downshift_data_diesel = def_downshift_data_diesel;
this->def_upshift_data_petrol = def_upshift_data_petrol;
this->def_downshift_data_petrol = def_downshift_data_petrol;
this->upshift_table = new TcmMap(SHIFT_MAP_X_SIZE, SHIFT_MAP_Y_SIZE, shift_table_x_header, upshift_y_headers);
this->downshift_table = new TcmMap(SHIFT_MAP_X_SIZE, SHIFT_MAP_Y_SIZE, shift_table_x_header, downshift_y_headers);
int16_t up_map[SHIFT_MAP_SIZE];
int16_t down_map[SHIFT_MAP_SIZE];
if (!this->upshift_table->allocate_ok() || !this->downshift_table->allocate_ok()) {
ESP_LOGE(tag_id, "Upshift/Downshift map allocation failed!");
delete this->upshift_table;
delete this->downshift_table;
const char* key_name;
const int16_t* default_map;
/** Upshift map **/
if (is_diesel) {
key_name = upshift_map_name_diesel;
default_map = def_upshift_data_diesel;
} else {
if (is_diesel) {
EEPROM::read_nvs_map_data(upshift_map_name_diesel, up_map, def_upshift_data_diesel, SHIFT_MAP_SIZE);
EEPROM::read_nvs_map_data(downshift_map_name_diesel, down_map, def_downshift_data_diesel, SHIFT_MAP_SIZE);
} else {
EEPROM::read_nvs_map_data(upshift_map_name_petrol, up_map, def_upshift_data_petrol, SHIFT_MAP_SIZE);
EEPROM::read_nvs_map_data(downshift_map_name_petrol, down_map, def_downshift_data_petrol, SHIFT_MAP_SIZE);
}
if (this->upshift_table->add_data(up_map, SHIFT_MAP_SIZE) && this->downshift_table->add_data(down_map, SHIFT_MAP_SIZE)) {
ESP_LOGI(tag_id, "Upshift and downshift maps loaded OK!");
} else {
ESP_LOGE(tag_id, "Upshift/Downshift map data add failed!");
delete this->upshift_table;
delete this->downshift_table;
}
key_name = upshift_map_name_petrol;
default_map = def_upshift_data_petrol;
}
this->upshift_table = new StoredTcuMap(key_name, SHIFT_MAP_SIZE, shift_table_x_header, upshift_y_headers, SHIFT_MAP_X_SIZE, SHIFT_MAP_Y_SIZE, default_map);
if (this->upshift_table->init_status() != ESP_OK) {
delete[] this->upshift_table;
}
/** Downshift map **/
if (is_diesel) {
key_name = downshift_map_name_diesel;
default_map = def_downshift_data_diesel;
} else {
key_name = downshift_map_name_petrol;
default_map = def_downshift_data_petrol;
}
this->downshift_table = new StoredTcuMap(key_name, SHIFT_MAP_SIZE, shift_table_x_header, upshift_y_headers, SHIFT_MAP_X_SIZE, SHIFT_MAP_Y_SIZE, default_map);
if (this->downshift_table->init_status() != ESP_OK) {
delete[] this->downshift_table;
}
// Up/downshift time tables
int16_t redline = is_diesel ? VEHICLE_CONFIG.red_line_rpm_diesel : VEHICLE_CONFIG.red_line_rpm_petrol;
int16_t step_size = (redline-1000) / 4;
int16_t shift_rpm_points[5] = {(int16_t)1000, (int16_t)(1000+(step_size)), (int16_t)(1000+(step_size*2)), (int16_t)(1000+(step_size*3)), redline};
this->upshift_time_map = new StoredTcuMap(upshift_time_map_name, SHIFT_TIME_MAP_SIZE, shift_time_table_x_header, const_cast<int16_t*>(shift_rpm_points), 6, 5, def_upshift_time_data);
if (this->upshift_time_map->init_status() != ESP_OK) {
delete[] this->upshift_time_map;
}
this->downshift_time_map = new StoredTcuMap(downshift_time_map_name, SHIFT_TIME_MAP_SIZE, shift_time_table_x_header, const_cast<int16_t*>(shift_rpm_points), 6, 5, def_downshift_time_data);
if (this->downshift_time_map->init_status() != ESP_OK) {
delete[] this->downshift_time_map;
}
}
void AbstractProfile::reload_data() {
if (this->upshift_table == nullptr || this->downshift_table == nullptr) {
return;
}
int16_t up_map[SHIFT_MAP_SIZE];
int16_t down_map[SHIFT_MAP_SIZE];
if (is_diesel) {
EEPROM::read_nvs_map_data(upshift_map_name_diesel, up_map, def_upshift_data_diesel, SHIFT_MAP_SIZE);
EEPROM::read_nvs_map_data(downshift_map_name_diesel, down_map, def_downshift_data_diesel, SHIFT_MAP_SIZE);
} else {
EEPROM::read_nvs_map_data(upshift_map_name_petrol, up_map, def_upshift_data_petrol, SHIFT_MAP_SIZE);
EEPROM::read_nvs_map_data(downshift_map_name_petrol, down_map, def_downshift_data_petrol, SHIFT_MAP_SIZE);
}
if (this->upshift_table->add_data(up_map, SHIFT_MAP_SIZE) && this->downshift_table->add_data(down_map, SHIFT_MAP_SIZE)) {
ESP_LOGI(tag_id, "Upshift and downshift maps re-loaded OK!");
ShiftCharacteristics AbstractProfile::get_shift_characteristics(ProfileGearChange requested, SensorData* sensors) {
ShiftCharacteristics result;
switch (requested) {
case ProfileGearChange::ONE_TWO:
case ProfileGearChange::TWO_THREE:
case ProfileGearChange::THREE_FOUR:
case ProfileGearChange::FOUR_FIVE:
result.target_shift_time = this->get_upshift_time(sensors->input_rpm, ((float)sensors->pedal_pos*100.0)/250.0);
break;
case ProfileGearChange::FIVE_FOUR:
case ProfileGearChange::FOUR_THREE:
case ProfileGearChange::THREE_TWO:
case ProfileGearChange::TWO_ONE:
result.target_shift_time = this->get_downshift_time(sensors->input_rpm, ((float)sensors->pedal_pos*100.0)/250.0);
break;
default:
result.target_shift_time = 500;
break;
}
return result;
}
@ -83,10 +98,14 @@ AgilityProfile::AgilityProfile(bool is_diesel) : AbstractProfile(
MAP_NAME_A_DIESEL_DOWNSHIFT,
MAP_NAME_A_PETROL_UPSHIFT,
MAP_NAME_A_PETROL_DOWNSHIFT,
MAP_NAME_A_UPSHIFT_TIME,
MAP_NAME_A_DOWNSHIFT_TIME,
A_DIESEL_UPSHIFT_MAP,
A_DIESEL_DOWNSHIFT_MAP,
A_PETROL_UPSHIFT_MAP,
A_PETROL_DOWNSHIFT_MAP
A_PETROL_DOWNSHIFT_MAP,
A_UPSHIFT_TIME_MAP,
A_DOWNSHIFT_TIME_MAP
) {
}
@ -110,27 +129,12 @@ GearboxDisplayGear AgilityProfile::get_display_gear(GearboxGear target, GearboxG
return GearboxDisplayGear::Four;
case GearboxGear::Fifth:
return GearboxDisplayGear::Five;
case GearboxGear::SignalNotAvaliable:
case GearboxGear::SignalNotAvailable:
default:
return GearboxDisplayGear::SNA;
}
}
ShiftCharacteristics AgilityProfile::get_shift_characteristics(ProfileGearChange requested, SensorData* sensors) {
float dp = ((float)(sensors->pedal_pos*10)/250.0f);
if (dp > 10) {
dp = 10;
}
if (dp == 0) {
dp = 1;
}
return ShiftCharacteristics {
.target_d_rpm = 60,
.shift_speed = dp,
};
}
bool AgilityProfile::should_upshift(GearboxGear current_gear, SensorData* sensors) {
if (current_gear == GearboxGear::Fifth) {
return false;
@ -165,21 +169,17 @@ ComfortProfile::ComfortProfile(bool is_diesel) : AbstractProfile(
MAP_NAME_C_DIESEL_DOWNSHIFT,
MAP_NAME_C_PETROL_UPSHIFT,
MAP_NAME_C_PETROL_DOWNSHIFT,
MAP_NAME_C_UPSHIFT_TIME,
MAP_NAME_C_DOWNSHIFT_TIME,
C_DIESEL_UPSHIFT_MAP,
C_DIESEL_DOWNSHIFT_MAP,
C_PETROL_UPSHIFT_MAP,
C_PETROL_DOWNSHIFT_MAP
C_PETROL_DOWNSHIFT_MAP,
C_UPSHIFT_TIME_MAP,
C_DOWNSHIFT_TIME_MAP
) {
}
ShiftCharacteristics ComfortProfile::get_shift_characteristics(ProfileGearChange requested, SensorData* sensors) {
return ShiftCharacteristics {
.target_d_rpm = 30,
.shift_speed = 2.0,
};
}
GearboxDisplayGear ComfortProfile::get_display_gear(GearboxGear target, GearboxGear actual) {
switch (target) {
case GearboxGear::Park:
@ -199,7 +199,7 @@ GearboxDisplayGear ComfortProfile::get_display_gear(GearboxGear target, GearboxG
return GearboxDisplayGear::Four;
case GearboxGear::Fifth:
return GearboxDisplayGear::Five;
case GearboxGear::SignalNotAvaliable:
case GearboxGear::SignalNotAvailable:
default:
return GearboxDisplayGear::SNA;
}
@ -239,20 +239,17 @@ WinterProfile::WinterProfile(bool is_diesel) : AbstractProfile(
MAP_NAME_M_DIESEL_DOWNSHIFT,
MAP_NAME_M_PETROL_UPSHIFT,
MAP_NAME_M_PETROL_DOWNSHIFT,
MAP_NAME_C_UPSHIFT_TIME,
MAP_NAME_C_DOWNSHIFT_TIME,
M_DIESEL_UPSHIFT_MAP,
M_DIESEL_DOWNSHIFT_MAP,
M_PETROL_UPSHIFT_MAP,
M_PETROL_DOWNSHIFT_MAP
M_PETROL_DOWNSHIFT_MAP,
C_UPSHIFT_TIME_MAP,
C_DOWNSHIFT_TIME_MAP
) {
}
ShiftCharacteristics WinterProfile::get_shift_characteristics(ProfileGearChange requested, SensorData* sensors) {
return ShiftCharacteristics {
.target_d_rpm = 20,
.shift_speed = 3.0,
};
}
GearboxDisplayGear WinterProfile::get_display_gear(GearboxGear target, GearboxGear actual) {
switch (target) {
case GearboxGear::Park:
@ -272,7 +269,7 @@ GearboxDisplayGear WinterProfile::get_display_gear(GearboxGear target, GearboxGe
return GearboxDisplayGear::Four;
case GearboxGear::Fifth:
return GearboxDisplayGear::Five;
case GearboxGear::SignalNotAvaliable:
case GearboxGear::SignalNotAvailable:
default:
return GearboxDisplayGear::SNA;
}
@ -283,14 +280,10 @@ bool WinterProfile::should_upshift(GearboxGear current_gear, SensorData* sensors
}
bool WinterProfile::should_downshift(GearboxGear current_gear, SensorData* sensors) {
#ifdef MANUAL_AUTO_DOWNSHIFT
if (current_gear == GearboxGear::Second) {
return false;
}
return manual->should_downshift(current_gear, sensors);
#else
return false;
#endif
}
// Minimum lockup
@ -308,20 +301,17 @@ StandardProfile::StandardProfile(bool is_diesel) : AbstractProfile(
MAP_NAME_S_DIESEL_DOWNSHIFT,
MAP_NAME_S_PETROL_UPSHIFT,
MAP_NAME_S_PETROL_DOWNSHIFT,
MAP_NAME_S_UPSHIFT_TIME,
MAP_NAME_S_DOWNSHIFT_TIME,
S_DIESEL_UPSHIFT_MAP,
S_DIESEL_DOWNSHIFT_MAP,
S_PETROL_UPSHIFT_MAP,
S_PETROL_DOWNSHIFT_MAP
S_PETROL_DOWNSHIFT_MAP,
S_UPSHIFT_TIME_MAP,
S_DOWNSHIFT_TIME_MAP
) {
}
ShiftCharacteristics StandardProfile::get_shift_characteristics(ProfileGearChange requested, SensorData* sensors) {
return ShiftCharacteristics {
.target_d_rpm = 50,
.shift_speed = 5.0,
};
}
GearboxDisplayGear StandardProfile::get_display_gear(GearboxGear target, GearboxGear actual) {
switch (target) {
case GearboxGear::Park:
@ -341,7 +331,7 @@ GearboxDisplayGear StandardProfile::get_display_gear(GearboxGear target, Gearbox
return GearboxDisplayGear::Four;
case GearboxGear::Fifth:
return GearboxDisplayGear::Five;
case GearboxGear::SignalNotAvaliable:
case GearboxGear::SignalNotAvailable:
default:
return GearboxDisplayGear::SNA;
}
@ -409,13 +399,6 @@ TccLockupBounds StandardProfile::get_tcc_lockup_bounds(SensorData* sensors, Gear
};
}
ShiftCharacteristics ManualProfile::get_shift_characteristics(ProfileGearChange requested, SensorData* sensors) {
return ShiftCharacteristics {
.target_d_rpm = 70,
.shift_speed = 9.0,
};
}
GearboxDisplayGear ManualProfile::get_display_gear(GearboxGear target, GearboxGear actual) {
switch (target) {
case GearboxGear::Park:
@ -435,7 +418,7 @@ GearboxDisplayGear ManualProfile::get_display_gear(GearboxGear target, GearboxGe
return GearboxDisplayGear::Four;
case GearboxGear::Fifth:
return GearboxDisplayGear::Five;
case GearboxGear::SignalNotAvaliable:
case GearboxGear::SignalNotAvailable:
default:
return GearboxDisplayGear::SNA;
}
@ -448,10 +431,14 @@ ManualProfile::ManualProfile(bool is_diesel) : AbstractProfile(
MAP_NAME_M_DIESEL_DOWNSHIFT,
MAP_NAME_M_PETROL_UPSHIFT,
MAP_NAME_M_PETROL_DOWNSHIFT,
MAP_NAME_M_UPSHIFT_TIME,
MAP_NAME_M_DOWNSHIFT_TIME,
M_DIESEL_UPSHIFT_MAP,
M_DIESEL_DOWNSHIFT_MAP,
M_PETROL_UPSHIFT_MAP,
M_PETROL_DOWNSHIFT_MAP
M_PETROL_DOWNSHIFT_MAP,
M_UPSHIFT_TIME_MAP,
M_DOWNSHIFT_TIME_MAP
) {
}
@ -460,16 +447,12 @@ bool ManualProfile::should_upshift(GearboxGear current_gear, SensorData* sensors
}
bool ManualProfile::should_downshift(GearboxGear current_gear, SensorData* sensors) {
#ifdef MANUAL_AUTO_DOWNSHIFT
if (current_gear == GearboxGear::First) {
return false;
} else if (sensors->input_rpm < 300 && sensors->engine_rpm < MIN_WORKING_RPM && sensors->pedal_pos == 0) {
return true;
}
return false;
#else
return false;
#endif
}
TccLockupBounds ManualProfile::get_tcc_lockup_bounds(SensorData* sensors, GearboxGear curr_gear) {
@ -479,6 +462,44 @@ TccLockupBounds ManualProfile::get_tcc_lockup_bounds(SensorData* sensors, Gearbo
};
}
RaceProfile::RaceProfile(bool is_diesel): AbstractProfile(
is_diesel,
"RACE",
MAP_NAME_M_DIESEL_UPSHIFT,
MAP_NAME_M_DIESEL_DOWNSHIFT,
MAP_NAME_M_PETROL_UPSHIFT,
MAP_NAME_M_PETROL_DOWNSHIFT,
MAP_NAME_R_UPSHIFT_TIME,
MAP_NAME_R_DOWNSHIFT_TIME,
M_DIESEL_UPSHIFT_MAP,
M_DIESEL_DOWNSHIFT_MAP,
M_PETROL_UPSHIFT_MAP,
M_PETROL_DOWNSHIFT_MAP,
R_UPSHIFT_TIME_MAP,
R_DOWNSHIFT_TIME_MAP
) {
}
GearboxDisplayGear RaceProfile::get_display_gear(GearboxGear target, GearboxGear actual) {
return manual->get_display_gear(target, actual);
}
bool RaceProfile::should_upshift(GearboxGear current_gear, SensorData* sensors) {
return manual->should_upshift(current_gear, sensors);
}
bool RaceProfile::should_downshift(GearboxGear current_gear, SensorData* sensors) {
return manual->should_downshift(current_gear, sensors);
}
TccLockupBounds RaceProfile::get_tcc_lockup_bounds(SensorData* sensors, GearboxGear curr_gear) {
return TccLockupBounds {
.max_slip_rpm = 50,
.min_slip_rpm = 10
};
}
/*
AgilityProfile* agility = new AgilityProfile();
ComfortProfile* comfort = new ComfortProfile();
@ -493,3 +514,4 @@ ComfortProfile* comfort = nullptr;
WinterProfile* winter = nullptr;
ManualProfile* manual = nullptr;
StandardProfile* standard = nullptr;
RaceProfile* race = nullptr;

103
src/profiles.h
View File

@ -1,19 +1,18 @@
// Profile code
#ifndef __PROFILES_H_
#define __PROFILES_H_
#ifndef PROFILES_H
#define PROFILES_H
#include "canbus/can_hal.h"
#include "common_structs.h"
#include "tcm_maths.h"
#define MAX_PROFILES 4
#include "stored_map.h"
#define PROFILE_ID_STANDARD 0
#define PROFILE_ID_COMFORT 1
#define PROFILE_ID_WINTER 2
#define PROFILE_ID_AGILITY 3
#define PROFILE_ID_MANUAL 4
#define PROFILE_ID_RACE 5
#define SHIFT_MAP_X_SIZE 11
#define SHIFT_MAP_Y_SIZE 4
@ -21,12 +20,15 @@ const int16_t shift_table_x_header[SHIFT_MAP_X_SIZE] = {0, 10, 20, 30, 40, 50, 6
const int16_t upshift_y_headers[SHIFT_MAP_Y_SIZE] = {1,2,3,4};
const int16_t downshift_y_headers[SHIFT_MAP_Y_SIZE] = {2,3,4,5};
const int16_t shift_time_table_x_header[6] = {0, 20, 40, 60, 80, 100};
/**
* A profile is designed to read the current conditions of the gearbox and request the gearbox to do something
*/
class AbstractProfile {
public:
static const uint8_t MAX_PROFILES = 4u;
AbstractProfile(
bool is_diesel,
const char* tag_id,
@ -34,72 +36,90 @@ public:
const char* downshift_map_name_diesel,
const char* upshift_map_name_petrol,
const char* downshift_map_name_petrol,
const char* upshift_time_map_name,
const char* downshift_time_map_name,
const int16_t* def_upshift_data_diesel,
const int16_t* def_downshift_data_diesel,
const int16_t* def_upshift_data_petrol,
const int16_t* def_downshift_data_petrol
const int16_t* def_downshift_data_petrol,
const int16_t* def_upshift_time_data,
const int16_t* def_downshift_time_data
);
virtual GearboxProfile get_profile() const = 0;
virtual GearboxProfile get_profile(void) const = 0;
virtual GearboxDisplayGear get_display_gear(GearboxGear target, GearboxGear actual) = 0;
virtual bool should_upshift(GearboxGear current_gear, SensorData* sensors) = 0;
virtual bool should_downshift(GearboxGear current_gear, SensorData* sensors) = 0;
virtual ShiftCharacteristics get_shift_characteristics(ProfileGearChange requested, SensorData* sensors) = 0;
ShiftCharacteristics get_shift_characteristics(ProfileGearChange requested, SensorData* sensors);
virtual TccLockupBounds get_tcc_lockup_bounds(SensorData* sensors, GearboxGear curr_gear) = 0;
TcmMap* get_upshift_map() {
StoredTcuMap* get_upshift_map(void) {
return this->upshift_table;
}
TcmMap* get_downshift_map() {
StoredTcuMap* get_downshift_map(void) {
return this->downshift_table;
}
virtual GearboxGear get_start_gear() const {
StoredTcuMap* get_upshift_time_map(void) {
return this->upshift_time_map;
}
StoredTcuMap* get_downshift_time_map(void) {
return this->downshift_time_map;
}
uint16_t get_upshift_time(uint16_t input_rpm, float pedal_percent) {
uint16_t result = 750u;
if (nullptr != this->upshift_time_map) {
result = (uint16_t)(this->upshift_time_map->get_value(pedal_percent, (float)input_rpm));
}
return result;
}
uint16_t get_downshift_time(uint16_t input_rpm, float pedal_percent) {
uint16_t result = 750u;
if (nullptr != this->downshift_time_map) {
result = (uint16_t)(this->downshift_time_map->get_value(pedal_percent, (float)input_rpm));
}
return result;
}
virtual GearboxGear get_start_gear(void) const {
return GearboxGear::First;
}
void increment_subprofile() {
profile_id += 1;
void increment_subprofile(void) {
profile_id += 1u;
if (profile_id >= MAX_PROFILES) {
profile_id = 0;
}
}
virtual uint8_t get_profile_id() = 0;
void reload_data();
virtual uint8_t get_profile_id(void) = 0;
protected:
uint8_t profile_id = 0;
TcmMap* upshift_table = nullptr;
TcmMap* downshift_table = nullptr;
StoredTcuMap* upshift_table = nullptr;
StoredTcuMap* downshift_table = nullptr;
StoredTcuMap* upshift_time_map = nullptr;
StoredTcuMap* downshift_time_map = nullptr;
private:
bool is_diesel;
const char* tag_id;
const char* upshift_map_name_diesel;
const char* downshift_map_name_diesel;
const char* upshift_map_name_petrol;
const char* downshift_map_name_petrol;
const int16_t* def_upshift_data_diesel;
const int16_t* def_downshift_data_diesel;
const int16_t* def_upshift_data_petrol;
const int16_t* def_downshift_data_petrol;
const char* tag_id;
};
class AgilityProfile : public AbstractProfile {
public:
AgilityProfile(bool is_diesel);
explicit AgilityProfile(bool is_diesel);
GearboxProfile get_profile() const override { return GearboxProfile::Agility; }
GearboxDisplayGear get_display_gear(GearboxGear target, GearboxGear actual) override;
bool should_upshift(GearboxGear current_gear, SensorData* sensors) override;
bool should_downshift(GearboxGear current_gear, SensorData* sensors) override;
ShiftCharacteristics get_shift_characteristics(ProfileGearChange requested, SensorData* sensors) override;
TccLockupBounds get_tcc_lockup_bounds(SensorData* sensors, GearboxGear curr_gear) override;
uint8_t get_profile_id() override { return PROFILE_ID_AGILITY; }
};
class ComfortProfile : public AbstractProfile {
public:
ComfortProfile(bool is_diesel);
explicit ComfortProfile(bool is_diesel);
GearboxProfile get_profile() const override { return GearboxProfile::Comfort; }
GearboxDisplayGear get_display_gear(GearboxGear target, GearboxGear actual) override;
bool should_upshift(GearboxGear current_gear, SensorData* sensors) override;
bool should_downshift(GearboxGear current_gear, SensorData* sensors) override;
ShiftCharacteristics get_shift_characteristics(ProfileGearChange requested, SensorData* sensors) override;
TccLockupBounds get_tcc_lockup_bounds(SensorData* sensors, GearboxGear curr_gear) override;
uint8_t get_profile_id() override { return PROFILE_ID_COMFORT; }
GearboxGear get_start_gear() const override {
@ -109,12 +129,11 @@ public:
class WinterProfile : public AbstractProfile {
public:
WinterProfile(bool is_diesel);
GearboxProfile get_profile() const override { return GearboxProfile::Individual; }
explicit WinterProfile(bool is_diesel);
GearboxProfile get_profile() const override { return GearboxProfile::Winter; }
GearboxDisplayGear get_display_gear(GearboxGear target, GearboxGear actual) override;
bool should_upshift(GearboxGear current_gear, SensorData* sensors) override;
bool should_downshift(GearboxGear current_gear, SensorData* sensors) override;
ShiftCharacteristics get_shift_characteristics(ProfileGearChange requested, SensorData* sensors) override;
TccLockupBounds get_tcc_lockup_bounds(SensorData* sensors, GearboxGear curr_gear) override;
uint8_t get_profile_id() override { return PROFILE_ID_WINTER; }
GearboxGear get_start_gear() const override {
@ -124,32 +143,42 @@ public:
class StandardProfile : public AbstractProfile {
public:
StandardProfile(bool is_diesel);
explicit StandardProfile(bool is_diesel);
GearboxProfile get_profile() const override { return GearboxProfile::Standard; }
GearboxDisplayGear get_display_gear(GearboxGear target, GearboxGear actual) override;
bool should_upshift(GearboxGear current_gear, SensorData* sensors) override;
bool should_downshift(GearboxGear current_gear, SensorData* sensors) override;
TccLockupBounds get_tcc_lockup_bounds(SensorData* sensors, GearboxGear curr_gear) override;
ShiftCharacteristics get_shift_characteristics(ProfileGearChange requested, SensorData* sensors) override;
uint8_t get_profile_id() override { return PROFILE_ID_STANDARD; }
};
class ManualProfile : public AbstractProfile {
public:
ManualProfile(bool is_diesel);
explicit ManualProfile(bool is_diesel);
GearboxProfile get_profile() const override { return GearboxProfile::Manual; }
GearboxDisplayGear get_display_gear(GearboxGear target, GearboxGear actual) override;
bool should_upshift(GearboxGear current_gear, SensorData* sensors) override;
bool should_downshift(GearboxGear current_gear, SensorData* sensors) override;
TccLockupBounds get_tcc_lockup_bounds(SensorData* sensors, GearboxGear curr_gear) override;
ShiftCharacteristics get_shift_characteristics(ProfileGearChange requested, SensorData* sensors) override;
uint8_t get_profile_id() override { return PROFILE_ID_MANUAL; }
};
class RaceProfile : public AbstractProfile {
public:
explicit RaceProfile(bool is_diesel);
GearboxProfile get_profile() const override { return GearboxProfile::Race; }
GearboxDisplayGear get_display_gear(GearboxGear target, GearboxGear actual) override;
bool should_upshift(GearboxGear current_gear, SensorData* sensors) override;
bool should_downshift(GearboxGear current_gear, SensorData* sensors) override;
TccLockupBounds get_tcc_lockup_bounds(SensorData* sensors, GearboxGear curr_gear) override;
uint8_t get_profile_id() override { return PROFILE_ID_RACE; }
};
extern AgilityProfile* agility;
extern ComfortProfile* comfort;
extern WinterProfile* winter;
extern ManualProfile* manual;
extern StandardProfile* standard;
extern RaceProfile* race;
#endif

469
src/sensors.cpp
View File

@ -7,8 +7,9 @@
#include "freertos/task.h"
#include "freertos/semphr.h"
#include "esp_log.h"
#include "pins.h"
#include "macros.h"
#include "board_config.h"
#include "nvs/eeprom_config.h"
#include "esp_check.h"
#define PULSES_PER_REV 60 // N2 and N3 are 60 pulses per revolution
#define PCNT_H_LIM 1
@ -17,208 +18,144 @@
const pcnt_unit_t PCNT_N2_RPM = PCNT_UNIT_0;
const pcnt_unit_t PCNT_N3_RPM = PCNT_UNIT_1;
// PCNT configurations
const pcnt_config_t pcnt_cfg_n2 {
.pulse_gpio_num = PIN_N2,
.ctrl_gpio_num = PCNT_PIN_NOT_USED,
.lctrl_mode = PCNT_MODE_KEEP,
.hctrl_mode = PCNT_MODE_KEEP,
.pos_mode = PCNT_COUNT_INC,
.neg_mode = PCNT_COUNT_DIS,
.counter_h_lim = PCNT_H_LIM,
.counter_l_lim = 0,
.unit = PCNT_N2_RPM,
.channel = PCNT_CHANNEL_0
};
const pcnt_config_t pcnt_cfg_n3 {
.pulse_gpio_num = PIN_N3,
.ctrl_gpio_num = PCNT_PIN_NOT_USED,
.lctrl_mode = PCNT_MODE_KEEP,
.hctrl_mode = PCNT_MODE_KEEP,
.pos_mode = PCNT_COUNT_INC,
.neg_mode = PCNT_COUNT_DIS,
.counter_h_lim = PCNT_H_LIM,
.counter_l_lim = 0,
.unit = PCNT_N3_RPM,
.channel = PCNT_CHANNEL_0
};
typedef struct {
// Voltage in mV
uint16_t v;
// ATF Temp in degrees C * 10
int temp;
} temp_reading_t;
#define NUM_TEMP_POINTS 22
#ifdef BOARD_V2
const static temp_reading_t atf_temp_lookup[NUM_TEMP_POINTS] = {
// mV, Temp(x10)
// mV Values are calibrated on 3.45V rail
// as that is how much the ATF sensor power gets
{725, -400},
{784, -300},
{842, -200},
{900, -100},
{957, 0},
{1013, 100},
{1068, 200},
{1123, 300},
{1177, 400},
{1230, 500},
{1281, 600},
{1332, 700},
{1384, 800},
{1438, 900},
{1488, 1000},
{1538, 1100},
{1587, 1200},
{1636, 1300},
{1685, 1400},
{1732, 1500},
{1779, 1600},
{1826, 1700}
};
#define ADC_CHANNEL_VBATT adc2_channel_t::ADC2_CHANNEL_8
#define ADC_CHANNEL_ATF adc2_channel_t::ADC2_CHANNEL_7
#else
const static temp_reading_t atf_temp_lookup[NUM_TEMP_POINTS] = {
// mV, Temp(x10)
// mV Values are calibrated on 3.45V rail
// as that is how much the ATF sensor power gets
{446, -400},
{461, -300},
{476, -200},
{491, -100},
{507, 0},
{523, 100},
{540, 200},
{557, 300},
{574, 400},
{592, 500},
{610, 600},
{629, 700},
{648, 800},
{669, 900},
{690, 1000},
{711, 1100},
{732, 1200},
{755, 1300},
{778, 1400},
{802, 1500},
{814, 1600},
{851, 1700}
};
#define ADC_CHANNEL_VBATT adc2_channel_t::ADC2_CHANNEL_8
#define ADC_CHANNEL_ATF adc2_channel_t::ADC2_CHANNEL_9
#endif
const pcnt_unit_t PCNT_OUT_RPM = PCNT_UNIT_2;
#define ADC2_ATTEN ADC_ATTEN_11db
#define ADC2_WIDTH ADC_WIDTH_12Bit
esp_adc_cal_characteristics_t adc2_cal_vbatt = {};
esp_adc_cal_characteristics_t adc2_cal_atf = {};
esp_adc_cal_characteristics_t adc2_cal = {};
portMUX_TYPE n2_mux = portMUX_INITIALIZER_UNLOCKED;
portMUX_TYPE n3_mux = portMUX_INITIALIZER_UNLOCKED;
portMUX_TYPE output_mux = portMUX_INITIALIZER_UNLOCKED;
volatile uint64_t n3_intr_times[2] = {0,0};
volatile uint64_t n2_intr_times[2] = {0,0};
volatile uint64_t n3_intr_times[2] = {0, 0};
volatile uint64_t n2_intr_times[2] = {0, 0};
volatile uint64_t output_intr_times[2] = {0,0};
uint64_t t_n2 = 0;
uint64_t t_n3 = 0;
uint64_t t_output = 0;
bool output_rpm_ok = false;
static void IRAM_ATTR on_pcnt_overflow_n2(void* args) {
static void IRAM_ATTR on_pcnt_overflow_n2(void *args)
{
t_n2 = esp_timer_get_time();
if (t_n2 - n2_intr_times[1] < 10) {
return;
if (t_n2 - n2_intr_times[1] > 10)
{
portENTER_CRITICAL_ISR(&n2_mux);
n2_intr_times[0] = n2_intr_times[1];
n2_intr_times[1] = t_n2;
portEXIT_CRITICAL_ISR(&n2_mux);
}
portENTER_CRITICAL_ISR(&n2_mux);
n2_intr_times[0] = n2_intr_times[1];
n2_intr_times[1] = t_n2;
portEXIT_CRITICAL_ISR(&n2_mux);
}
static void IRAM_ATTR on_pcnt_overflow_n3(void* args) {
static void IRAM_ATTR on_pcnt_overflow_n3(void *args)
{
t_n3 = esp_timer_get_time();
if (t_n3 - n3_intr_times[1] < 10) {
return;
if (t_n3 - n3_intr_times[1] > 10)
{
portENTER_CRITICAL_ISR(&n3_mux);
n3_intr_times[0] = n3_intr_times[1];
n3_intr_times[1] = t_n3;
portEXIT_CRITICAL_ISR(&n3_mux);
}
portENTER_CRITICAL_ISR(&n3_mux);
n3_intr_times[0] = n3_intr_times[1];
n3_intr_times[1] = t_n3;
portEXIT_CRITICAL_ISR(&n3_mux);
}
bool Sensors::init_sensors(){
esp_err_t res;
CHECK_ESP_FUNC(gpio_set_direction(PIN_VBATT, GPIO_MODE_INPUT), "Failed to set PIN_VBATT to Input! %s", esp_err_to_name(res))
CHECK_ESP_FUNC(gpio_set_direction(PIN_ATF, GPIO_MODE_INPUT), "Failed to set PIN_ATF to Input! %s", esp_err_to_name(res))
CHECK_ESP_FUNC(gpio_set_direction(PIN_N2, GPIO_MODE_INPUT), "Failed to set PIN_N2 to Input! %s", esp_err_to_name(res))
CHECK_ESP_FUNC(gpio_set_direction(PIN_N3, GPIO_MODE_INPUT), "Failed to set PIN_N3 to Input! %s", esp_err_to_name(res))
static void IRAM_ATTR on_pcnt_overflow_output(void* args) {
t_output = esp_timer_get_time();
if (t_output - output_intr_times[1] > 10) {
portENTER_CRITICAL_ISR(&output_mux);
output_intr_times[0] = output_intr_times[1];
output_intr_times[1] = t_output;
portEXIT_CRITICAL_ISR(&output_mux);
}
}
esp_err_t Sensors::init_sensors(){
const pcnt_config_t pcnt_cfg_n2{
.pulse_gpio_num = pcb_gpio_matrix->n2_pin,
.ctrl_gpio_num = PCNT_PIN_NOT_USED,
.lctrl_mode = PCNT_MODE_KEEP,
.hctrl_mode = PCNT_MODE_KEEP,
.pos_mode = PCNT_COUNT_INC,
.neg_mode = PCNT_COUNT_DIS,
.counter_h_lim = PCNT_H_LIM,
.counter_l_lim = 0,
.unit = PCNT_N2_RPM,
.channel = PCNT_CHANNEL_0};
const pcnt_config_t pcnt_cfg_n3{
.pulse_gpio_num = pcb_gpio_matrix->n3_pin,
.ctrl_gpio_num = PCNT_PIN_NOT_USED,
.lctrl_mode = PCNT_MODE_KEEP,
.hctrl_mode = PCNT_MODE_KEEP,
.pos_mode = PCNT_COUNT_INC,
.neg_mode = PCNT_COUNT_DIS,
.counter_h_lim = PCNT_H_LIM,
.counter_l_lim = 0,
.unit = PCNT_N3_RPM,
.channel = PCNT_CHANNEL_0};
ESP_RETURN_ON_ERROR(gpio_set_direction(pcb_gpio_matrix->vsense_pin, GPIO_MODE_INPUT), "SENSORS", "Failed to set PIN_VBATT to Input!");
ESP_RETURN_ON_ERROR(gpio_set_direction(pcb_gpio_matrix->atf_pin, GPIO_MODE_INPUT), "SENSORS", "Failed to set PIN_ATF to Input!");
ESP_RETURN_ON_ERROR(gpio_set_direction(pcb_gpio_matrix->n2_pin, GPIO_MODE_INPUT), "SENSORS", "Failed to set PIN_N2 to Input!");
ESP_RETURN_ON_ERROR(gpio_set_direction(pcb_gpio_matrix->n3_pin, GPIO_MODE_INPUT), "SENSORS", "Failed to set PIN_N3 to Input!");
// Set RPM pins to pullup
CHECK_ESP_FUNC(gpio_set_pull_mode(PIN_N2, GPIO_PULLUP_ONLY), "Failed to set PIN_N2 to Input! %s", esp_err_to_name(res))
CHECK_ESP_FUNC(gpio_set_pull_mode(PIN_N3, GPIO_PULLUP_ONLY), "Failed to set PIN_N3 to Input! %s", esp_err_to_name(res))
ESP_RETURN_ON_ERROR(gpio_set_pull_mode(pcb_gpio_matrix->n2_pin, GPIO_PULLUP_ONLY), "SENSORS", "Failed to set PIN_N2 to Input!");
ESP_RETURN_ON_ERROR(gpio_set_pull_mode(pcb_gpio_matrix->n3_pin, GPIO_PULLUP_ONLY), "SENSORS", "Failed to set PIN_N3 to Input!");
// Configure ADC2 for analog readings
CHECK_ESP_FUNC(adc2_config_channel_atten(ADC_CHANNEL_VBATT, ADC_ATTEN_11db), "Failed to set ADC attenuation for PIN_ATF! %s", esp_err_to_name(res))
CHECK_ESP_FUNC(adc2_config_channel_atten(ADC_CHANNEL_ATF, ADC_ATTEN_11db), "Failed to set ADC attenuation for PIN_VBATT! %s", esp_err_to_name(res))
// Characterise ADC2
esp_adc_cal_characterize(adc_unit_t::ADC_UNIT_2, adc_atten_t::ADC_ATTEN_DB_11, ADC2_WIDTH, 0, &adc2_cal_atf);
esp_adc_cal_characterize(adc_unit_t::ADC_UNIT_2, adc_atten_t::ADC_ATTEN_DB_11, ADC2_WIDTH, 0, &adc2_cal_vbatt);
ESP_RETURN_ON_ERROR(adc2_config_channel_atten(pcb_gpio_matrix->sensor_data.atf_channel, ADC_ATTEN_11db), "SENSORS", "Failed to set ADC attenuation for PIN_ATF!");
ESP_RETURN_ON_ERROR(adc2_config_channel_atten(pcb_gpio_matrix->sensor_data.batt_channel, ADC_ATTEN_11db), "SENSORS", "Failed to set ADC attenuation for PIN_VBATT!");
// Characterise ADC2
esp_adc_cal_characterize(adc_unit_t::ADC_UNIT_2, adc_atten_t::ADC_ATTEN_DB_11, ADC2_WIDTH, 0, &adc2_cal);
// Now configure PCNT to begin counting!
CHECK_ESP_FUNC(pcnt_unit_config(&pcnt_cfg_n2), "Failed to configure PCNT for N2 RPM reading! %s", esp_err_to_name(res))
CHECK_ESP_FUNC(pcnt_unit_config(&pcnt_cfg_n3), "Failed to configure PCNT for N3 RPM reading! %s", esp_err_to_name(res))
ESP_RETURN_ON_ERROR(pcnt_unit_config(&pcnt_cfg_n2), "SENSORS", "Failed to configure PCNT for N2 RPM reading!");
ESP_RETURN_ON_ERROR(pcnt_unit_config(&pcnt_cfg_n3), "SENSORS", "Failed to configure PCNT for N3 RPM reading!");
// Pause PCNTs unit configuration is complete
CHECK_ESP_FUNC(pcnt_counter_pause(PCNT_N2_RPM), "Failed to pause PCNT N2 RPM! %s", esp_err_to_name(res))
CHECK_ESP_FUNC(pcnt_counter_pause(PCNT_N3_RPM), "Failed to pause PCNT N3 RPM! %s", esp_err_to_name(res))
// Clear their stored values (If present)
CHECK_ESP_FUNC(pcnt_counter_clear(PCNT_N2_RPM), "Failed to clear PCNT N2 RPM! %s", esp_err_to_name(res))
CHECK_ESP_FUNC(pcnt_counter_clear(PCNT_N3_RPM), "Failed to clear PCNT N3 RPM! %s", esp_err_to_name(res))
ESP_RETURN_ON_ERROR(pcnt_counter_pause(PCNT_N2_RPM), "SENSORS", "Failed to pause PCNT N2 RPM!");
ESP_RETURN_ON_ERROR(pcnt_counter_pause(PCNT_N3_RPM), "SENSORS", "Failed to pause PCNT N3 RPM!");
// Clear their stored values (If present)
ESP_RETURN_ON_ERROR(pcnt_counter_clear(PCNT_N2_RPM), "SENSORS", "Failed to clear PCNT N2 RPM!");
ESP_RETURN_ON_ERROR(pcnt_counter_clear(PCNT_N3_RPM), "SENSORS", "Failed to clear PCNT N3 RPM!");
// Setup filter to ignore ultra short pulses (possibly noise)
// Using a value of 40 at 80Mhz APB_CLOCK = this will correctly filter noise up to 30,000RPM
CHECK_ESP_FUNC(pcnt_set_filter_value(PCNT_N2_RPM, 1000), "Failed to set filter for PCNT N2! %s", esp_err_to_name(res))
CHECK_ESP_FUNC(pcnt_set_filter_value(PCNT_N3_RPM, 1000), "Failed to set filter for PCNT N3! %s", esp_err_to_name(res))
CHECK_ESP_FUNC(pcnt_filter_enable(PCNT_N2_RPM), "Failed to enable filter for PCNT N2! %s", esp_err_to_name(res))
CHECK_ESP_FUNC(pcnt_filter_enable(PCNT_N3_RPM), "Failed to enable filter for PCNT N3! %s", esp_err_to_name(res))
// Using a value of 40 at 80Mhz APB_CLOCK = this will correctly filter noise up to 30,000RPM
ESP_RETURN_ON_ERROR(pcnt_set_filter_value(PCNT_N2_RPM, 1000), "SENSORS", "Failed to set filter for PCNT N2!");
ESP_RETURN_ON_ERROR(pcnt_set_filter_value(PCNT_N3_RPM, 1000), "SENSORS", "Failed to set filter for PCNT N3!");
ESP_RETURN_ON_ERROR(pcnt_filter_enable(PCNT_N2_RPM), "SENSORS", "Failed to enable filter for PCNT N2!");
ESP_RETURN_ON_ERROR(pcnt_filter_enable(PCNT_N3_RPM), "SENSORS", "Failed to enable filter for PCNT N3!");
// Now install and register ISR interrupts
CHECK_ESP_FUNC(pcnt_isr_service_install(0), "Failed to install ISR service for PCNT! %s", esp_err_to_name(res))
CHECK_ESP_FUNC(pcnt_isr_handler_add(PCNT_N2_RPM, &on_pcnt_overflow_n2, nullptr), "Failed to add PCNT N2 to ISR handler! %s", esp_err_to_name(res))
CHECK_ESP_FUNC(pcnt_isr_handler_add(PCNT_N3_RPM, &on_pcnt_overflow_n3, nullptr), "Failed to add PCNT N3 to ISR handler! %s", esp_err_to_name(res))
ESP_RETURN_ON_ERROR(pcnt_isr_service_install(0), "SENSORS", "Failed to install ISR service for PCNT!");
ESP_RETURN_ON_ERROR(pcnt_isr_handler_add(PCNT_N2_RPM, &on_pcnt_overflow_n2, nullptr), "SENSORS", "Failed to add PCNT N2 to ISR handler!");
ESP_RETURN_ON_ERROR(pcnt_isr_handler_add(PCNT_N3_RPM, &on_pcnt_overflow_n3, nullptr), "SENSORS", "Failed to add PCNT N3 to ISR handler!");
// Enable interrupts on hitting hlim on PCNTs
CHECK_ESP_FUNC(pcnt_event_enable(PCNT_N2_RPM, pcnt_evt_type_t::PCNT_EVT_H_LIM), "Failed to register event for PCNT N2! %s", esp_err_to_name(res))
CHECK_ESP_FUNC(pcnt_event_enable(PCNT_N3_RPM, pcnt_evt_type_t::PCNT_EVT_H_LIM), "Failed to register event for PCNT N3! %s", esp_err_to_name(res))
ESP_RETURN_ON_ERROR(pcnt_event_enable(PCNT_N2_RPM, pcnt_evt_type_t::PCNT_EVT_H_LIM), "SENSORS", "Failed to register event for PCNT N2!");
ESP_RETURN_ON_ERROR(pcnt_event_enable(PCNT_N3_RPM, pcnt_evt_type_t::PCNT_EVT_H_LIM), "SENSORS", "Failed to register event for PCNT N3!");
// Resume counting
CHECK_ESP_FUNC(pcnt_counter_resume(PCNT_N2_RPM), "Failed to resume PCNT N2 RPM! %s", esp_err_to_name(res))
CHECK_ESP_FUNC(pcnt_counter_resume(PCNT_N3_RPM), "Failed to resume PCNT N3 RPM! %s", esp_err_to_name(res))
ESP_RETURN_ON_ERROR(pcnt_counter_resume(PCNT_N2_RPM), "SENSORS", "Failed to resume PCNT N2 RPM!");
ESP_RETURN_ON_ERROR(pcnt_counter_resume(PCNT_N3_RPM), "SENSORS", "Failed to resume PCNT N3 RPM!");
ESP_LOG_LEVEL(ESP_LOG_INFO, LOG_TAG, "Sensors INIT OK!");
return true;
return ESP_OK;
}
#define US_PER_SECOND 1000000 // Microseconds per pulse per RPM
// #define US_PER_SECOND 1000000 // Microseconds per pulse per RPM
// 1rpm = 60 pulse/sec -> 1 pulse every 20ms at MINIMUM
bool Sensors::read_input_rpm(RpmReading* dest, bool check_sanity) {
esp_err_t Sensors::read_input_rpm(RpmReading *dest, bool check_sanity)
{
uint64_t n2[2];
uint64_t n3[2];
portENTER_CRITICAL(&n2_mux);
@ -233,118 +170,144 @@ bool Sensors::read_input_rpm(RpmReading* dest, bool check_sanity) {
uint64_t d_n2 = n2[1] - n2[0];
uint64_t d_n3 = n3[1] - n3[0];
uint64_t now = esp_timer_get_time();
if (d_n2 == 0 || now - n2_intr_times[1] > 20000) {
esp_err_t res = ESP_OK;
if (d_n2 == 0 || now - n2_intr_times[1] > 20000)
{
dest->n2_raw = 0;
} else {
}
else
{
dest->n2_raw = 1000000 / d_n2;
}
if (d_n3 == 0 || now - n3_intr_times[1] > 20000) {
if (d_n3 == 0 || now - n3_intr_times[1] > 20000)
{
dest->n3_raw = 0;
} else {
}
else
{
dest->n3_raw = 1000000 / d_n3;
}
if (dest->n2_raw < 60 && dest->n3_raw < 60) { // Stationary ( < 1rpm ), break here to avoid divideBy0Ex, and also noise
if (dest->n2_raw < 60 && dest->n3_raw < 60)
{ // Stationary ( < 1rpm ), break here to avoid divideBy0Ex, and also noise
dest->calc_rpm = 0;
return true;
} else if (dest->n2_raw == 0) { // In gears R1 or R2 (as N2 is 0)
dest->calc_rpm = dest->n3_raw;
return true;
} else {
if (abs((int)dest->n2_raw - (int)dest->n3_raw) < 10) {
dest->calc_rpm = (dest->n2_raw+dest->n3_raw)/2;
return true;
}
// More difficult calculation for all forward gears
// This calculation works when both RPM sensors are the same (Gears 2,3,4)
// Or when N3 is 0 and N2 is reporting ~0.61x real Rpm (Gears 1 and 5)
// Also nicely handles transitionary phases between RPM readings, making gear shift RPM readings
// a lot more accurate for the rest of the TCM code
float ratio = (float)dest->n3_raw/(float)dest->n2_raw;
float f2 = (float)dest->n2_raw;
float f3 = (float)dest->n3_raw;
dest->calc_rpm = ((f2*1.64f)*(1.0f-ratio))+(f3*ratio);
// If we need to check sanity, check it, in gears 2,3 and 4, RPM readings should be the same,
// otherwise we have a faulty conductor place sensor!
return check_sanity ? abs((int)dest->n2_raw - (int)dest->n3_raw) < 150 : true;
}
else if (dest->n2_raw == 0)
{ // In gears R1 or R2 (as N2 is 0)
dest->calc_rpm = dest->n3_raw;
}
else
{
if (abs((int)dest->n2_raw - (int)dest->n3_raw) < 10)
{
dest->calc_rpm = (dest->n2_raw + dest->n3_raw) / 2;
} else {
// More difficult calculation for all forward gears
// This calculation works when both RPM sensors are the same (Gears 2,3,4)
// Or when N3 is 0 and N2 is reporting ~0.61x real Rpm (Gears 1 and 5)
// Also nicely handles transitionary phases between RPM readings, making gear shift RPM readings
// a lot more accurate for the rest of the TCM code
float ratio = (float)dest->n3_raw / (float)dest->n2_raw;
float f2 = (float)dest->n2_raw;
float f3 = (float)dest->n3_raw;
dest->calc_rpm = ((f2 * 1.64f) * (1.0f - ratio)) + (f3 * ratio);
// If we need to check sanity, check it, in gears 2,3 and 4, RPM readings should be the same,
// otherwise we have a faulty conductor place sensor!
if (check_sanity && abs((int)dest->n2_raw - (int)dest->n3_raw) > 150) {
res = ESP_ERR_INVALID_STATE;
}
}
}
return res;
}
bool Sensors::read_vbatt(uint16_t *dest){
uint32_t v;
if (esp_adc_cal_get_voltage(adc_channel_t::ADC_CHANNEL_8, &adc2_cal_vbatt, &v) != ESP_OK) {
return false;
esp_err_t Sensors::read_output_rpm(uint16_t* dest) {
esp_err_t res;
if (output_rpm_ok == false) {
res = ESP_ERR_INVALID_STATE;
} else {
// Vin = Vout(R1+R2)/R2
*dest = v*5.54; // 5.54 = (100+22)/22
return true;
uint64_t output[2];
portENTER_CRITICAL(&output_mux);
output[0] = output_intr_times[0];
output[1] = output_intr_times[1];
portEXIT_CRITICAL(&output_mux);
uint64_t delta = output[1]-output[0];
uint64_t now = esp_timer_get_time();
if (delta == 0 || now - output[1] > 20000) {
*dest = 0; // Too slow for accurate measurement
} else {
*dest = ((1000000 / delta) / VEHICLE_CONFIG.input_sensor_pulses_per_rev);
}
}
return res;
}
esp_err_t Sensors::read_vbatt(uint16_t *dest){
uint32_t v;
esp_err_t res = esp_adc_cal_get_voltage(pcb_gpio_matrix->sensor_data.adc_batt, &adc2_cal, &v);
if (res == ESP_OK) {
// Vin = Vout(R1+R2)/R2
*dest = v * 5.54; // 5.54 = (100+22)/22
}
return res;
}
// Returns ATF temp in *C
bool Sensors::read_atf_temp(int16_t* dest) {
uint32_t avg = 0;
#ifdef BOARD_V2
static const float ATF_TEMP_CORR = 1.0;
#else
uint32_t raw = 0;
static const float ATF_TEMP_CORR = 0.8;
#endif
#ifdef BOARD_V2
esp_err_t res = esp_adc_cal_get_voltage(adc_channel_t::ADC_CHANNEL_7, &adc2_cal_atf, &avg);
if (res != ESP_OK) {
return false;
esp_err_t Sensors::read_atf_temp(int16_t *dest)
{
esp_err_t res;
uint32_t avg = 0;
float ATF_TEMP_CORR = BOARD_CONFIG.board_ver >= 2 ? 1.0 : 0.8;
if (BOARD_CONFIG.board_ver >= 2)
{
res = esp_adc_cal_get_voltage(adc_channel_t::ADC_CHANNEL_7, &adc2_cal, &avg);
}
#else
#define NUM_ATF_SAMPLES 5
for (uint8_t i = 0; i < NUM_ATF_SAMPLES; i++) {
esp_err_t res = esp_adc_cal_get_voltage(adc_channel_t::ADC_CHANNEL_9, &adc2_cal_atf, &raw);
if (res != ESP_OK) {
//ESP_LOG_LEVEL(ESP_LOG_WARN, "READ_ATF", "Failed to query ATF temp. %s", esp_err_to_name(res));
return false;
else
{
res = esp_adc_cal_get_voltage(adc_channel_t::ADC_CHANNEL_9, &adc2_cal, &avg);
}
if (avg >= 3000)
{
res = ESP_ERR_INVALID_STATE; // Parking lock engaged, cannot read.
}
if (res == ESP_OK) {
const temp_reading_t *atf_temp_lookup = pcb_gpio_matrix->sensor_data.atf_calibration_curve;
if (avg <= atf_temp_lookup[0].v)
{
*dest = (int16_t)((float)atf_temp_lookup[0].temp * ATF_TEMP_CORR);
}
avg += raw;
}
avg /= NUM_ATF_SAMPLES;
#endif
if (avg >= 3000) {
return false; // Parking lock engaged, cannot read.
}
if (avg <= atf_temp_lookup[0].v) {
*dest = (int16_t)((float)atf_temp_lookup[0].temp * ATF_TEMP_CORR);
return true;
} else if (avg >= atf_temp_lookup[NUM_TEMP_POINTS-1].v) {
*dest = (int16_t)(ATF_TEMP_CORR * (float)(atf_temp_lookup[NUM_TEMP_POINTS-1].temp) / 10.0);
return true;
} else {
for (uint8_t i = 0; i < NUM_TEMP_POINTS-1; i++) {
// Found! Interpolate linearly to get a better estimate of ATF Temp
if (atf_temp_lookup[i].v <= avg && atf_temp_lookup[i+1].v >= avg) {
float dx = avg - atf_temp_lookup[i].v;
float dy = atf_temp_lookup[i+1].v - atf_temp_lookup[i].v;
*dest = (int16_t)(ATF_TEMP_CORR * (atf_temp_lookup[i].temp + (atf_temp_lookup[i+1].temp-atf_temp_lookup[i].temp) * ((dx)/dy)) / 10.0);
return true;
else if (avg >= atf_temp_lookup[NUM_TEMP_POINTS - 1].v)
{
*dest = (int16_t)(ATF_TEMP_CORR * (float)(atf_temp_lookup[NUM_TEMP_POINTS - 1].temp) / 10.0);
}
else
{
for (uint8_t i = 0; i < NUM_TEMP_POINTS - 1; i++)
{
// Found! Interpolate linearly to get a better estimate of ATF Temp
if (atf_temp_lookup[i].v <= avg && atf_temp_lookup[i + 1].v >= avg)
{
float dx = avg - atf_temp_lookup[i].v;
float dy = atf_temp_lookup[i + 1].v - atf_temp_lookup[i].v;
*dest = (int16_t)(ATF_TEMP_CORR * (atf_temp_lookup[i].temp + (atf_temp_lookup[i + 1].temp - atf_temp_lookup[i].temp) * ((dx) / dy)) / 10.0);
break;
}
}
}
return true;
}
return res;
}
bool Sensors::parking_lock_engaged(bool* dest){
esp_err_t Sensors::parking_lock_engaged(bool *dest)
{
uint32_t raw;
#ifdef BOARD_V2
esp_err_t res = esp_adc_cal_get_voltage(adc_channel_t::ADC_CHANNEL_7, &adc2_cal_atf, &raw);
#else
esp_err_t res = esp_adc_cal_get_voltage(adc_channel_t::ADC_CHANNEL_9, &adc2_cal_atf, &raw);
#endif
if (res != ESP_OK) {
return false;
} else {
esp_err_t res = esp_adc_cal_get_voltage(pcb_gpio_matrix->sensor_data.adc_atf, &adc2_cal, &raw);
if (res == ESP_OK)
{
*dest = raw >= 3000;
return true;
}
return res;
}

39
src/sensors.h
View File

@ -1,21 +1,22 @@
/** @file */
#ifndef __SENSORS_H_
#define __SENSORS_H_
#ifndef SENSORS_H
#define SENSORS_H
#include <stdint.h>
#include "esp_err.h"
/**
* @brief RPM Reading structure
*
*/
typedef struct {
struct RpmReading{
/// N2 Raw Pulse count
uint32_t n2_raw;
/// N3 Raw pulse count
uint32_t n3_raw;
/// Calculated input RPM
uint32_t calc_rpm;
} RpmReading;
};
namespace Sensors {
/**
@ -24,7 +25,7 @@ namespace Sensors {
* @return true If initialization OK
* @return false if initialization failed
*/
bool init_sensors();
esp_err_t init_sensors(void);
/**
* @brief Reads the input RPM of the gearbox
@ -36,7 +37,16 @@ namespace Sensors {
* @return true If sensor reading OK
* @return false If sensor reading failed, or if sanity check failed
*/
bool read_input_rpm(RpmReading* dest, bool check_sanity);
esp_err_t read_input_rpm(RpmReading* dest, bool check_sanity);
/**
* @brief ONLY FOR BOARD V1.3+! Reads output RPM of the gearbox when the GPIO pin is configured for RPM reading
*
* @param dest Destination to store the output RPM
* @return true RPM Reading OK
* @return false Error reading RPM
*/
esp_err_t read_output_rpm(uint16_t* dest);
/**
* @brief Reads the PCB supply voltage pin in mV
@ -45,9 +55,18 @@ namespace Sensors {
* @return true
* @return false
*/
bool read_vbatt(uint16_t* dest);
bool read_atf_temp(int16_t* dest);
bool parking_lock_engaged(bool* dest);
esp_err_t read_vbatt(uint16_t* dest);
/**
* @brief Reads the ATF temp from the TFT sensor in degrees C
*
* @param dest
* @return ESP_OK - ATF temp reading OK
* ESP_ERR_INVALID_STATE - Parking lock engaged
*/
esp_err_t read_atf_temp(int16_t* dest);
esp_err_t parking_lock_engaged(bool* dest);
}
#endif // __SENSORS_H_
#endif // SENSORS_H

16
src/solenoids/constant_current.cpp
View File

@ -55,16 +55,12 @@ void ConstantCurrentDriver::update() {
uint16_t actual_current = this->solenoid->get_current_avg();
float error = 0; // Current error (reading vs target)
// Solenoid was commanded on but hasn't activated yet, or req current is too small to measure
if (this->current_target == 0 || actual_current == 0){ //(now-this->last_change_time) < 50) {
// if (this->current_target == 0 || actual_current == 0){ //(now-this->last_change_time) < 50) {
if (0u == actual_current){ //(now-this->last_change_time) < 50) {
error = 0;
} else {
error = (float)(this->current_target-actual_current)/((float)12000.0/calc_resistance);
error = (float)(this->current_target-actual_current)/(12000.0F/calc_resistance);
error *= this->current_target/((float)12000.0/calc_resistance);
//if (error > 0.05) {
// error = 0.05;
//} else if (error < -0.05) {
// error = -0.05;
//}
}
//ESP_LOGI("CC", "SOL %s, E %.2f, ADJ %.2f", this->name, error, this->pwm_adjustment_percent);
if (abs(this->current_target-actual_current) > 10 && error != 0) {
@ -82,6 +78,9 @@ void ConstantCurrentDriver::update() {
}
void constant_current_driver_thread(void*) {
if (!Solenoids::init_routine_completed()) {
vTaskDelay(1);
}
while(true) {
mpc_cc->update();
spc_cc->update();
@ -94,13 +93,12 @@ void constant_current_driver_thread(void*) {
}
namespace CurrentDriver {
bool init_current_driver() {
void init_current_driver() {
mpc_cc = new ConstantCurrentDriver(sol_mpc, "MPC");
spc_cc = new ConstantCurrentDriver(sol_spc, "SPC");
mpc_cc->set_target_current(0);
spc_cc->set_target_current(0);
xTaskCreate(constant_current_driver_thread, "CCDriver", 4096, nullptr, 3, nullptr);
return true;
}
}

Some files were not shown because too many files have changed in this diff Show More