Compare commits
10 Commits
12ee18adee
...
Firmware_1
Author | SHA1 | Date | |
---|---|---|---|
2b588b3be2 | |||
3a6c102b45 | |||
c6d65f50bf | |||
3e69485696 | |||
ec9a75e472 | |||
1966705f7f | |||
9cb3a61184 | |||
f735ea7b0d | |||
05f476bae2 | |||
c998cce1a8 |
@@ -1,8 +0,0 @@
|
||||
import subprocess
|
||||
|
||||
revision = (
|
||||
subprocess.check_output(["git", "rev-parse", "--short=10", "HEAD"])
|
||||
.strip()
|
||||
.decode("utf-8")
|
||||
)
|
||||
print("-DGIT_REV='\"%s\"'" % revision)
|
@@ -1,9 +1,87 @@
|
||||
Import("env") # pylint: disable=undefined-variable
|
||||
env.Execute("\"$PYTHONEXE\" -m pip install jinja2")
|
||||
env.Replace(PROGNAME="firmware_pcb_1.%s.fw" % env.GetProjectOption("custom_pcb_revision"))
|
||||
# run_pre.py — PlatformIO pre-build script
|
||||
import os
|
||||
import subprocess
|
||||
from pathlib import Path
|
||||
|
||||
Import("env") # provided by PlatformIO
|
||||
|
||||
# ---- helper ----
|
||||
def parse_ver(s: str):
|
||||
"""
|
||||
Accepts '1.04', '1.4', '1,04' etc. -> returns (major:int, minor:int, norm_str:'1.04')
|
||||
"""
|
||||
s = (s or "").strip().replace(",", ".")
|
||||
if not s:
|
||||
return 0, 0, "0.00"
|
||||
parts = s.split(".")
|
||||
try:
|
||||
major = int(parts[0])
|
||||
minor = int(parts[1]) if len(parts) > 1 else 0
|
||||
except ValueError:
|
||||
major, minor = 0, 0
|
||||
norm_str = f"{major}.{minor:02d}"
|
||||
return major, minor, norm_str
|
||||
|
||||
def read_text_file(p: Path):
|
||||
try:
|
||||
return p.read_text(encoding="utf-8").strip()
|
||||
except Exception:
|
||||
return ""
|
||||
|
||||
def git_short_hash():
|
||||
try:
|
||||
out = subprocess.check_output(
|
||||
["git", "rev-parse", "--short", "HEAD"],
|
||||
stderr=subprocess.DEVNULL
|
||||
).decode("utf-8").strip()
|
||||
return out or "nogit"
|
||||
except Exception:
|
||||
return "nogit"
|
||||
|
||||
# ---- ensure jinja present, like before ----
|
||||
env.Execute("\"$PYTHONEXE\" -m pip install jinja2")
|
||||
|
||||
# ---- keep your other pre-steps ----
|
||||
import struct2json
|
||||
import dtcs
|
||||
|
||||
struct2json.struct2json()
|
||||
dtcs.build_dtcs()
|
||||
dtcs.build_dtcs()
|
||||
|
||||
# ---- collect inputs ----
|
||||
proj_dir = Path(env["PROJECT_DIR"])
|
||||
|
||||
# user options from platformio.ini
|
||||
pcb_rev = env.GetProjectOption("custom_pcb_revision", default="")
|
||||
fw_ver_opt = env.GetProjectOption("custom_firmware_version", default="") # new
|
||||
|
||||
# required flash version from data/version
|
||||
req_file = proj_dir / "data" / "version"
|
||||
req_ver_raw = read_text_file(req_file)
|
||||
|
||||
fw_major, fw_minor, fw_norm = parse_ver(fw_ver_opt)
|
||||
req_major, req_minor, req_norm = parse_ver(req_ver_raw)
|
||||
|
||||
githash = git_short_hash()
|
||||
|
||||
# ---- export as preprocessor defines ----
|
||||
# numeric defines
|
||||
env.Append(CPPDEFINES=[
|
||||
("FW_VERSION_MAJOR", fw_major),
|
||||
("FW_VERSION_MINOR", fw_minor),
|
||||
("REQ_FLASH_MAJOR", req_major),
|
||||
("REQ_FLASH_MINOR", req_minor),
|
||||
])
|
||||
|
||||
# useful string defines (if du sie im Code/Logging brauchst)
|
||||
env.Append(CPPDEFINES=[
|
||||
("FW_VERSION_STR", f"\"{fw_norm}\""),
|
||||
("REQ_FLASH_STR", f"\"{req_norm}\""),
|
||||
("GIT_REV", f"\"{githash}\""),
|
||||
])
|
||||
|
||||
# ---- build artifact name ----
|
||||
# bisher: firmware_pcb_1.<pcb>.fw
|
||||
# jetzt: firmware_pcb_<pcb>_v<fw>_<git>.fw (gut identifizierbar)
|
||||
pcb_part = f"{pcb_rev}".strip() or "X"
|
||||
fname = f"firmware_pcb_{pcb_part}_v{fw_norm}_{githash}.fw"
|
||||
env.Replace(PROGNAME=fname)
|
||||
|
@@ -1,37 +0,0 @@
|
||||
/**
|
||||
* @file can.h
|
||||
*
|
||||
* @brief Header file for Controller Area Network (CAN) functionality in the ChainLube application.
|
||||
*
|
||||
* This file provides functions and structures related to Controller Area Network (CAN)
|
||||
* communication for the ChainLube project. It includes functions for initializing CAN,
|
||||
* processing CAN messages, and retrieving wheel speed from CAN data.
|
||||
*
|
||||
* @author Marcel Peterkau
|
||||
* @date 09.01.2024
|
||||
*/
|
||||
|
||||
#ifndef _CAN_H_
|
||||
#define _CAN_H_
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <mcp_can.h>
|
||||
#include <SPI.h>
|
||||
#include "common.h"
|
||||
#include "globals.h"
|
||||
#include "dtc.h"
|
||||
#include "debugger.h"
|
||||
|
||||
// CAN frame structure definition
|
||||
struct can_frame
|
||||
{
|
||||
unsigned long can_id;
|
||||
uint8_t can_dlc;
|
||||
uint8_t data[8] __attribute__((aligned(8)));
|
||||
};
|
||||
|
||||
// Function prototypes
|
||||
void Init_CAN();
|
||||
uint32_t Process_CAN_WheelSpeed();
|
||||
|
||||
#endif
|
59
Software/include/can_hal.h
Normal file
59
Software/include/can_hal.h
Normal file
@@ -0,0 +1,59 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
#include <mcp_can.h>
|
||||
#include "common.h"
|
||||
|
||||
// ==== Board-Pin ====
|
||||
// Falls nicht bereits global definiert:
|
||||
#ifndef GPIO_CS_CAN
|
||||
#define GPIO_CS_CAN 5
|
||||
#endif
|
||||
|
||||
// ==== Öffentliche, einzige Instanz ====
|
||||
extern MCP_CAN CAN0;
|
||||
|
||||
// ==== Init-Config ====
|
||||
struct CanHalConfig {
|
||||
uint8_t baud = CAN_500KBPS; // laut Lib
|
||||
uint8_t clock = MCP_16MHZ; // 8/16 MHz je nach Quarz
|
||||
uint16_t listenOnlyProbeMs = 0; // optionaler kurzer Hörtest
|
||||
uint16_t modeSettleMs = 10; // Wartezeit für Mode-Set (Retry-Fenster)
|
||||
};
|
||||
|
||||
// ==== Universeller Filter-Descriptor ====
|
||||
struct CanFilter {
|
||||
uint32_t id; // 11-bit oder 29-bit Roh-ID
|
||||
bool ext; // false=STD(11-bit), true=EXT(29-bit)
|
||||
};
|
||||
|
||||
// ==== API ====
|
||||
|
||||
// 1) Einmalige Hardware-Initialisierung + integrierter Loopback-Selftest.
|
||||
// - begin()
|
||||
// - LOOPBACK senden/echo prüfen (ohne Bus)
|
||||
// - optional: ListenOnly-Probe (nur Heuristik)
|
||||
// - Default: Filter/Masks weit offen, NORMAL-Mode
|
||||
// Rückgabe: true = bereit; false = Fehler (kein CAN verwenden)
|
||||
bool CAN_HAL_Init(const CanHalConfig& cfg);
|
||||
|
||||
// Ist die HAL bereit (nach Init)?
|
||||
bool CAN_HAL_IsReady();
|
||||
|
||||
// Bestätigter Moduswechsel (CONFIG/NORMAL/LISTENONLY/LOOPBACK)
|
||||
// true = erfolgreich; setzt bei Misserfolg DTC_CAN_TRANSCEIVER_FAILED
|
||||
bool CAN_HAL_SetMode(uint8_t mode);
|
||||
|
||||
// Masken/Filter
|
||||
bool CAN_HAL_SetMask(uint8_t bank, bool ext, uint32_t rawMask);
|
||||
bool CAN_HAL_SetStdMask11(uint8_t bank, uint16_t mask11);
|
||||
void CAN_HAL_ClearFilters();
|
||||
bool CAN_HAL_AddFilter(const CanFilter& f);
|
||||
bool CAN_HAL_SetFilters(const CanFilter* list, size_t count);
|
||||
|
||||
// Non-blocking IO
|
||||
bool CAN_HAL_Read(unsigned long& id, uint8_t& len, uint8_t data[8]); // true=Frame gelesen
|
||||
uint8_t CAN_HAL_Send(unsigned long id, bool ext, uint8_t len, const uint8_t* data); // CAN_OK bei Erfolg
|
||||
|
||||
// Diagnose/Utilities (nutzen MCP-APIs)
|
||||
uint8_t CAN_HAL_GetErrorFlags(); // Intern: getError()
|
||||
void CAN_HAL_GetErrorCounters(uint8_t& tec, uint8_t& rec); // TX/RX Error Counter
|
11
Software/include/can_native.h
Normal file
11
Software/include/can_native.h
Normal file
@@ -0,0 +1,11 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
#include "can_hal.h"
|
||||
|
||||
// Initialisiert den Native-CAN-Profilpfad (setzt Masken/Filter und NORMAL-Mode).
|
||||
// Voraussetzung: CAN_HAL_Init(...) hat zuvor true geliefert.
|
||||
bool Init_CAN_Native();
|
||||
|
||||
// Liest Frames non-blocking, extrahiert Hinterradgeschwindigkeit je nach Bike,
|
||||
// integriert mm über dt und liefert die seit letztem Aufruf addierten Millimeter.
|
||||
uint32_t Process_CAN_Native_WheelSpeed();
|
16
Software/include/can_obd2.h
Normal file
16
Software/include/can_obd2.h
Normal file
@@ -0,0 +1,16 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
#include "can_hal.h"
|
||||
|
||||
// Initialisiert das OBD2-CAN-Profil:
|
||||
// - setzt Masken/Filter für 0x7E8..0x7EF (ECU-Antworten)
|
||||
// - Normal-Mode sicherstellen
|
||||
// Voraussetzung: CAN_HAL_Init(...) hat zuvor true geliefert.
|
||||
bool Init_CAN_OBD2();
|
||||
|
||||
// Polling-Prozess für OBD2 über CAN (non-blocking):
|
||||
// - sendet zyklisch Requests (0x7DF) auf PID 0x0D (Fahrzeuggeschwindigkeit)
|
||||
// - verarbeitet Antworten 0x7E8..0x7EF
|
||||
// - integriert Millimeter über dt
|
||||
// Rückgabe: seit letztem Aufruf addierte Millimeter (uint32_t)
|
||||
uint32_t Process_CAN_OBD2_Speed();
|
@@ -68,6 +68,7 @@
|
||||
// -> 6.90µl / Pulse
|
||||
#define DEFAULT_PUMP_DOSE 7
|
||||
|
||||
// --- System status enum with sentinel ---
|
||||
typedef enum eSystem_Status
|
||||
{
|
||||
sysStat_Startup,
|
||||
@@ -76,7 +77,8 @@ typedef enum eSystem_Status
|
||||
sysStat_Wash,
|
||||
sysStat_Purge,
|
||||
sysStat_Error,
|
||||
sysStat_Shutdown
|
||||
sysStat_Shutdown,
|
||||
SYSSTAT_COUNT // <- sentinel (must be last)
|
||||
} tSystem_Status;
|
||||
|
||||
// Enum for different sources of speed input
|
||||
@@ -89,13 +91,10 @@ typedef enum SpeedSource_e
|
||||
SOURCE_GPS,
|
||||
SOURCE_CAN,
|
||||
SOURCE_OBD2_KLINE,
|
||||
SOURCE_OBD2_CAN
|
||||
SOURCE_OBD2_CAN,
|
||||
SPEEDSOURCE_COUNT // <- sentinel (must be last)
|
||||
} SpeedSource_t;
|
||||
|
||||
// String representation of SpeedSource enum
|
||||
extern const char *SpeedSourceString[];
|
||||
extern const size_t SpeedSourceString_Elements;
|
||||
|
||||
// Enum for GPS baud rates
|
||||
typedef enum GPSBaudRate_e
|
||||
{
|
||||
@@ -104,23 +103,29 @@ typedef enum GPSBaudRate_e
|
||||
BAUD_19200,
|
||||
BAUD_38400,
|
||||
BAUD_57600,
|
||||
BAUD_115200
|
||||
BAUD_115200,
|
||||
GPSBAUDRATE_COUNT // <- sentinel (must be last)
|
||||
} GPSBaudRate_t;
|
||||
|
||||
// String representation of GPSBaudRate enum
|
||||
extern const char *GPSBaudRateString[];
|
||||
extern const size_t GPSBaudRateString_Elements;
|
||||
|
||||
// Enum for CAN bus sources
|
||||
typedef enum CANSource_e
|
||||
{
|
||||
KTM_890_ADV_R_2021,
|
||||
KTM_1290_SD_R_2023
|
||||
KTM_1290_SD_R_2023,
|
||||
CANSOURCE_COUNT // <- sentinel (must be last)
|
||||
} CANSource_t;
|
||||
|
||||
// String representation of CANSource enum
|
||||
extern const char *CANSourceString[];
|
||||
extern const size_t CANSourceString_Elements;
|
||||
// String tables (kept internal to the module)
|
||||
extern const char * const SystemStatusString[SYSSTAT_COUNT];
|
||||
extern const char * const SpeedSourceString[SPEEDSOURCE_COUNT];
|
||||
extern const char * const GPSBaudRateString[GPSBAUDRATE_COUNT];
|
||||
extern const char * const CANSourceString[CANSOURCE_COUNT];
|
||||
|
||||
// Safe getters (centralized bounds check)
|
||||
const char* ToString(SpeedSource_t v);
|
||||
const char* ToString(GPSBaudRate_t v);
|
||||
const char* ToString(CANSource_t v);
|
||||
const char* ToString(tSystem_Status v);
|
||||
|
||||
#define STARTUP_DELAY 2500
|
||||
#define SHUTDOWN_DELAY_MS 2500
|
||||
|
@@ -1,27 +1,26 @@
|
||||
/**
|
||||
* @file config.h
|
||||
* @brief Configuration structures and EEPROM API for ChainLube firmware.
|
||||
*
|
||||
* @brief Header file for configuration settings and EEPROM operations in the ChainLube application.
|
||||
* Defines EEPROM layout versions, configuration and persistence data structures,
|
||||
* and the public functions for storing, loading, formatting and validating
|
||||
* configuration/persistence records.
|
||||
*
|
||||
* This file defines configuration settings for the ChainLube project, including default values,
|
||||
* EEPROM structures, and functions for EEPROM operations. It also defines enums for different sources
|
||||
* of speed input, GPS baud rates, and CAN bus sources. Additionally, it includes functions for EEPROM handling
|
||||
* such as storing, retrieving, and formatting configuration data.
|
||||
*
|
||||
* @author Marcel Peterkau
|
||||
* @date 09.01.2024
|
||||
* Notes:
|
||||
* - The system always boots with defaults in RAM; EEPROM is optional.
|
||||
* - DTC handling for EEPROM availability and integrity is centralized in EEPROM_Process().
|
||||
*/
|
||||
|
||||
#ifndef _CONFIG_H_
|
||||
#define _CONFIG_H_
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <Wire.h>
|
||||
#include <stdint.h>
|
||||
#include <I2C_eeprom.h>
|
||||
#include "dtc.h"
|
||||
#include "common.h"
|
||||
|
||||
#define EEPROM_STRUCTURE_REVISION 4 // Increment this version when changing EEPROM structures
|
||||
// Increment when EEPROM structure changes
|
||||
#define EEPROM_STRUCTURE_REVISION 4
|
||||
|
||||
#if PCB_REV == 1 || PCB_REV == 2 || PCB_REV == 3
|
||||
#define EEPROM_SIZE_BYTES I2C_DEVICESIZE_24LC64
|
||||
@@ -29,9 +28,14 @@
|
||||
#define EEPROM_SIZE_BYTES I2C_DEVICESIZE_24LC256
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief EEPROM request state machine codes.
|
||||
*
|
||||
* Used by globals.requestEEAction to schedule EEPROM operations.
|
||||
*/
|
||||
typedef enum EERequest_e
|
||||
{
|
||||
EE_IDLE,
|
||||
EE_IDLE = 0,
|
||||
EE_CFG_SAVE,
|
||||
EE_CFG_LOAD,
|
||||
EE_CFG_FORMAT,
|
||||
@@ -39,11 +43,13 @@ typedef enum EERequest_e
|
||||
EE_PDS_LOAD,
|
||||
EE_PDS_FORMAT,
|
||||
EE_FORMAT_ALL,
|
||||
EE_ALL_SAVE
|
||||
|
||||
EE_ALL_SAVE,
|
||||
EE_REINITIALIZE
|
||||
} EERequest_t;
|
||||
|
||||
// Structure for persistence data stored in EEPROM
|
||||
/**
|
||||
* @brief Wear-levelled persistence data block.
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
uint16_t writeCycleCounter;
|
||||
@@ -54,7 +60,9 @@ typedef struct
|
||||
uint32_t checksum;
|
||||
} persistenceData_t;
|
||||
|
||||
// Structure for configuration settings stored in EEPROM
|
||||
/**
|
||||
* @brief User configuration stored in EEPROM.
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
uint8_t EEPROM_Version;
|
||||
@@ -85,7 +93,9 @@ typedef struct
|
||||
uint32_t checksum;
|
||||
} LubeConfig_t;
|
||||
|
||||
// Default configuration settings
|
||||
/**
|
||||
* @brief Factory defaults for configuration (in RAM).
|
||||
*/
|
||||
const LubeConfig_t LubeConfig_defaults = {
|
||||
0, 8000, 4000, 320, DEFAULT_PUMP_DOSE, 30, 1, 150, 70, 18, 2000, 25, 500, 10, SOURCE_IMPULSE,
|
||||
BAUD_115200,
|
||||
@@ -100,21 +110,31 @@ const LubeConfig_t LubeConfig_defaults = {
|
||||
true,
|
||||
0};
|
||||
|
||||
/* ==== Public API ==== */
|
||||
|
||||
// Initialization & main process
|
||||
void InitEEPROM();
|
||||
void EEPROM_Process();
|
||||
|
||||
// Config & persistence access
|
||||
void StoreConfig_EEPROM();
|
||||
void GetConfig_EEPROM();
|
||||
void StorePersistence_EEPROM();
|
||||
void GetPersistence_EEPROM();
|
||||
void FormatConfig_EEPROM();
|
||||
void FormatPersistence_EEPROM();
|
||||
void MovePersistencePage_EEPROM(boolean reset);
|
||||
|
||||
// Utilities
|
||||
uint32_t Checksum_EEPROM(uint8_t const *data, size_t len);
|
||||
void dumpEEPROM(uint16_t memoryAddress, uint16_t length);
|
||||
void MovePersistencePage_EEPROM(boolean reset);
|
||||
uint32_t ConfigSanityCheck(bool autocorrect = false);
|
||||
bool validateWiFiString(char *string, size_t size);
|
||||
|
||||
/* ==== Externals ==== */
|
||||
|
||||
extern LubeConfig_t LubeConfig;
|
||||
extern persistenceData_t PersistenceData;
|
||||
extern uint16_t eePersistenceMarker;
|
||||
extern uint16_t eePersistenceAddress;
|
||||
|
||||
#endif // _CONFIG_H_
|
||||
|
@@ -18,39 +18,63 @@
|
||||
#include "config.h"
|
||||
#include "common.h"
|
||||
|
||||
#ifndef FW_VERSION_MAJOR
|
||||
#define FW_VERSION_MAJOR 0
|
||||
#endif
|
||||
#ifndef FW_VERSION_MINOR
|
||||
#define FW_VERSION_MINOR 0
|
||||
#endif
|
||||
|
||||
#ifndef REQ_FLASH_MAJOR
|
||||
#define REQ_FLASH_MAJOR 0
|
||||
#endif
|
||||
#ifndef REQ_FLASH_MINOR
|
||||
#define REQ_FLASH_MINOR 0
|
||||
#endif
|
||||
|
||||
#ifndef GIT_REV
|
||||
#define GIT_REV "nogit"
|
||||
#endif
|
||||
|
||||
#ifndef FW_VERSION_STR
|
||||
#define FW_VERSION_STR "0.00"
|
||||
#endif
|
||||
#ifndef REQ_FLASH_STR
|
||||
#define REQ_FLASH_STR "0.00"
|
||||
#endif
|
||||
|
||||
typedef struct Globals_s
|
||||
{
|
||||
tSystem_Status systemStatus = sysStat_Startup; /**< Current system status */
|
||||
tSystem_Status resumeStatus = sysStat_Startup; /**< Status to resume after rain mode */
|
||||
char systemStatustxt[16] = ""; /**< Text representation of system status */
|
||||
uint16_t purgePulses = 0; /**< Number of purge pulses */
|
||||
EERequest_t requestEEAction = EE_IDLE; /**< EEPROM-related request */
|
||||
char DeviceName[33]; /**< Device name */
|
||||
char FlashVersion[10]; /**< Flash version */
|
||||
uint16_t eePersistanceAdress; /**< EEPROM persistence address */
|
||||
uint8_t TankPercentage; /**< Tank percentage */
|
||||
bool hasDTC; /**< Flag indicating the presence of Diagnostic Trouble Codes (DTC) */
|
||||
bool measurementActive; /**< Flag indicating active measurement */
|
||||
uint32_t measuredPulses; /**< Number of measured pulses */
|
||||
tSystem_Status systemStatus = sysStat_Startup; /**< Current system status */
|
||||
tSystem_Status resumeStatus = sysStat_Startup; /**< Status to resume after rain mode */
|
||||
uint16_t purgePulses = 0; /**< Number of purge pulses */
|
||||
EERequest_t requestEEAction = EE_IDLE; /**< EEPROM-related request */
|
||||
char DeviceName[33]; /**< Device name */
|
||||
char FlashVersion[10]; /**< Flash version */
|
||||
uint16_t eePersistenceAddress; /**< EEPROM persistence address */
|
||||
uint8_t TankPercentage; /**< Tank percentage */
|
||||
bool hasDTC; /**< Flag indicating the presence of Diagnostic Trouble Codes (DTC) */
|
||||
bool measurementActive; /**< Flag indicating active measurement */
|
||||
uint32_t measuredPulses; /**< Number of measured pulses */
|
||||
bool toggle_wifi;
|
||||
uint16_t isr_debug;
|
||||
} Globals_t;
|
||||
|
||||
extern Globals_t globals; /**< Global variable struct */
|
||||
extern Globals_t globals; /**< Global variable struct */
|
||||
|
||||
typedef struct Constants_s
|
||||
{
|
||||
uint8_t FW_Version_major; /**< Firmware version major number */
|
||||
uint8_t FW_Version_minor; /**< Firmware version minor number */
|
||||
uint8_t FW_Version_major; /**< Firmware version major number */
|
||||
uint8_t FW_Version_minor; /**< Firmware version minor number */
|
||||
uint8_t Required_Flash_Version_major; /**< Required flash version major number */
|
||||
uint8_t Required_Flash_Version_minor; /**< Required flash version minor number */
|
||||
char GitHash[11]; /**< Git hash string */
|
||||
char GitHash[11]; /**< Git hash string */
|
||||
} Constants_t;
|
||||
|
||||
const Constants_t constants PROGMEM = {
|
||||
1,4, // Firmware_Version
|
||||
1,4, // Required Flash Version
|
||||
GIT_REV // Git-Hash-String
|
||||
FW_VERSION_MAJOR, FW_VERSION_MINOR, // Firmware_Version
|
||||
REQ_FLASH_MAJOR, REQ_FLASH_MINOR, // Required Flash Version
|
||||
QUOTE(GIT_REV) // Git-Hash-String
|
||||
};
|
||||
|
||||
/**
|
||||
|
@@ -1,10 +0,0 @@
|
||||
#ifndef _OBD2_CAN_H_
|
||||
#define _OBD2_CAN_H_
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
// === Funktionen ===
|
||||
void Init_OBD2_CAN();
|
||||
uint32_t Process_OBD2_CAN_Speed();
|
||||
|
||||
#endif
|
@@ -19,8 +19,9 @@ board = d1_mini
|
||||
framework = arduino
|
||||
upload_speed = 921600
|
||||
|
||||
custom_firmware_version = 1.05
|
||||
|
||||
build_flags =
|
||||
!python codegen/git_rev_macro.py
|
||||
-DWIFI_SSID_CLIENT=${wifi_cred.wifi_ssid_client}
|
||||
-DWIFI_PASSWORD_CLIENT=${wifi_cred.wifi_password_client}
|
||||
-DADMIN_PASSWORD=${wifi_cred.admin_password}
|
||||
|
@@ -1,184 +0,0 @@
|
||||
/**
|
||||
* @file can.cpp
|
||||
*
|
||||
* @brief Implementation file for CAN-related functions in the ChainLube application.
|
||||
*
|
||||
* This file contains the implementation of functions related to CAN (Controller Area Network)
|
||||
* communication within the ChainLube application. It includes the initialization of the CAN module,
|
||||
* setup of masks and filters, and processing of CAN messages. Additionally, a debug message function
|
||||
* is included if CAN debugging is enabled.
|
||||
*
|
||||
* @author Your Name
|
||||
* @date Date
|
||||
*/
|
||||
|
||||
#include "can.h"
|
||||
|
||||
MCP_CAN CAN0(GPIO_CS_CAN);
|
||||
#ifdef CAN_DEBUG_MESSAGE
|
||||
#define MAX_DEBUG_RETRIES 100
|
||||
void sendCANDebugMessage();
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Initializes the CAN module, sets masks, and filters based on the configured CAN source.
|
||||
*
|
||||
* This function initializes the CAN module, sets masks and filters based on the configured CAN source
|
||||
* in the application settings, and sets the CAN module in normal mode for communication.
|
||||
*/
|
||||
void Init_CAN()
|
||||
{
|
||||
if (CAN0.begin(MCP_STDEXT, CAN_500KBPS, MCP_16MHZ) != CAN_OK)
|
||||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||||
|
||||
CAN0.init_Mask(0, 0, 0x07FF0000); // Init first mask...
|
||||
CAN0.init_Mask(1, 0, 0x07FF0000); // Init second mask...
|
||||
|
||||
switch (LubeConfig.CANSource)
|
||||
{
|
||||
case KTM_890_ADV_R_2021:
|
||||
CAN0.init_Filt(0, 0, 0x012D0000); // Init first filter...
|
||||
break;
|
||||
case KTM_1290_SD_R_2023:
|
||||
CAN0.init_Filt(0, 0, 0x012D0000); // Init first filter...
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
CAN0.setMode(MCP_NORMAL);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Processes CAN messages to determine the wheel speed based on the configured CAN source.
|
||||
*
|
||||
* This function reads incoming CAN messages and extracts the rear wheel speed information.
|
||||
* The wheel speed is then converted to millimeters per second based on the configured CAN source.
|
||||
* The function also monitors the CAN signal for potential issues and triggers diagnostic trouble codes (DTCs).
|
||||
*
|
||||
* @return The calculated distance traveled in millimeters since the last call.
|
||||
*/
|
||||
uint32_t Process_CAN_WheelSpeed()
|
||||
{
|
||||
#define FACTOR_RWP_KMH_890ADV 18 // Divider to convert Raw Data to km/h
|
||||
#define FACTOR_RWP_KMH_1290SD 18 // Divider to convert Raw Data to km/h
|
||||
can_frame canMsg;
|
||||
static uint32_t lastRecTimestamp = 0;
|
||||
uint16_t RearWheelSpeed_raw;
|
||||
uint32_t milimeters_to_add = 0;
|
||||
uint32_t RWP_millimeter_per_second = 0;
|
||||
|
||||
if (CAN0.readMsgBuf(&canMsg.can_id, &canMsg.can_dlc, canMsg.data) == CAN_OK)
|
||||
{
|
||||
|
||||
switch (LubeConfig.CANSource)
|
||||
{
|
||||
case KTM_890_ADV_R_2021:
|
||||
// raw / FACTOR_RWP_KMH_890ADV -> km/h * 100000 -> cm/h / 3600 -> cm/s
|
||||
// raw * 500 -> cm/s
|
||||
RearWheelSpeed_raw = (uint16_t)canMsg.data[5] << 8 | canMsg.data[6];
|
||||
RWP_millimeter_per_second = (((uint32_t)RearWheelSpeed_raw * 1000000) / FACTOR_RWP_KMH_890ADV) / 3600;
|
||||
break;
|
||||
case KTM_1290_SD_R_2023:
|
||||
// raw / FACTOR_RWP_KMH_1290SD -> km/h * 100000 -> cm/h / 3600 -> cm/s
|
||||
// raw * 500 -> cm/s
|
||||
RearWheelSpeed_raw = (uint16_t)canMsg.data[5] << 8 | canMsg.data[6];
|
||||
RWP_millimeter_per_second = (((uint32_t)RearWheelSpeed_raw * 1000000) / FACTOR_RWP_KMH_1290SD) / 3600;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
uint32_t timesincelast = millis() - lastRecTimestamp;
|
||||
lastRecTimestamp = millis();
|
||||
|
||||
milimeters_to_add = (RWP_millimeter_per_second * timesincelast) / 1000;
|
||||
}
|
||||
|
||||
if (lastRecTimestamp > 1000)
|
||||
{
|
||||
MaintainDTC(DTC_NO_CAN_SIGNAL, (millis() > lastRecTimestamp + 10000 ? true : false));
|
||||
}
|
||||
|
||||
#ifdef CAN_DEBUG_MESSAGE
|
||||
static uint32_t previousMillis = 0;
|
||||
|
||||
if (millis() - previousMillis >= 1000)
|
||||
{
|
||||
sendCANDebugMessage();
|
||||
previousMillis = millis();
|
||||
}
|
||||
#endif
|
||||
|
||||
return milimeters_to_add;
|
||||
}
|
||||
|
||||
#ifdef CAN_DEBUG_MESSAGE
|
||||
/**
|
||||
* @brief Sends periodic CAN debug messages for monitoring and diagnostics.
|
||||
*
|
||||
* This function sends periodic CAN debug messages containing various system information for monitoring and diagnostics.
|
||||
* The information includes system status, timestamps, tank percentage, DTC flags, and other relevant data.
|
||||
* The debug messages are sent with a configurable multiplexer to broadcast different types of information in each cycle.
|
||||
*/
|
||||
void sendCANDebugMessage()
|
||||
{
|
||||
#define MAX_DEBUG_MULTIPLEXER 6
|
||||
static uint16_t DebugSendFailTimeout = 0;
|
||||
static uint8_t debugMultiplexer = 0;
|
||||
byte data[8] = {debugMultiplexer, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
|
||||
uint32_t millisValue = millis();
|
||||
|
||||
switch (debugMultiplexer)
|
||||
{
|
||||
case 0:
|
||||
memcpy(&data[1], &millisValue, sizeof(millisValue));
|
||||
memcpy(&data[5], &globals.purgePulses, sizeof(globals.purgePulses));
|
||||
break;
|
||||
case 1:
|
||||
data[1] = (uint8_t)globals.systemStatus;
|
||||
data[2] = (uint8_t)globals.resumeStatus;
|
||||
data[3] = (uint8_t)globals.requestEEAction;
|
||||
data[4] = globals.TankPercentage;
|
||||
data[5] = (0x01 & globals.hasDTC) | ((0x01 & globals.measurementActive) << 1);
|
||||
break;
|
||||
case 2:
|
||||
memcpy(&data[1], &globals.eePersistanceAdress, sizeof(globals.eePersistanceAdress));
|
||||
memcpy(&data[3], &PersistenceData.tankRemain_microL, sizeof(PersistenceData.tankRemain_microL));
|
||||
break;
|
||||
case 3:
|
||||
memcpy(&data[1], &PersistenceData.writeCycleCounter, sizeof(PersistenceData.writeCycleCounter));
|
||||
memcpy(&data[3], &PersistenceData.TravelDistance_highRes_mm, sizeof(PersistenceData.TravelDistance_highRes_mm));
|
||||
break;
|
||||
case 4:
|
||||
memcpy(&data[1], &PersistenceData.odometer_mm, sizeof(PersistenceData.odometer_mm));
|
||||
break;
|
||||
case 5:
|
||||
memcpy(&data[1], &PersistenceData.odometer, sizeof(PersistenceData.odometer));
|
||||
break;
|
||||
case 6:
|
||||
memcpy(&data[1], &PersistenceData.checksum, sizeof(PersistenceData.checksum));
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
debugMultiplexer++;
|
||||
debugMultiplexer = debugMultiplexer > MAX_DEBUG_MULTIPLEXER ? 0 : debugMultiplexer;
|
||||
|
||||
if (DebugSendFailTimeout < MAX_DEBUG_RETRIES)
|
||||
{
|
||||
byte sndStat = CAN0.sendMsgBuf(0x7FF, 0, 8, data);
|
||||
if (sndStat != CAN_OK)
|
||||
{
|
||||
Debug_pushMessage("failed sending CAN-DbgMsg: %d, %d\n", sndStat, DebugSendFailTimeout);
|
||||
DebugSendFailTimeout++;
|
||||
}
|
||||
}
|
||||
else if (DebugSendFailTimeout == MAX_DEBUG_RETRIES)
|
||||
{
|
||||
Debug_pushMessage("disable CAN-DbgMsg due to timeout\n");
|
||||
DebugSendFailTimeout++;
|
||||
}
|
||||
}
|
||||
#endif
|
240
Software/src/can_hal.cpp
Normal file
240
Software/src/can_hal.cpp
Normal file
@@ -0,0 +1,240 @@
|
||||
#include "can_hal.h"
|
||||
#include "dtc.h"
|
||||
// #include "debugger.h" // optional für Logs
|
||||
|
||||
// ==== Interner Zustand/Helper ====
|
||||
MCP_CAN CAN0(GPIO_CS_CAN);
|
||||
|
||||
static bool s_ready = false;
|
||||
static uint8_t s_nextFiltSlot = 0; // 0..5 (MCP2515 hat 6 Filter-Slots)
|
||||
static uint16_t s_modeSettleMs = 10; // Default aus Config
|
||||
|
||||
// 11-bit: Lib erwartet (value << 16)
|
||||
static inline uint32_t _std_to_hw(uint16_t v11) { return ((uint32_t)v11) << 16; }
|
||||
|
||||
// „Bestätigter“ Mode-Wechsel mithilfe der Lib-Funktion setMode(newMode)
|
||||
// Viele Forks nutzen intern mcp2515_requestNewMode(); wir retryen kurz.
|
||||
static bool _trySetMode(uint8_t mode, uint16_t settleMs)
|
||||
{
|
||||
const uint32_t t0 = millis();
|
||||
do {
|
||||
if (CAN0.setMode(mode) == CAN_OK) return true;
|
||||
delay(1);
|
||||
} while ((millis() - t0) < settleMs);
|
||||
return false;
|
||||
}
|
||||
|
||||
// LOOPBACK-Selftest (ohne Bus)
|
||||
static bool _selftest_loopback(uint16_t windowMs)
|
||||
{
|
||||
if (!_trySetMode(MCP_LOOPBACK, s_modeSettleMs)) return false;
|
||||
|
||||
const unsigned long tid = 0x123;
|
||||
uint8_t tx[8] = {0xA5, 0x5A, 0x11, 0x22, 0x33, 0x44, 0x77, 0x88};
|
||||
if (CAN0.sendMsgBuf(tid, 0, 8, tx) != CAN_OK) {
|
||||
(void)_trySetMode(MCP_NORMAL, s_modeSettleMs);
|
||||
return false;
|
||||
}
|
||||
|
||||
bool got = false;
|
||||
const uint32_t t0 = millis();
|
||||
while ((millis() - t0) < windowMs) {
|
||||
if (CAN0.checkReceive() == CAN_MSGAVAIL) {
|
||||
unsigned long rid; uint8_t len, rx[8];
|
||||
if (CAN0.readMsgBuf(&rid, &len, rx) == CAN_OK) {
|
||||
if (rid == tid && len == 8 && memcmp(tx, rx, 8) == 0) { got = true; break; }
|
||||
}
|
||||
}
|
||||
delay(1);
|
||||
}
|
||||
|
||||
(void)_trySetMode(MCP_NORMAL, s_modeSettleMs);
|
||||
return got;
|
||||
}
|
||||
|
||||
// Optional: kurzer ListenOnly-Hörtest (nur Heuristik, keine DTC-Änderung)
|
||||
static void _probe_listen_only(uint16_t ms)
|
||||
{
|
||||
if (ms == 0) return;
|
||||
if (!_trySetMode(MCP_LISTENONLY, s_modeSettleMs)) return;
|
||||
const uint32_t t0 = millis();
|
||||
while ((millis() - t0) < ms) {
|
||||
if (CAN0.checkReceive() == CAN_MSGAVAIL) break;
|
||||
delay(1);
|
||||
}
|
||||
(void)_trySetMode(MCP_NORMAL, s_modeSettleMs);
|
||||
}
|
||||
|
||||
// ==== Öffentliche API ====
|
||||
|
||||
bool CAN_HAL_Init(const CanHalConfig& cfg)
|
||||
{
|
||||
s_ready = false;
|
||||
s_modeSettleMs = cfg.modeSettleMs ? cfg.modeSettleMs : 10;
|
||||
|
||||
// 1) SPI/MCP starten
|
||||
// HIER: MCP_STDEXT statt MCP_STD, damit die Lib nicht ins default/Failure läuft
|
||||
if (CAN0.begin(MCP_STDEXT, cfg.baud, cfg.clock) != CAN_OK) {
|
||||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||||
return false;
|
||||
}
|
||||
|
||||
// 2) Loopback‑Selftest (ohne Bus)
|
||||
if (!_selftest_loopback(20)) {
|
||||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||||
return false;
|
||||
}
|
||||
|
||||
// 3) Optional Listen‑Only‑Probe (nur Info)
|
||||
_probe_listen_only(cfg.listenOnlyProbeMs);
|
||||
|
||||
// 4) Default: Filter/Masks neutral, Mode NORMAL
|
||||
// -> Für Masken/Filter müssen wir in CONFIG sein (hier: MODE_CONFIG laut deiner Lib)
|
||||
if (!_trySetMode(MODE_CONFIG, s_modeSettleMs)) {
|
||||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||||
return false;
|
||||
}
|
||||
|
||||
// weit offen (STD)
|
||||
CAN0.init_Mask(0, 0, _std_to_hw(0x000));
|
||||
CAN0.init_Mask(1, 0, _std_to_hw(0x000));
|
||||
for (uint8_t i = 0; i < 6; ++i) {
|
||||
CAN0.init_Filt(i, 0, _std_to_hw(0x000));
|
||||
}
|
||||
s_nextFiltSlot = 0;
|
||||
|
||||
if (!_trySetMode(MCP_NORMAL, s_modeSettleMs)) {
|
||||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||||
return false;
|
||||
}
|
||||
|
||||
// Erfolgreich
|
||||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, false);
|
||||
s_ready = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CAN_HAL_IsReady() { return s_ready; }
|
||||
|
||||
bool CAN_HAL_SetMode(uint8_t mode)
|
||||
{
|
||||
const bool ok = _trySetMode(mode, s_modeSettleMs);
|
||||
if (!ok) MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||||
return ok;
|
||||
}
|
||||
|
||||
bool CAN_HAL_SetMask(uint8_t bank, bool ext, uint32_t rawMask)
|
||||
{
|
||||
if (bank > 1) return false;
|
||||
if (!CAN_HAL_SetMode(MODE_CONFIG)) return false;
|
||||
|
||||
const bool ok = (CAN0.init_Mask(bank, ext ? 1 : 0, rawMask) == CAN_OK);
|
||||
|
||||
if (!CAN_HAL_SetMode(MCP_NORMAL)) {
|
||||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||||
return false;
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
|
||||
bool CAN_HAL_SetStdMask11(uint8_t bank, uint16_t mask11)
|
||||
{
|
||||
return CAN_HAL_SetMask(bank, false, _std_to_hw(mask11));
|
||||
}
|
||||
|
||||
void CAN_HAL_ClearFilters()
|
||||
{
|
||||
if (!CAN_HAL_SetMode(MODE_CONFIG)) {
|
||||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||||
return;
|
||||
}
|
||||
|
||||
CAN0.init_Mask(0, 0, _std_to_hw(0x000));
|
||||
CAN0.init_Mask(1, 0, _std_to_hw(0x000));
|
||||
for (uint8_t i = 0; i < 6; ++i) {
|
||||
CAN0.init_Filt(i, 0, _std_to_hw(0x000));
|
||||
}
|
||||
s_nextFiltSlot = 0;
|
||||
|
||||
if (!CAN_HAL_SetMode(MCP_NORMAL)) {
|
||||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||||
}
|
||||
}
|
||||
|
||||
bool CAN_HAL_AddFilter(const CanFilter& f)
|
||||
{
|
||||
if (s_nextFiltSlot >= 6) return false;
|
||||
if (!CAN_HAL_SetMode(MODE_CONFIG)) {
|
||||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||||
return false;
|
||||
}
|
||||
|
||||
const uint32_t hwId = f.ext ? f.id : _std_to_hw((uint16_t)f.id);
|
||||
const uint8_t slot = s_nextFiltSlot++;
|
||||
const bool ok = (CAN0.init_Filt(slot, f.ext ? 1 : 0, hwId) == CAN_OK);
|
||||
|
||||
if (!CAN_HAL_SetMode(MCP_NORMAL)) {
|
||||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||||
return false;
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
|
||||
bool CAN_HAL_SetFilters(const CanFilter* list, size_t count)
|
||||
{
|
||||
if (!CAN_HAL_SetMode(MODE_CONFIG)) {
|
||||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||||
return false;
|
||||
}
|
||||
|
||||
// Slots zurücksetzen
|
||||
s_nextFiltSlot = 0;
|
||||
for (uint8_t i = 0; i < 6; ++i) {
|
||||
CAN0.init_Filt(i, 0, _std_to_hw(0x000));
|
||||
}
|
||||
|
||||
// Setzen
|
||||
for (size_t i = 0; i < count && s_nextFiltSlot < 6; ++i) {
|
||||
const auto& f = list[i];
|
||||
const uint32_t hwId = f.ext ? f.id : _std_to_hw((uint16_t)f.id);
|
||||
CAN0.init_Filt(s_nextFiltSlot++, f.ext ? 1 : 0, hwId);
|
||||
}
|
||||
|
||||
if (!CAN_HAL_SetMode(MCP_NORMAL)) {
|
||||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CAN_HAL_Read(unsigned long& id, uint8_t& len, uint8_t data[8])
|
||||
{
|
||||
if (CAN0.checkReceive() != CAN_MSGAVAIL) return false;
|
||||
if (CAN0.readMsgBuf(&id, &len, data) != CAN_OK) {
|
||||
// Echte Lese-Fehler -> vermutlich SPI/Controller-Problem
|
||||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
uint8_t CAN_HAL_Send(unsigned long id, bool ext, uint8_t len, const uint8_t* data)
|
||||
{
|
||||
// Sende-Fehler (CAN_FAILTX) müssen nicht zwingend Transceiver-Defekte sein (z. B. Bus-Off).
|
||||
// Höhere Ebene kann bei Bedarf DTCs setzen. Hier nur durchreichen.
|
||||
return CAN0.sendMsgBuf(id, ext ? 1 : 0, len, const_cast<uint8_t*>(data));
|
||||
}
|
||||
|
||||
// ==== Diagnose/Utilities ====
|
||||
|
||||
uint8_t CAN_HAL_GetErrorFlags()
|
||||
{
|
||||
// getError() liefert MCP_EFLG Snapshot (Lib-abhängig)
|
||||
return CAN0.getError();
|
||||
}
|
||||
|
||||
void CAN_HAL_GetErrorCounters(uint8_t& tec, uint8_t& rec)
|
||||
{
|
||||
tec = CAN0.errorCountTX();
|
||||
rec = CAN0.errorCountRX();
|
||||
}
|
143
Software/src/can_native.cpp
Normal file
143
Software/src/can_native.cpp
Normal file
@@ -0,0 +1,143 @@
|
||||
#include "can_native.h"
|
||||
#include "globals.h" // für LubeConfig, etc.
|
||||
#include "dtc.h"
|
||||
#include "debugger.h"
|
||||
|
||||
// ===== Bike-spezifische Konstanten =====
|
||||
// Faktor zur Umrechnung der Rohdaten -> km/h (aus deinem bisherigen Code)
|
||||
static constexpr uint16_t FACTOR_RWP_KMH_890ADV = 18;
|
||||
static constexpr uint16_t FACTOR_RWP_KMH_1290SD = 18;
|
||||
|
||||
// Erwartete CAN-ID(s) für die genutzten Bikes (11-bit)
|
||||
static constexpr uint16_t ID_KTM_REAR_WHEEL = 0x12D; // aus deinem Filter-Setup
|
||||
|
||||
// ===== Interner Status =====
|
||||
static uint32_t s_lastIntegrateMs = 0;
|
||||
static uint32_t s_lastRxMs = 0; // für DTC_NO_CAN_SIGNAL
|
||||
static uint32_t s_lastSpeed_mmps = 0; // mm pro Sekunde (Rear Wheel)
|
||||
|
||||
// Hilfsfunktion: aus km/h -> mm/s
|
||||
static inline uint32_t kmh_to_mmps(uint16_t kmh)
|
||||
{
|
||||
// 1 km/h = 1'000'000 mm / 3600 s
|
||||
return (uint32_t)kmh * 1000000UL / 3600UL;
|
||||
}
|
||||
|
||||
// Hilfsfunktion: aus Rohdaten -> mm/s je nach Bike-Konfiguration
|
||||
static uint32_t parse_speed_mmps_from_frame(uint8_t dlc, const uint8_t data[8])
|
||||
{
|
||||
if (dlc < 7)
|
||||
return 0; // wir brauchen data[5] & data[6]
|
||||
uint16_t raw = (uint16_t)data[5] << 8 | data[6];
|
||||
|
||||
switch (LubeConfig.CANSource)
|
||||
{
|
||||
case KTM_890_ADV_R_2021:
|
||||
// (raw / FACTOR) km/h -> mm/s
|
||||
// Deine Kommentare: raw * 500 -> cm/s — hier sauber über kmh_to_mmps
|
||||
return (((uint32_t)raw * 1000000UL) / FACTOR_RWP_KMH_890ADV) / 3600UL;
|
||||
|
||||
case KTM_1290_SD_R_2023:
|
||||
return (((uint32_t)raw * 1000000UL) / FACTOR_RWP_KMH_1290SD) / 3600UL;
|
||||
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
bool Init_CAN_Native()
|
||||
{
|
||||
// 1) HAL bereitstellen (Selftest inklusive). Nur initialisieren, wenn noch nicht ready.
|
||||
if (!CAN_HAL_IsReady())
|
||||
{
|
||||
CanHalConfig cfg;
|
||||
cfg.baud = CAN_500KBPS;
|
||||
cfg.clock = MCP_16MHZ;
|
||||
cfg.listenOnlyProbeMs = 50; // kurzer, unkritischer „Bus lebt?“-Blick
|
||||
|
||||
if (!CAN_HAL_Init(cfg))
|
||||
{
|
||||
// Hardware/Selftest failed → native Pfad nicht nutzbar
|
||||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||||
Debug_pushMessage("CAN(Native): HAL init failed\n");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// 2) Masken/Filter setzen
|
||||
CAN_HAL_SetStdMask11(0, 0x7FF);
|
||||
CAN_HAL_SetStdMask11(1, 0x7FF);
|
||||
|
||||
CanFilter flist[1] = {{ID_KTM_REAR_WHEEL, false}};
|
||||
CAN_HAL_SetFilters(flist, 1);
|
||||
|
||||
CAN_HAL_SetMode(MCP_NORMAL);
|
||||
|
||||
// 3) Startzustand/Flags
|
||||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, false);
|
||||
// DTC_NO_CAN_SIGNAL wird in Process_* verwaltet
|
||||
|
||||
// 4) Status resetten
|
||||
s_lastIntegrateMs = millis();
|
||||
s_lastRxMs = 0;
|
||||
s_lastSpeed_mmps = 0;
|
||||
|
||||
Debug_pushMessage("CAN(Native): Filters set (ID=0x%03X), NORMAL mode\n", ID_KTM_REAR_WHEEL);
|
||||
return true;
|
||||
}
|
||||
|
||||
uint32_t Process_CAN_Native_WheelSpeed()
|
||||
{
|
||||
const uint32_t now = millis();
|
||||
uint32_t add_mm = 0;
|
||||
|
||||
// 1) Frames non-blocking ziehen und relevante verarbeiten
|
||||
for (uint8_t i = 0; i < 6; ++i)
|
||||
{ // kleine Obergrenze gegen Busy-Loops
|
||||
unsigned long id;
|
||||
uint8_t dlc;
|
||||
uint8_t buf[8];
|
||||
if (!CAN_HAL_Read(id, dlc, buf))
|
||||
break;
|
||||
|
||||
// Wir erwarten 11-bit 0x12D (Filter sind gesetzt, aber doppelter Boden schadet nicht)
|
||||
if (id == ID_KTM_REAR_WHEEL)
|
||||
{
|
||||
s_lastSpeed_mmps = parse_speed_mmps_from_frame(dlc, buf);
|
||||
s_lastRxMs = now;
|
||||
// Kein "break": falls mehrere Frames in der Queue sind, nehmen wir das letzte als aktuellsten
|
||||
}
|
||||
}
|
||||
|
||||
// 2) CAN-Heartbeat -> DTC_NO_CAN_SIGNAL (Warnung, wenn >10s nix mehr kam)
|
||||
if (s_lastRxMs != 0)
|
||||
{
|
||||
const bool stale = (now - s_lastRxMs) > 10000UL;
|
||||
MaintainDTC(DTC_NO_CAN_SIGNAL, stale);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Seit Start noch kein Frame gesehen -> noch nicht entscheiden, DTC-Logik darf warten
|
||||
// Optional: nach 1s ohne Frames Warnung setzen
|
||||
static uint32_t t0 = now;
|
||||
if (now - t0 > 1000UL)
|
||||
{
|
||||
MaintainDTC(DTC_NO_CAN_SIGNAL, true);
|
||||
}
|
||||
}
|
||||
|
||||
// 3) Integration der Distanz (mm) über dt
|
||||
if (s_lastIntegrateMs == 0)
|
||||
s_lastIntegrateMs = now;
|
||||
const uint32_t dt_ms = now - s_lastIntegrateMs;
|
||||
s_lastIntegrateMs = now;
|
||||
|
||||
// Wenn seit 600 ms keine neue Geschwindigkeit kam, setze v -> 0 (Stale-Schutz)
|
||||
const bool speedStale = (s_lastRxMs == 0) || ((now - s_lastRxMs) > 600UL);
|
||||
const uint32_t v_mmps = speedStale ? 0 : s_lastSpeed_mmps;
|
||||
|
||||
// mm = (mm/s * ms) / 1000
|
||||
add_mm = (uint64_t)v_mmps * dt_ms / 1000ULL;
|
||||
|
||||
return add_mm;
|
||||
}
|
243
Software/src/can_obd2.cpp
Normal file
243
Software/src/can_obd2.cpp
Normal file
@@ -0,0 +1,243 @@
|
||||
#include "can_obd2.h"
|
||||
#include "dtc.h"
|
||||
#include "debugger.h"
|
||||
#include "globals.h" // falls du später Einstellungen brauchst
|
||||
|
||||
// =======================
|
||||
// Konfiguration (anpassbar)
|
||||
// =======================
|
||||
|
||||
// Abfrageintervall für Speed (PID 0x0D)
|
||||
#ifndef OBD2_QUERY_INTERVAL_MS
|
||||
#define OBD2_QUERY_INTERVAL_MS 100 // 10 Hz
|
||||
#endif
|
||||
|
||||
// Antwort-Timeout auf eine einzelne Anfrage
|
||||
#ifndef OBD2_RESP_TIMEOUT_MS
|
||||
#define OBD2_RESP_TIMEOUT_MS 60 // ~60 ms
|
||||
#endif
|
||||
|
||||
// Wenn so lange keine valide Antwort kam, gilt die Geschwindigkeit als stale -> v=0
|
||||
#ifndef OBD2_STALE_MS
|
||||
#define OBD2_STALE_MS 600 // 0,6 s
|
||||
#endif
|
||||
|
||||
// Begrenzung, wie viele RX-Frames pro Aufruf maximal gezogen werden
|
||||
#ifndef OBD2_MAX_READS_PER_CALL
|
||||
#define OBD2_MAX_READS_PER_CALL 4
|
||||
#endif
|
||||
|
||||
// Optionales Debug-Rate-Limit
|
||||
#ifndef OBD2_DEBUG_INTERVAL_MS
|
||||
#define OBD2_DEBUG_INTERVAL_MS 1000
|
||||
#endif
|
||||
|
||||
// =======================
|
||||
// OBD-II IDs (11-bit)
|
||||
// =======================
|
||||
static constexpr uint16_t OBD_REQ_ID = 0x7DF; // Broadcast-Request
|
||||
static constexpr uint16_t OBD_RESP_MIN = 0x7E8; // ECUs antworten 0x7E8..0x7EF
|
||||
static constexpr uint16_t OBD_RESP_MAX = 0x7EF;
|
||||
|
||||
// =======================
|
||||
// Interner Status
|
||||
// =======================
|
||||
enum class ObdState : uint8_t
|
||||
{
|
||||
Idle = 0,
|
||||
Waiting = 1
|
||||
};
|
||||
|
||||
static ObdState s_state = ObdState::Idle;
|
||||
static uint32_t s_lastQueryTime = 0;
|
||||
static uint32_t s_requestDeadline = 0;
|
||||
|
||||
static uint32_t s_lastRespTime = 0;
|
||||
static uint32_t s_lastIntegrateMs = 0;
|
||||
static uint32_t s_lastSpeedMMps = 0;
|
||||
|
||||
static uint32_t s_lastDbgMs = 0;
|
||||
|
||||
// =======================
|
||||
// Hilfsfunktionen
|
||||
// =======================
|
||||
static inline bool isResponseId(unsigned long id)
|
||||
{
|
||||
return (id >= OBD_RESP_MIN) && (id <= OBD_RESP_MAX);
|
||||
}
|
||||
|
||||
static inline uint32_t kmh_to_mmps(uint16_t kmh)
|
||||
{
|
||||
return (uint32_t)kmh * 1000000UL / 3600UL;
|
||||
}
|
||||
|
||||
static inline void maybeDebug(uint32_t now, const char *fmt, ...)
|
||||
{
|
||||
#if 1
|
||||
if (now - s_lastDbgMs < OBD2_DEBUG_INTERVAL_MS)
|
||||
return;
|
||||
s_lastDbgMs = now;
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
Debug_pushMessage(fmt, ap);
|
||||
va_end(ap);
|
||||
#else
|
||||
(void)now;
|
||||
(void)fmt;
|
||||
#endif
|
||||
}
|
||||
|
||||
// =======================
|
||||
// Öffentliche API
|
||||
// =======================
|
||||
bool Init_CAN_OBD2()
|
||||
{
|
||||
// 1) HAL bereitstellen (Selftest inklusive). Nur initialisieren, wenn noch nicht ready.
|
||||
if (!CAN_HAL_IsReady())
|
||||
{
|
||||
CanHalConfig cfg;
|
||||
cfg.baud = CAN_500KBPS;
|
||||
cfg.clock = MCP_16MHZ;
|
||||
cfg.listenOnlyProbeMs = 50;
|
||||
|
||||
if (!CAN_HAL_Init(cfg))
|
||||
{
|
||||
// Hardware/Selftest failed → OBD2-CAN nicht nutzbar
|
||||
MaintainDTC(DTC_OBD2_CAN_TIMEOUT, true);
|
||||
Debug_pushMessage("CAN(OBD2): HAL init failed\n");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// 2) Filter/Masken für 0x7E8..0x7EF
|
||||
CAN_HAL_SetStdMask11(0, 0x7F0);
|
||||
CAN_HAL_SetStdMask11(1, 0x7F0);
|
||||
|
||||
CanFilter flist[6] = {
|
||||
{0x7E8, false},
|
||||
{0x7E9, false},
|
||||
{0x7EA, false},
|
||||
{0x7EB, false},
|
||||
{0x7EC, false},
|
||||
{0x7ED, false},
|
||||
};
|
||||
CAN_HAL_SetFilters(flist, 6);
|
||||
|
||||
CAN_HAL_SetMode(MCP_NORMAL);
|
||||
|
||||
// 3) DTC-Startzustände
|
||||
MaintainDTC(DTC_OBD2_CAN_TIMEOUT, false);
|
||||
MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, true); // bis erste Antwort kommt
|
||||
|
||||
// 4) Zeitbasen resetten
|
||||
const uint32_t now = millis();
|
||||
s_state = ObdState::Idle;
|
||||
s_lastQueryTime = now;
|
||||
s_requestDeadline = 0;
|
||||
s_lastRespTime = 0;
|
||||
s_lastIntegrateMs = now;
|
||||
s_lastSpeedMMps = 0;
|
||||
s_lastDbgMs = 0;
|
||||
|
||||
Debug_pushMessage("CAN(OBD2): Filters set (7E8..7EF), NORMAL mode\n");
|
||||
return true;
|
||||
}
|
||||
|
||||
uint32_t Process_CAN_OBD2_Speed()
|
||||
{
|
||||
const uint32_t now = millis();
|
||||
|
||||
// 1) Anfrage senden (nur wenn Idle und Intervall um)
|
||||
if (s_state == ObdState::Idle && (now - s_lastQueryTime) >= OBD2_QUERY_INTERVAL_MS)
|
||||
{
|
||||
uint8_t req[8] = {0x02, 0x01, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00}; // Mode 01, PID 0x0D (Speed)
|
||||
const uint8_t st = CAN_HAL_Send(OBD_REQ_ID, /*ext=*/false, 8, req);
|
||||
s_lastQueryTime = now;
|
||||
|
||||
if (st == CAN_OK)
|
||||
{
|
||||
s_state = ObdState::Waiting;
|
||||
s_requestDeadline = now + OBD2_RESP_TIMEOUT_MS;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Senden fehlgeschlagen -> harter Timeout-DTC
|
||||
MaintainDTC(DTC_OBD2_CAN_TIMEOUT, true);
|
||||
maybeDebug(now, "OBD2-CAN send failed (%u)\n", st);
|
||||
}
|
||||
}
|
||||
|
||||
// 2) Non-blocking Receive: wenige Frames pro Tick ziehen
|
||||
for (uint8_t i = 0; i < OBD2_MAX_READS_PER_CALL; ++i)
|
||||
{
|
||||
unsigned long rxId;
|
||||
uint8_t len;
|
||||
uint8_t rx[8];
|
||||
if (!CAN_HAL_Read(rxId, len, rx))
|
||||
break;
|
||||
if (!isResponseId(rxId))
|
||||
continue;
|
||||
if (len < 3)
|
||||
continue;
|
||||
|
||||
// Erwartete Formate:
|
||||
// - Einfache Antwort: 0x41 0x0D <A> ...
|
||||
// - Mit Längen-Byte: 0x03 0x41 0x0D <A> ...
|
||||
uint8_t modeResp = 0, pid = 0, speedKmh = 0;
|
||||
if (rx[0] == 0x03 && len >= 4 && rx[1] == 0x41 && rx[2] == 0x0D)
|
||||
{
|
||||
modeResp = rx[1];
|
||||
pid = rx[2];
|
||||
speedKmh = rx[3];
|
||||
}
|
||||
else if (rx[0] == 0x41 && rx[1] == 0x0D && len >= 3)
|
||||
{
|
||||
modeResp = rx[0];
|
||||
pid = rx[1];
|
||||
speedKmh = rx[2];
|
||||
}
|
||||
else
|
||||
{
|
||||
continue; // anderes PID/Format ignorieren
|
||||
}
|
||||
|
||||
if (modeResp == 0x41 && pid == 0x0D)
|
||||
{
|
||||
// Valide Antwort
|
||||
s_lastSpeedMMps = kmh_to_mmps(speedKmh);
|
||||
s_lastRespTime = now;
|
||||
s_state = ObdState::Idle;
|
||||
|
||||
MaintainDTC(DTC_OBD2_CAN_TIMEOUT, false);
|
||||
MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, false);
|
||||
|
||||
maybeDebug(now, "OBD2 speed: %u km/h (%lu mm/s)\n",
|
||||
(unsigned)speedKmh, (unsigned long)s_lastSpeedMMps);
|
||||
break; // eine valide Antwort pro Zyklus reicht
|
||||
}
|
||||
}
|
||||
|
||||
// 3) Offene Anfrage: Timeout prüfen
|
||||
if (s_state == ObdState::Waiting && (int32_t)(now - s_requestDeadline) >= 0)
|
||||
{
|
||||
// Keine passende Antwort erhalten
|
||||
MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, true);
|
||||
s_state = ObdState::Idle;
|
||||
}
|
||||
|
||||
// 4) Integration (mm) über dt
|
||||
uint32_t add_mm = 0;
|
||||
if (s_lastIntegrateMs == 0)
|
||||
s_lastIntegrateMs = now;
|
||||
const uint32_t dt_ms = now - s_lastIntegrateMs;
|
||||
s_lastIntegrateMs = now;
|
||||
|
||||
// Stale-Schutz: wenn lange keine Antwort -> v=0
|
||||
const bool stale = (s_lastRespTime == 0) || ((now - s_lastRespTime) > OBD2_STALE_MS);
|
||||
const uint32_t v_mmps = stale ? 0 : s_lastSpeedMMps;
|
||||
|
||||
// mm = (mm/s * ms) / 1000
|
||||
add_mm = (uint64_t)v_mmps * dt_ms / 1000ULL;
|
||||
|
||||
return add_mm;
|
||||
}
|
@@ -1,7 +1,27 @@
|
||||
#include "common.h"
|
||||
|
||||
#define ARR_LEN(a) (sizeof(a)/sizeof((a)[0]))
|
||||
|
||||
static_assert(ARR_LEN(SystemStatusString) == SYSSTAT_COUNT, "SystemStatusString size mismatch");
|
||||
static_assert(ARR_LEN(SpeedSourceString) == SPEEDSOURCE_COUNT, "SpeedSourceString size mismatch");
|
||||
static_assert(ARR_LEN(GPSBaudRateString) == GPSBAUDRATE_COUNT, "GPSBaudRateString size mismatch");
|
||||
static_assert(ARR_LEN(CANSourceString) == CANSOURCE_COUNT, "CANSourceString size mismatch");
|
||||
|
||||
static const char kUnknownStr[] = "Unknown";
|
||||
|
||||
// ---- System status string table ----
|
||||
const char *const SystemStatusString[SYSSTAT_COUNT] = {
|
||||
"Startup",
|
||||
"Normal",
|
||||
"Rain",
|
||||
"Wash",
|
||||
"Purge",
|
||||
"Error",
|
||||
"Shutdown",
|
||||
};
|
||||
|
||||
// String representation of SpeedSource enum
|
||||
const char *SpeedSourceString[] = {
|
||||
const char *const SpeedSourceString[SPEEDSOURCE_COUNT] = {
|
||||
#ifdef FEATURE_ENABLE_TIMER
|
||||
"Timer",
|
||||
#endif
|
||||
@@ -12,10 +32,8 @@ const char *SpeedSourceString[] = {
|
||||
"OBD2 (CAN)",
|
||||
};
|
||||
|
||||
const size_t SpeedSourceString_Elements = sizeof(SpeedSourceString) / sizeof(SpeedSourceString[0]);
|
||||
|
||||
// String representation of GPSBaudRate enum
|
||||
const char *GPSBaudRateString[] = {
|
||||
const char *const GPSBaudRateString[GPSBAUDRATE_COUNT] = {
|
||||
"4800",
|
||||
"9600",
|
||||
"19200",
|
||||
@@ -24,12 +42,49 @@ const char *GPSBaudRateString[] = {
|
||||
"115200",
|
||||
};
|
||||
|
||||
const size_t GPSBaudRateString_Elements = sizeof(GPSBaudRateString) / sizeof(GPSBaudRateString[0]);
|
||||
|
||||
// String representation of CANSource enum
|
||||
const char *CANSourceString[] = {
|
||||
const char *const CANSourceString[CANSOURCE_COUNT] = {
|
||||
"KTM 890 Adventure R (2021)",
|
||||
"KTM 1290 Superduke R (2023)",
|
||||
};
|
||||
|
||||
const size_t CANSourceString_Elements = sizeof(CANSourceString) / sizeof(CANSourceString[0]);
|
||||
// ---- Centralized, safe getters ----
|
||||
|
||||
// ---- Local helper for range check ----
|
||||
static inline bool in_range(int v, int max_exclusive)
|
||||
{
|
||||
return (v >= 0) && (v < max_exclusive);
|
||||
}
|
||||
|
||||
// ---- Safe getter ----
|
||||
const char *ToString(tSystem_Status v)
|
||||
{
|
||||
const int i = static_cast<int>(v);
|
||||
return in_range(i, static_cast<int>(SYSSTAT_COUNT))
|
||||
? SystemStatusString[i]
|
||||
: kUnknownStr;
|
||||
}
|
||||
|
||||
const char *ToString(SpeedSource_t v)
|
||||
{
|
||||
const int i = static_cast<int>(v);
|
||||
return in_range(i, static_cast<int>(SPEEDSOURCE_COUNT))
|
||||
? SpeedSourceString[i]
|
||||
: kUnknownStr;
|
||||
}
|
||||
|
||||
const char *ToString(GPSBaudRate_t v)
|
||||
{
|
||||
const int i = static_cast<int>(v);
|
||||
return in_range(i, static_cast<int>(GPSBAUDRATE_COUNT))
|
||||
? GPSBaudRateString[i]
|
||||
: kUnknownStr;
|
||||
}
|
||||
|
||||
const char *ToString(CANSource_t v)
|
||||
{
|
||||
const int i = static_cast<int>(v);
|
||||
return in_range(i, static_cast<int>(CANSOURCE_COUNT))
|
||||
? CANSourceString[i]
|
||||
: kUnknownStr;
|
||||
}
|
||||
|
@@ -1,56 +1,181 @@
|
||||
/**
|
||||
* @file config.cpp
|
||||
* @brief Implementation of EEPROM and configuration-related functions.
|
||||
* @brief EEPROM handling and configuration storage for the ChainLube firmware.
|
||||
*
|
||||
* This file contains functions for managing EEPROM storage and handling configuration data.
|
||||
* It includes the definitions of configuration structures, EEPROM access, and utility functions.
|
||||
* Responsibilities:
|
||||
* - Bring-up of the external I²C EEPROM
|
||||
* - Robust availability checks with optional bus recovery
|
||||
* - Central processing of EEPROM requests (save/load/format/move page)
|
||||
* - CRC32 utilities and debug dump helpers
|
||||
*
|
||||
* Design notes:
|
||||
* - The device boots with sane in-RAM defaults so the system stays operable
|
||||
* even when EEPROM is missing. Actual lube execution is gated by DTCs elsewhere.
|
||||
* - The DTC DTC_NO_EEPROM_FOUND is set/cleared only in EEPROM_Process(), never here ad-hoc.
|
||||
* - Background recovery is non-blocking and driven by millis().
|
||||
*/
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <Wire.h>
|
||||
|
||||
#include "config.h"
|
||||
#include "debugger.h"
|
||||
#include "globals.h"
|
||||
|
||||
// Instance of I2C_eeprom for EEPROM access
|
||||
// Recovery edge flag: set when availability changes 0 -> 1
|
||||
static bool eeRecoveredOnce = false;
|
||||
// Non-blocking retry scheduling
|
||||
static uint32_t eeNextTryMs = 0;
|
||||
static uint32_t eeRetryIntervalMs = 2000; // ms between background attempts
|
||||
|
||||
// I²C EEPROM instance
|
||||
I2C_eeprom ee(0x50, EEPROM_SIZE_BYTES);
|
||||
|
||||
// Configuration and persistence data structures
|
||||
// Configuration and persistence data
|
||||
LubeConfig_t LubeConfig;
|
||||
persistenceData_t PersistenceData;
|
||||
|
||||
// EEPROM version identifier
|
||||
const uint16_t eeVersion = EEPROM_STRUCTURE_REVISION;
|
||||
// EEPROM structure version (bumped when layout changes)
|
||||
const uint16_t eeVersion = EEPROM_STRUCTURE_REVISION;
|
||||
|
||||
// Flag indicating whether EEPROM is available
|
||||
boolean eeAvailable = false;
|
||||
// Latched availability flag
|
||||
static bool eeAvailable = false;
|
||||
|
||||
// Offsets within EEPROM for LubeConfig and PersistenceData
|
||||
// EEPROM layout offsets
|
||||
const uint16_t startofLubeConfig = 16;
|
||||
const uint16_t startofPersistence = 16 + sizeof(LubeConfig) + (sizeof(LubeConfig) % 16);
|
||||
|
||||
// Function prototype to check EEPROM availability
|
||||
boolean checkEEPROMavailable();
|
||||
// availability probe
|
||||
bool EEPROM_Available(bool recover = false, uint8_t attempts = 3, uint16_t delay_ms = 25);
|
||||
|
||||
// Robust EEPROM handling (internal helpers)
|
||||
void I2C_BusReset();
|
||||
bool TryRecoverEEPROM(uint8_t attempts = 5, uint16_t delay_ms = 50);
|
||||
|
||||
/**
|
||||
* @brief Initializes EEPROM and checks its availability.
|
||||
* @brief Initialize I²C and EEPROM driver, load in-RAM defaults.
|
||||
*
|
||||
* This function initializes the EEPROM using the I2C_eeprom instance and checks if it's available.
|
||||
* Loads defaults into RAM to keep the application operational.
|
||||
* Availability is checked but no DTC is set here—EEPROM_Process() is the single place
|
||||
* that sets/clears DTC_NO_EEPROM_FOUND.
|
||||
*/
|
||||
void InitEEPROM()
|
||||
{
|
||||
LubeConfig = LubeConfig_defaults;
|
||||
PersistenceData = {0};
|
||||
|
||||
Wire.begin();
|
||||
ee.begin();
|
||||
checkEEPROMavailable();
|
||||
|
||||
eeAvailable = ee.isConnected();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Processes EEPROM actions based on the request from the global state.
|
||||
* @brief Try to free a stuck I²C bus and enforce a STOP condition.
|
||||
*
|
||||
* This function processes EEPROM actions based on the request from the global state.
|
||||
* It performs actions such as saving, loading, and formatting EEPROM data for both configuration and persistence.
|
||||
* Pulses SCL up to 9 times to release a held SDA, then issues a STOP (SDA ↑ while SCL ↑).
|
||||
* Finally returns control to Wire.
|
||||
*/
|
||||
void I2C_BusReset()
|
||||
{
|
||||
pinMode(SCL, OUTPUT_OPEN_DRAIN);
|
||||
pinMode(SDA, INPUT_PULLUP);
|
||||
|
||||
for (int i = 0; i < 9; i++)
|
||||
{
|
||||
digitalWrite(SCL, LOW);
|
||||
delayMicroseconds(5);
|
||||
digitalWrite(SCL, HIGH);
|
||||
delayMicroseconds(5);
|
||||
}
|
||||
pinMode(SDA, OUTPUT_OPEN_DRAIN);
|
||||
digitalWrite(SDA, LOW);
|
||||
delayMicroseconds(5);
|
||||
digitalWrite(SCL, HIGH);
|
||||
delayMicroseconds(5);
|
||||
digitalWrite(SDA, HIGH);
|
||||
delayMicroseconds(5);
|
||||
|
||||
pinMode(SCL, INPUT);
|
||||
pinMode(SDA, INPUT);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Attempt to recover EEPROM connectivity.
|
||||
*
|
||||
* Sequence per attempt:
|
||||
* - I²C bus reset
|
||||
* - Wire.begin()
|
||||
* - ee.begin()
|
||||
* - short settle delay
|
||||
*
|
||||
* On first successful probe (0->1) the eeRecoveredOnce flag is raised.
|
||||
*
|
||||
* @param attempts Number of attempts
|
||||
* @param delay_ms Delay between attempts (after ee.begin())
|
||||
* @return true if EEPROM is reachable after recovery, false otherwise
|
||||
*/
|
||||
bool TryRecoverEEPROM(uint8_t attempts, uint16_t delay_ms)
|
||||
{
|
||||
for (uint8_t n = 0; n < attempts; n++)
|
||||
{
|
||||
I2C_BusReset();
|
||||
|
||||
// ESP8266 core: Wire.end() is not available; re-begin is sufficient
|
||||
Wire.begin();
|
||||
delay(2);
|
||||
|
||||
ee.begin();
|
||||
delay(delay_ms);
|
||||
|
||||
if (ee.isConnected())
|
||||
{
|
||||
if (!eeAvailable)
|
||||
eeRecoveredOnce = true; // edge 0 -> 1
|
||||
eeAvailable = true;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
eeAvailable = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Central EEPROM task: background recovery, DTC handling, and request dispatch.
|
||||
*
|
||||
* Called periodically from the main loop. Non-blocking by design.
|
||||
* - Schedules gentle recovery tries based on millis()
|
||||
* - Sets DTC_NO_EEPROM_FOUND when unavailable
|
||||
* - On successful recovery edge, clears DTC and reloads CFG/PDS exactly once
|
||||
* - Executes requested actions (save/load/format/move)
|
||||
*/
|
||||
void EEPROM_Process()
|
||||
{
|
||||
// Background recovery (single soft attempt per interval)
|
||||
const uint32_t now = millis();
|
||||
if (!EEPROM_Available() && now >= eeNextTryMs)
|
||||
{
|
||||
(void)TryRecoverEEPROM(1, 10);
|
||||
eeNextTryMs = now + eeRetryIntervalMs;
|
||||
}
|
||||
|
||||
// Central DTC handling
|
||||
if (!EEPROM_Available())
|
||||
{
|
||||
MaintainDTC(DTC_NO_EEPROM_FOUND, true);
|
||||
}
|
||||
|
||||
// Recovery edge: clear DTC and reload persisted data exactly once
|
||||
if (EEPROM_Available() && eeRecoveredOnce)
|
||||
{
|
||||
MaintainDTC(DTC_NO_EEPROM_FOUND, false);
|
||||
GetConfig_EEPROM();
|
||||
GetPersistence_EEPROM();
|
||||
eeRecoveredOnce = false;
|
||||
Debug_pushMessage("EEPROM recovered – reloaded CFG/PDS\n");
|
||||
}
|
||||
|
||||
// Request dispatcher
|
||||
switch (globals.requestEEAction)
|
||||
{
|
||||
case EE_CFG_SAVE:
|
||||
@@ -58,33 +183,39 @@ void EEPROM_Process()
|
||||
globals.requestEEAction = EE_IDLE;
|
||||
Debug_pushMessage("Stored EEPROM CFG\n");
|
||||
break;
|
||||
|
||||
case EE_CFG_LOAD:
|
||||
GetConfig_EEPROM();
|
||||
globals.requestEEAction = EE_IDLE;
|
||||
Debug_pushMessage("Loaded EEPROM CFG\n");
|
||||
break;
|
||||
|
||||
case EE_CFG_FORMAT:
|
||||
FormatConfig_EEPROM();
|
||||
globals.requestEEAction = EE_IDLE;
|
||||
GetConfig_EEPROM();
|
||||
Debug_pushMessage("Formatted EEPROM CFG\n");
|
||||
break;
|
||||
|
||||
case EE_PDS_SAVE:
|
||||
StorePersistence_EEPROM();
|
||||
globals.requestEEAction = EE_IDLE;
|
||||
Debug_pushMessage("Stored EEPROM PDS\n");
|
||||
break;
|
||||
|
||||
case EE_PDS_LOAD:
|
||||
GetPersistence_EEPROM();
|
||||
globals.requestEEAction = EE_IDLE;
|
||||
Debug_pushMessage("Loaded EEPROM PDS\n");
|
||||
break;
|
||||
|
||||
case EE_PDS_FORMAT:
|
||||
FormatPersistence_EEPROM();
|
||||
globals.requestEEAction = EE_IDLE;
|
||||
GetPersistence_EEPROM();
|
||||
Debug_pushMessage("Formatted EEPROM PDS\n");
|
||||
break;
|
||||
|
||||
case EE_FORMAT_ALL:
|
||||
FormatConfig_EEPROM();
|
||||
FormatPersistence_EEPROM();
|
||||
@@ -93,73 +224,100 @@ void EEPROM_Process()
|
||||
globals.requestEEAction = EE_IDLE;
|
||||
Debug_pushMessage("Formatted EEPROM ALL\n");
|
||||
break;
|
||||
|
||||
case EE_ALL_SAVE:
|
||||
StorePersistence_EEPROM();
|
||||
StoreConfig_EEPROM();
|
||||
globals.requestEEAction = EE_IDLE;
|
||||
Debug_pushMessage("Stored EEPROM ALL\n");
|
||||
break;
|
||||
|
||||
case EE_REINITIALIZE:
|
||||
{
|
||||
// quick burst of attempts
|
||||
const bool ok = TryRecoverEEPROM(5, 20);
|
||||
if (ok)
|
||||
{
|
||||
// Edge & reload are handled by the block above
|
||||
Debug_pushMessage("EEPROM reinitialize OK\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
MaintainDTC(DTC_NO_EEPROM_FOUND, true);
|
||||
Debug_pushMessage("EEPROM reinitialize FAILED\n");
|
||||
}
|
||||
globals.requestEEAction = EE_IDLE;
|
||||
break;
|
||||
}
|
||||
|
||||
case EE_IDLE:
|
||||
default:
|
||||
globals.requestEEAction = EE_IDLE;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Stores the configuration data in EEPROM.
|
||||
* @brief Store configuration to EEPROM (with CRC and sanity report).
|
||||
*
|
||||
* This function calculates the checksum for the configuration data, updates it, and stores it in EEPROM.
|
||||
* It also performs a sanity check on the configuration and raises a diagnostic trouble code (DTC) if needed.
|
||||
* Writes only if EEPROM is available. On completion, DTC_EEPROM_CFG_SANITY is
|
||||
* raised if any config fields are out of plausible bounds (bitmask payload).
|
||||
*/
|
||||
void StoreConfig_EEPROM()
|
||||
{
|
||||
LubeConfig.checksum = 0;
|
||||
LubeConfig.checksum = Checksum_EEPROM((uint8_t *)&LubeConfig, sizeof(LubeConfig));
|
||||
|
||||
if (!checkEEPROMavailable())
|
||||
if (!EEPROM_Available())
|
||||
return;
|
||||
|
||||
ee.updateBlock(startofLubeConfig, (uint8_t *)&LubeConfig, sizeof(LubeConfig));
|
||||
|
||||
uint32_t ConfigSanityCheckResult = ConfigSanityCheck(false);
|
||||
|
||||
if (ConfigSanityCheckResult > 0)
|
||||
const uint32_t sanity = ConfigSanityCheck(false);
|
||||
if (sanity > 0)
|
||||
{
|
||||
MaintainDTC(DTC_EEPROM_CFG_SANITY, true, ConfigSanityCheckResult);
|
||||
MaintainDTC(DTC_EEPROM_CFG_SANITY, true, sanity);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Retrieves the configuration data from EEPROM.
|
||||
* @brief Load configuration from EEPROM and validate.
|
||||
*
|
||||
* This function reads the configuration data from EEPROM, performs a checksum validation,
|
||||
* and conducts a sanity check on the configuration. It raises a diagnostic trouble code (DTC) if needed.
|
||||
* On CRC failure: raise DTC_EEPROM_CFG_BAD and fall back to in-RAM defaults (no writes).
|
||||
* On CRC OK: run sanity with autocorrect=true and raise DTC_EEPROM_CFG_SANITY with bitmask if needed.
|
||||
*/
|
||||
void GetConfig_EEPROM()
|
||||
{
|
||||
if (!checkEEPROMavailable())
|
||||
if (!EEPROM_Available())
|
||||
return;
|
||||
|
||||
ee.readBlock(startofLubeConfig, (uint8_t *)&LubeConfig, sizeof(LubeConfig));
|
||||
|
||||
uint32_t checksum = LubeConfig.checksum;
|
||||
const uint32_t checksum = LubeConfig.checksum;
|
||||
LubeConfig.checksum = 0;
|
||||
|
||||
MaintainDTC(DTC_EEPROM_CFG_BAD, (Checksum_EEPROM((uint8_t *)&LubeConfig, sizeof(LubeConfig)) != checksum));
|
||||
const bool badCrc = (Checksum_EEPROM((uint8_t *)&LubeConfig, sizeof(LubeConfig)) != checksum);
|
||||
MaintainDTC(DTC_EEPROM_CFG_BAD, badCrc);
|
||||
|
||||
if (badCrc) {
|
||||
// Don’t keep corrupted data in RAM
|
||||
LubeConfig = LubeConfig_defaults;
|
||||
LubeConfig.EEPROM_Version = EEPROM_STRUCTURE_REVISION; // explicit in-RAM version
|
||||
return;
|
||||
}
|
||||
|
||||
// CRC OK → restore checksum and sanitize (with autocorrect)
|
||||
LubeConfig.checksum = checksum;
|
||||
|
||||
uint32_t ConfigSanityCheckResult = ConfigSanityCheck(false);
|
||||
|
||||
MaintainDTC(DTC_EEPROM_CFG_SANITY, (ConfigSanityCheckResult > 0), ConfigSanityCheckResult);
|
||||
|
||||
const uint32_t sanity = ConfigSanityCheck(true);
|
||||
MaintainDTC(DTC_EEPROM_CFG_SANITY, (sanity > 0), sanity);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Stores the persistence data in EEPROM.
|
||||
* @brief Store persistence record to EEPROM (wear-levelled page).
|
||||
*
|
||||
* This function increments the write cycle counter, performs a checksum calculation on the persistence data,
|
||||
* and stores it in EEPROM. It also handles EEPROM page movement when needed.
|
||||
* Increments the write-cycle counter and moves the page if close to the limit.
|
||||
* Writes only if EEPROM is available.
|
||||
*/
|
||||
void StorePersistence_EEPROM()
|
||||
{
|
||||
@@ -171,103 +329,108 @@ void StorePersistence_EEPROM()
|
||||
PersistenceData.checksum = 0;
|
||||
PersistenceData.checksum = Checksum_EEPROM((uint8_t *)&PersistenceData, sizeof(PersistenceData));
|
||||
|
||||
if (!checkEEPROMavailable())
|
||||
if (!EEPROM_Available())
|
||||
return;
|
||||
|
||||
ee.updateBlock(globals.eePersistanceAdress, (uint8_t *)&PersistenceData, sizeof(PersistenceData));
|
||||
ee.updateBlock(globals.eePersistenceAddress, (uint8_t *)&PersistenceData, sizeof(PersistenceData));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Retrieves the persistence data from EEPROM.
|
||||
* @brief Load persistence record, validating address range and CRC.
|
||||
*
|
||||
* This function reads the EEPROM to get the start address of the persistence data.
|
||||
* If the start address is out of range, it resets and stores defaults. Otherwise,
|
||||
* it reads from EEPROM and checks if the data is correct.
|
||||
* If the stored start address is out of range, the persistence partition is reset,
|
||||
* formatted, and DTC_EEPROM_PDSADRESS_BAD is raised.
|
||||
* Otherwise, the record is read and checked; on CRC failure DTC_EEPROM_PDS_BAD is raised
|
||||
* and the in-RAM persistence data is reset to a safe default (no writes performed here).
|
||||
*/
|
||||
void GetPersistence_EEPROM()
|
||||
{
|
||||
if (!checkEEPROMavailable())
|
||||
if (!EEPROM_Available())
|
||||
return;
|
||||
|
||||
ee.readBlock(0, (uint8_t *)&globals.eePersistanceAdress, sizeof(globals.eePersistanceAdress));
|
||||
// if we got the StartAdress of Persistance and it's out of Range - we Reset it and store defaults
|
||||
// otherwise we Read from eeprom and check if everything is correct
|
||||
if (globals.eePersistanceAdress < startofPersistence || globals.eePersistanceAdress > ee.getDeviceSize())
|
||||
// Read wear-level start address
|
||||
ee.readBlock(0, (uint8_t *)&globals.eePersistenceAddress, sizeof(globals.eePersistenceAddress));
|
||||
|
||||
const uint16_t addr = globals.eePersistenceAddress;
|
||||
const uint16_t need = sizeof(PersistenceData);
|
||||
const uint16_t dev = ee.getDeviceSize();
|
||||
|
||||
// Strict range check: addr must be within partition and block must fit into device
|
||||
if (addr < startofPersistence || (uint32_t)addr + (uint32_t)need > (uint32_t)dev)
|
||||
{
|
||||
MovePersistencePage_EEPROM(true);
|
||||
FormatPersistence_EEPROM();
|
||||
MaintainDTC(DTC_EEPROM_PDSADRESS_BAD, true);
|
||||
return;
|
||||
}
|
||||
else
|
||||
|
||||
// Safe to read the record
|
||||
ee.readBlock(addr, (uint8_t *)&PersistenceData, sizeof(PersistenceData));
|
||||
|
||||
const uint32_t checksum = PersistenceData.checksum;
|
||||
PersistenceData.checksum = 0;
|
||||
|
||||
const bool badCrc = (Checksum_EEPROM((uint8_t *)&PersistenceData, sizeof(PersistenceData)) != checksum);
|
||||
MaintainDTC(DTC_EEPROM_PDS_BAD, badCrc);
|
||||
|
||||
if (badCrc)
|
||||
{
|
||||
ee.readBlock(globals.eePersistanceAdress, (uint8_t *)&PersistenceData, sizeof(PersistenceData));
|
||||
|
||||
uint32_t checksum = PersistenceData.checksum;
|
||||
PersistenceData.checksum = 0;
|
||||
|
||||
MaintainDTC(DTC_EEPROM_PDS_BAD, (Checksum_EEPROM((uint8_t *)&PersistenceData, sizeof(PersistenceData)) != checksum));
|
||||
|
||||
PersistenceData.checksum = checksum;
|
||||
// Do not keep corrupted data in RAM; leave DTC set, no EEPROM writes here
|
||||
PersistenceData = {0};
|
||||
return;
|
||||
}
|
||||
|
||||
// CRC ok -> restore checksum into the struct kept in RAM
|
||||
PersistenceData.checksum = checksum;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Formats the configuration partition in EEPROM.
|
||||
*
|
||||
* This function resets the configuration data to defaults and stores it in EEPROM.
|
||||
* @brief Reset the configuration partition to defaults and write it.
|
||||
*/
|
||||
void FormatConfig_EEPROM()
|
||||
{
|
||||
Debug_pushMessage("Formatting Config-Partition\n");
|
||||
Debug_pushMessage("Formatting Config partition\n");
|
||||
LubeConfig = LubeConfig_defaults;
|
||||
LubeConfig.EEPROM_Version = eeVersion;
|
||||
StoreConfig_EEPROM();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Formats the persistence partition in EEPROM.
|
||||
*
|
||||
* This function resets the persistence data to defaults and stores it in EEPROM.
|
||||
* @brief Reset the persistence partition and write an empty record.
|
||||
*/
|
||||
void FormatPersistence_EEPROM()
|
||||
{
|
||||
Debug_pushMessage("Formatting Persistance-Partition\n");
|
||||
Debug_pushMessage("Formatting Persistence partition\n");
|
||||
PersistenceData = {0};
|
||||
// memset(&PersistenceData, 0, sizeof(PersistenceData));
|
||||
StorePersistence_EEPROM();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Moves the persistence page in EEPROM.
|
||||
* @brief Advance the persistence page (wear levelling) and store the new start address.
|
||||
*
|
||||
* This function adjusts the persistence page address and resets the write cycle counter.
|
||||
* When end-of-device (or reset=true), wrap back to startofPersistence.
|
||||
* Requires EEPROM availability.
|
||||
*
|
||||
* @param reset If true, the function resets the persistence page address to the start of the partition.
|
||||
* @param reset If true, force wrap to the start of the partition.
|
||||
*/
|
||||
void MovePersistencePage_EEPROM(boolean reset)
|
||||
{
|
||||
if (!checkEEPROMavailable())
|
||||
if (!EEPROM_Available())
|
||||
return;
|
||||
|
||||
globals.eePersistanceAdress += sizeof(PersistenceData);
|
||||
globals.eePersistenceAddress += sizeof(PersistenceData);
|
||||
PersistenceData.writeCycleCounter = 0;
|
||||
|
||||
// Check if we reached the end of the EEPROM and start over at the beginning
|
||||
if ((globals.eePersistanceAdress + sizeof(PersistenceData)) > ee.getDeviceSize() || reset)
|
||||
if ((globals.eePersistenceAddress + sizeof(PersistenceData)) > ee.getDeviceSize() || reset)
|
||||
{
|
||||
globals.eePersistanceAdress = startofPersistence;
|
||||
globals.eePersistenceAddress = startofPersistence;
|
||||
}
|
||||
|
||||
ee.updateBlock(0, (uint8_t *)&globals.eePersistanceAdress, sizeof(globals.eePersistanceAdress));
|
||||
ee.updateBlock(0, (uint8_t *)&globals.eePersistenceAddress, sizeof(globals.eePersistenceAddress));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Calculate CRC-32 checksum for a block of data.
|
||||
*
|
||||
* This function implements the CRC-32 algorithm.
|
||||
*
|
||||
* @param data Pointer to the data block.
|
||||
* @param len Length of the data block in bytes.
|
||||
* @return CRC-32 checksum.
|
||||
* @brief Compute CRC-32 (poly 0xEDB88320) over a byte buffer.
|
||||
*/
|
||||
uint32_t Checksum_EEPROM(uint8_t const *data, size_t len)
|
||||
{
|
||||
@@ -275,55 +438,43 @@ uint32_t Checksum_EEPROM(uint8_t const *data, size_t len)
|
||||
return 0;
|
||||
|
||||
uint32_t crc = 0xFFFFFFFF;
|
||||
uint32_t mask;
|
||||
|
||||
while (len--)
|
||||
{
|
||||
crc ^= *data++;
|
||||
|
||||
for (uint8_t k = 0; k < 8; k++)
|
||||
{
|
||||
mask = -(crc & 1);
|
||||
crc = (crc >> 1) ^ (0xEDB88320 & mask);
|
||||
}
|
||||
crc = (crc >> 1) ^ (0xEDB88320 & (-(int32_t)(crc & 1)));
|
||||
}
|
||||
|
||||
return ~crc;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Dump a portion of EEPROM contents for debugging.
|
||||
* @brief Print a hex/ASCII dump of a region of the EEPROM for debugging.
|
||||
*
|
||||
* This function prints the contents of a specified portion of EEPROM in a formatted way.
|
||||
*
|
||||
* @param memoryAddress Starting address in EEPROM.
|
||||
* @param length Number of bytes to dump.
|
||||
* Output format:
|
||||
* Address 00 01 02 ... 0F ASCII
|
||||
* 0x00000: XX XX ... .....
|
||||
*/
|
||||
void dumpEEPROM(uint16_t memoryAddress, uint16_t length)
|
||||
{
|
||||
#define BLOCK_TO_LENGTH 16
|
||||
|
||||
if (!checkEEPROMavailable())
|
||||
if (!EEPROM_Available())
|
||||
return;
|
||||
|
||||
char ascii_buf[BLOCK_TO_LENGTH + 1];
|
||||
sprintf(ascii_buf, "%*s", BLOCK_TO_LENGTH, "ASCII");
|
||||
|
||||
// Print column headers
|
||||
Debug_pushMessage(PSTR("\nAddress "));
|
||||
for (int x = 0; x < BLOCK_TO_LENGTH; x++)
|
||||
Debug_pushMessage("%3d", x);
|
||||
|
||||
// Align address and length to BLOCK_TO_LENGTH boundaries
|
||||
memoryAddress = memoryAddress / BLOCK_TO_LENGTH * BLOCK_TO_LENGTH;
|
||||
length = (length + BLOCK_TO_LENGTH - 1) / BLOCK_TO_LENGTH * BLOCK_TO_LENGTH;
|
||||
memoryAddress = (memoryAddress / BLOCK_TO_LENGTH) * BLOCK_TO_LENGTH;
|
||||
length = ((length + BLOCK_TO_LENGTH - 1) / BLOCK_TO_LENGTH) * BLOCK_TO_LENGTH;
|
||||
|
||||
// Iterate through the specified portion of EEPROM
|
||||
for (unsigned int i = 0; i < length; i++)
|
||||
{
|
||||
int blockpoint = memoryAddress % BLOCK_TO_LENGTH;
|
||||
const int blockpoint = memoryAddress % BLOCK_TO_LENGTH;
|
||||
|
||||
// Print ASCII representation header for each block
|
||||
if (blockpoint == 0)
|
||||
{
|
||||
ascii_buf[BLOCK_TO_LENGTH] = 0;
|
||||
@@ -331,55 +482,54 @@ void dumpEEPROM(uint16_t memoryAddress, uint16_t length)
|
||||
Debug_pushMessage("\n0x%05X:", memoryAddress);
|
||||
}
|
||||
|
||||
// Read and print each byte
|
||||
ascii_buf[blockpoint] = ee.readByte(memoryAddress);
|
||||
Debug_pushMessage(" %02X", ascii_buf[blockpoint]);
|
||||
|
||||
// Replace non-printable characters with dots in ASCII representation
|
||||
if (ascii_buf[blockpoint] < 0x20 || ascii_buf[blockpoint] > 0x7E)
|
||||
ascii_buf[blockpoint] = '.';
|
||||
|
||||
memoryAddress++;
|
||||
}
|
||||
|
||||
// Print a new line at the end of the dump
|
||||
Debug_pushMessage("\n");
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Check if EEPROM is available and connected.
|
||||
* @brief Unified availability probe with optional recovery.
|
||||
*
|
||||
* This function checks if the EEPROM is available and connected. If not, it triggers
|
||||
* a diagnostic trouble code (DTC) indicating the absence of EEPROM.
|
||||
* Fast path returns the latched availability flag. If not available,
|
||||
* performs a direct probe and, optionally, a recovery sequence.
|
||||
*
|
||||
* @param recover If true, attempt recovery when not available (default: false).
|
||||
* @param attempts Recovery attempts (default: 3).
|
||||
* @param delay_ms Delay between attempts in ms (default: 25).
|
||||
* @return true if EEPROM is available, false otherwise.
|
||||
*/
|
||||
boolean checkEEPROMavailable()
|
||||
bool EEPROM_Available(bool recover, uint8_t attempts, uint16_t delay_ms)
|
||||
{
|
||||
// Check if EEPROM is connected
|
||||
if (!ee.isConnected())
|
||||
if (eeAvailable)
|
||||
return true;
|
||||
|
||||
if (ee.isConnected())
|
||||
{
|
||||
// Trigger DTC for no EEPROM found
|
||||
MaintainDTC(DTC_NO_EEPROM_FOUND, true);
|
||||
return false;
|
||||
eeAvailable = true;
|
||||
eeRecoveredOnce = true; // edge 0 -> 1
|
||||
return true;
|
||||
}
|
||||
|
||||
// Clear DTC for no EEPROM found since it's available now
|
||||
MaintainDTC(DTC_NO_EEPROM_FOUND, false);
|
||||
if (recover)
|
||||
{
|
||||
return TryRecoverEEPROM(attempts, delay_ms);
|
||||
}
|
||||
|
||||
// EEPROM is available
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Perform sanity check on configuration settings.
|
||||
* @brief Validate config fields; return bitmask of invalid entries.
|
||||
*
|
||||
* This function checks the validity of various configuration settings and returns a bitmask
|
||||
* indicating which settings need to be reset. If autocorrect is enabled, it resets the settings
|
||||
* to their default values.
|
||||
*
|
||||
* @param autocorrect If true, automatically correct invalid settings by resetting to defaults.
|
||||
* @return A bitmask indicating which settings need to be reset.
|
||||
* If autocorrect is true, invalid fields are reset to default values.
|
||||
* Each bit in the returned mask identifies a specific field-group that was out-of-bounds.
|
||||
*/
|
||||
uint32_t ConfigSanityCheck(bool autocorrect)
|
||||
{
|
||||
@@ -465,21 +615,21 @@ uint32_t ConfigSanityCheck(bool autocorrect)
|
||||
LubeConfig.BleedingPulses = LubeConfig_defaults.BleedingPulses;
|
||||
}
|
||||
|
||||
if (!(LubeConfig.SpeedSource >= 0) || !(LubeConfig.SpeedSource < SpeedSourceString_Elements))
|
||||
if (!(LubeConfig.SpeedSource >= 0) || !(LubeConfig.SpeedSource < SPEEDSOURCE_COUNT))
|
||||
{
|
||||
SET_BIT(setting_reset_bits, 11);
|
||||
if (autocorrect)
|
||||
LubeConfig.SpeedSource = LubeConfig_defaults.SpeedSource;
|
||||
}
|
||||
|
||||
if (!(LubeConfig.GPSBaudRate >= 0) || !(LubeConfig.GPSBaudRate < GPSBaudRateString_Elements))
|
||||
if (!(LubeConfig.GPSBaudRate >= 0) || !(LubeConfig.GPSBaudRate < GPSBAUDRATE_COUNT))
|
||||
{
|
||||
SET_BIT(setting_reset_bits, 12);
|
||||
if (autocorrect)
|
||||
LubeConfig.GPSBaudRate = LubeConfig_defaults.GPSBaudRate;
|
||||
}
|
||||
|
||||
if (!(LubeConfig.CANSource >= 0) || !(LubeConfig.CANSource < CANSourceString_Elements))
|
||||
if (!(LubeConfig.CANSource >= 0) || !(LubeConfig.CANSource < CANSOURCE_COUNT))
|
||||
{
|
||||
SET_BIT(setting_reset_bits, 13);
|
||||
if (autocorrect)
|
||||
@@ -513,22 +663,17 @@ uint32_t ConfigSanityCheck(bool autocorrect)
|
||||
if (autocorrect)
|
||||
strncpy(LubeConfig.wifi_client_password, LubeConfig_defaults.wifi_client_password, sizeof(LubeConfig.wifi_client_password));
|
||||
}
|
||||
// Return the bitmask indicating which settings need to be reset
|
||||
|
||||
return setting_reset_bits;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Validates whether a given string contains only characters allowed in WiFi SSIDs and passwords.
|
||||
* @brief Validate that a string contains only characters allowed for Wi‑Fi SSIDs/passwords.
|
||||
*
|
||||
* This function checks each character in the provided string to ensure
|
||||
* that it contains only characters allowed in WiFi SSIDs and passwords.
|
||||
* It considers characters from 'A' to 'Z', 'a' to 'z', '0' to '9', as well as
|
||||
* the following special characters: ! " # $ % & ' ( ) * + , - . / : ; < = > ? @ [ \ ] ^ _ ` { | } ~
|
||||
* Allowed: A‑Z, a‑z, 0‑9 and the printable ASCII punctuation: ! " # $ % & ' ( ) * + , - . / : ;
|
||||
* < = > ? @ [ \ ] ^ _ ` { | } ~
|
||||
*
|
||||
* @param string Pointer to the string to be validated.
|
||||
* @param size Size of the string including the null-terminator.
|
||||
* @return true if the string contains only allowed characters or is NULL,
|
||||
* false otherwise.
|
||||
* @return true if valid (or empty), false otherwise.
|
||||
*/
|
||||
bool validateWiFiString(char *string, size_t size)
|
||||
{
|
||||
@@ -539,10 +684,8 @@ bool validateWiFiString(char *string, size_t size)
|
||||
{
|
||||
char c = string[i];
|
||||
if (c == '\0')
|
||||
{
|
||||
// Reached the end of the string, all characters were valid WiFi characters.
|
||||
return true;
|
||||
}
|
||||
return true; // reached end with valid chars
|
||||
|
||||
if (!((c >= 'A' && c <= 'Z') || (c >= 'a' && c <= 'z') ||
|
||||
(c >= '0' && c <= '9') || c == '!' || c == '"' || c == '#' ||
|
||||
c == '$' || c == '%' || c == '&' || c == '\'' || c == '(' ||
|
||||
@@ -552,11 +695,9 @@ bool validateWiFiString(char *string, size_t size)
|
||||
c == '\\' || c == ']' || c == '^' || c == '_' || c == '`' ||
|
||||
c == '{' || c == '|' || c == '}' || c == '~'))
|
||||
{
|
||||
// Found a character that is not a valid WiFi character.
|
||||
return false;
|
||||
}
|
||||
}
|
||||
// If the loop completes without finding a null terminator, the string is invalid.
|
||||
// No NUL within buffer: treat as invalid
|
||||
return false;
|
||||
}
|
||||
|
||||
|
File diff suppressed because it is too large
Load Diff
@@ -56,7 +56,6 @@ void RunLubeApp(uint32_t add_milimeters)
|
||||
|
||||
if (lastSystemStatus != globals.systemStatus)
|
||||
{
|
||||
strcpy_P(globals.systemStatustxt, PSTR("Startup"));
|
||||
LEDControl_SetBasic(LED_STARTUP_NORMAL, LED_PATTERN_BLINK);
|
||||
lastSystemStatus = globals.systemStatus;
|
||||
globals.resumeStatus = sysStat_Startup;
|
||||
@@ -72,7 +71,6 @@ void RunLubeApp(uint32_t add_milimeters)
|
||||
case sysStat_Normal:
|
||||
if (lastSystemStatus != globals.systemStatus)
|
||||
{
|
||||
strcpy_P(globals.systemStatustxt, PSTR("Normal"));
|
||||
LEDControl_SetBasic(LED_NORMAL_COLOR, LED_PATTERN_ON);
|
||||
lastSystemStatus = globals.systemStatus;
|
||||
globals.resumeStatus = sysStat_Normal;
|
||||
@@ -89,7 +87,6 @@ void RunLubeApp(uint32_t add_milimeters)
|
||||
case sysStat_Rain:
|
||||
if (lastSystemStatus != globals.systemStatus)
|
||||
{
|
||||
strcpy_P(globals.systemStatustxt, PSTR("Rain"));
|
||||
LEDControl_SetBasic(LED_RAIN_COLOR, LED_PATTERN_ON);
|
||||
lastSystemStatus = globals.systemStatus;
|
||||
globals.resumeStatus = sysStat_Rain;
|
||||
@@ -107,7 +104,6 @@ void RunLubeApp(uint32_t add_milimeters)
|
||||
if (lastSystemStatus != globals.systemStatus)
|
||||
{
|
||||
washModeRemainDistance = LubeConfig.WashMode_Distance;
|
||||
strcpy_P(globals.systemStatustxt, PSTR("Wash"));
|
||||
LEDControl_SetBasic(LED_WASH_COLOR, LED_PATTERN_BREATH);
|
||||
lastSystemStatus = globals.systemStatus;
|
||||
}
|
||||
@@ -134,7 +130,6 @@ void RunLubeApp(uint32_t add_milimeters)
|
||||
if (lastSystemStatus != globals.systemStatus)
|
||||
{
|
||||
globals.purgePulses = LubeConfig.BleedingPulses;
|
||||
strcpy_P(globals.systemStatustxt, PSTR("Purge"));
|
||||
LEDControl_SetBasic(LED_PURGE_COLOR, LED_PATTERN_BLINK);
|
||||
lastSystemStatus = globals.systemStatus;
|
||||
}
|
||||
@@ -161,7 +156,6 @@ void RunLubeApp(uint32_t add_milimeters)
|
||||
|
||||
if (lastSystemStatus != globals.systemStatus)
|
||||
{
|
||||
strcpy_P(globals.systemStatustxt, PSTR("Error"));
|
||||
LEDControl_SetBasic(LED_ERROR_COLOR, LED_PATTERN_BLINK_FAST);
|
||||
lastSystemStatus = globals.systemStatus;
|
||||
}
|
||||
@@ -173,7 +167,6 @@ void RunLubeApp(uint32_t add_milimeters)
|
||||
|
||||
if (lastSystemStatus != globals.systemStatus)
|
||||
{
|
||||
strcpy_P(globals.systemStatustxt, PSTR("Shutdown"));
|
||||
LEDControl_SetBasic(LED_SHUTDOWN_COLOR, LED_PATTERN_BREATH_REVERSE);
|
||||
lastSystemStatus = globals.systemStatus;
|
||||
}
|
||||
|
@@ -33,12 +33,12 @@
|
||||
#include "config.h"
|
||||
#include "globals.h"
|
||||
#include "debugger.h"
|
||||
#include "can.h"
|
||||
#include "gps.h"
|
||||
#include "dtc.h"
|
||||
#include "led_colors.h"
|
||||
#include "obd2_kline.h"
|
||||
#include "obd2_can.h"
|
||||
#include "can_obd2.h"
|
||||
#include "can_native.h"
|
||||
#include "buttoncontrol.h"
|
||||
#include "button_actions.h"
|
||||
#include "ledcontrol.h"
|
||||
@@ -102,6 +102,9 @@ void setup()
|
||||
// Initialize and clear Diagnostic Trouble Code (DTC) storage
|
||||
ClearAllDTC();
|
||||
|
||||
Wire.begin();
|
||||
|
||||
|
||||
#ifdef FEATURE_ENABLE_WIFI_CLIENT
|
||||
// Configure WiFi settings for client mode if enabled
|
||||
WiFi.mode(WIFI_STA);
|
||||
@@ -142,8 +145,8 @@ void setup()
|
||||
switch (LubeConfig.SpeedSource)
|
||||
{
|
||||
case SOURCE_CAN:
|
||||
Init_CAN();
|
||||
wheelSpeedcapture = &Process_CAN_WheelSpeed;
|
||||
Init_CAN_Native();
|
||||
wheelSpeedcapture = &Process_CAN_Native_WheelSpeed;
|
||||
Serial.print("\nCAN-Init done");
|
||||
break;
|
||||
case SOURCE_GPS:
|
||||
@@ -163,8 +166,8 @@ void setup()
|
||||
Serial.print("\nOBD2-KLine-Init done");
|
||||
break;
|
||||
case SOURCE_OBD2_CAN:
|
||||
Init_OBD2_CAN();
|
||||
wheelSpeedcapture = &Process_OBD2_CAN_Speed;
|
||||
Init_CAN_OBD2();
|
||||
wheelSpeedcapture = &Process_CAN_OBD2_Speed;
|
||||
Serial.print("\nOBD2-CAN-Init done");
|
||||
break;
|
||||
default:
|
||||
@@ -398,7 +401,7 @@ void Display_Process()
|
||||
DistRemain = DistRemain - (PersistenceData.TravelDistance_highRes_mm / 1000);
|
||||
|
||||
// Display relevant information on the OLED screen based on system status
|
||||
u8x8.printf(PSTR("Mode: %10s\n"), globals.systemStatustxt);
|
||||
u8x8.printf(PSTR("Mode: %10s\n"), ToString(globals.systemStatus));
|
||||
if (globals.systemStatus == sysStat_Error)
|
||||
{
|
||||
// Display the last Diagnostic Trouble Code (DTC) in case of an error
|
||||
@@ -412,7 +415,8 @@ void Display_Process()
|
||||
u8x8.printf(PSTR("WiFi: %10s\n"), (WiFi.getMode() == WIFI_AP ? "AP" : WiFi.getMode() == WIFI_OFF ? "OFF"
|
||||
: WiFi.getMode() == WIFI_STA ? "CLIENT"
|
||||
: "UNKNOWN"));
|
||||
u8x8.printf(PSTR("Source: %8s\n"), SpeedSourceString[LubeConfig.SpeedSource]);
|
||||
u8x8.printf(PSTR("Source: %8s\n"), ToString(LubeConfig.SpeedSource));
|
||||
|
||||
u8x8.printf("%s\n", WiFi.localIP().toString().c_str());
|
||||
}
|
||||
|
||||
|
@@ -1,84 +0,0 @@
|
||||
#include "obd2_can.h"
|
||||
#include <mcp_can.h>
|
||||
#include <SPI.h>
|
||||
#include "common.h"
|
||||
#include "globals.h"
|
||||
#include "dtc.h"
|
||||
#include "debugger.h"
|
||||
|
||||
// === Setup: MCP2515 CS-Pin definieren ===
|
||||
#define OBD2_CAN_CS_PIN 10
|
||||
#define OBD2_OBD_REQUEST_ID 0x7DF
|
||||
#define OBD2_OBD_RESPONSE_ID 0x7E8
|
||||
|
||||
MCP_CAN OBD_CAN(OBD2_CAN_CS_PIN);
|
||||
|
||||
static uint32_t lastQueryTime = 0;
|
||||
static uint32_t lastRecvTime = 0;
|
||||
static uint32_t lastSpeedMMperSec = 0;
|
||||
|
||||
#define OBD2_QUERY_INTERVAL 500 // alle 500ms
|
||||
|
||||
void Init_OBD2_CAN()
|
||||
{
|
||||
if (OBD_CAN.begin(MCP_STD, CAN_500KBPS, MCP_16MHZ) != CAN_OK)
|
||||
{
|
||||
Serial.println("OBD2 CAN Init FAILED!");
|
||||
return;
|
||||
}
|
||||
|
||||
OBD_CAN.setMode(MCP_NORMAL);
|
||||
delay(100);
|
||||
Serial.println("OBD2 CAN Init OK");
|
||||
}
|
||||
|
||||
uint32_t Process_OBD2_CAN_Speed()
|
||||
{
|
||||
if (millis() - lastQueryTime < OBD2_QUERY_INTERVAL)
|
||||
return 0;
|
||||
|
||||
lastQueryTime = millis();
|
||||
|
||||
// Anfrage: 01 0D → Geschwindigkeit
|
||||
byte obdRequest[8] = {0x02, 0x01, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00};
|
||||
byte sendStat = OBD_CAN.sendMsgBuf(OBD2_OBD_REQUEST_ID, 0, 8, obdRequest);
|
||||
|
||||
if (sendStat != CAN_OK)
|
||||
{
|
||||
MaintainDTC(DTC_OBD2_CAN_TIMEOUT, true);
|
||||
Debug_pushMessage("OBD2_CAN: send failed (%d)\n", sendStat);
|
||||
return 0;
|
||||
}
|
||||
|
||||
unsigned long rxId;
|
||||
byte len = 0;
|
||||
byte rxBuf[8];
|
||||
uint32_t timeout = millis() + 100;
|
||||
|
||||
while (millis() < timeout)
|
||||
{
|
||||
if (OBD_CAN.checkReceive() == CAN_MSGAVAIL)
|
||||
{
|
||||
OBD_CAN.readMsgBuf(&rxId, &len, rxBuf);
|
||||
if ((rxId & 0xFFF8) == OBD2_OBD_RESPONSE_ID && rxBuf[1] == 0x0D)
|
||||
{
|
||||
MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, false); // alles ok
|
||||
|
||||
uint8_t speed_kmh = rxBuf[3];
|
||||
uint32_t speed_mm_per_sec = (uint32_t)speed_kmh * 1000000 / 3600;
|
||||
uint32_t dt = millis() - lastRecvTime;
|
||||
lastRecvTime = millis();
|
||||
lastSpeedMMperSec = speed_mm_per_sec;
|
||||
|
||||
Debug_pushMessage("OBD2_CAN: %d km/h (%lu mm/s)\n", speed_kmh, speed_mm_per_sec);
|
||||
return (speed_mm_per_sec * dt) / 1000;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Keine Antwort erhalten
|
||||
MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, true);
|
||||
Debug_pushMessage("OBD2_CAN: no response within timeout\n");
|
||||
return 0;
|
||||
}
|
||||
|
@@ -337,89 +337,145 @@ void WebserverFirmwareUpdate_Callback(AsyncWebServerRequest *request, const Stri
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Callback function for handling EEPROM restore via the web server.
|
||||
*
|
||||
* This function is invoked during the EEPROM restore process when a new EEPROM file
|
||||
* is received. It handles the restore process by reading the data from the received file,
|
||||
* deserializing the JSON data, and updating the configuration and persistence data accordingly.
|
||||
* If the restore is successful, it triggers a system shutdown.
|
||||
*
|
||||
* @param request Pointer to the AsyncWebServerRequest object.
|
||||
* @param filename The name of the file being restored.
|
||||
* @param index The index of the file being restored.
|
||||
* @param data Pointer to the data buffer.
|
||||
* @param len The length of the data buffer.
|
||||
* @param final Boolean indicating if this is the final chunk of data.
|
||||
*/
|
||||
void WebserverEERestore_Callback(AsyncWebServerRequest *request, const String &filename, size_t index, uint8_t *data, size_t len, bool final)
|
||||
void WebserverEERestore_Callback(AsyncWebServerRequest *request,
|
||||
const String &filename,
|
||||
size_t index,
|
||||
uint8_t *data,
|
||||
size_t len,
|
||||
bool final)
|
||||
{
|
||||
constexpr size_t kBufCap = 1536;
|
||||
|
||||
bool ee_done = false;
|
||||
static bool validext = false;
|
||||
static char *buffer = NULL;
|
||||
static char *buffer = nullptr;
|
||||
static uint32_t read_ptr = 0;
|
||||
DeserializationError error;
|
||||
|
||||
// kleines Helferlein zum sicheren Kopieren & Terminieren
|
||||
auto safe_copy = [](char *dst, size_t dst_sz, const char *src)
|
||||
{
|
||||
if (!dst || dst_sz == 0)
|
||||
return;
|
||||
if (!src)
|
||||
{
|
||||
dst[0] = '\0';
|
||||
return;
|
||||
}
|
||||
strncpy(dst, src, dst_sz - 1);
|
||||
dst[dst_sz - 1] = '\0';
|
||||
};
|
||||
|
||||
// Grenzen/Hilfen für Enum-Ranges (Sentinel bevorzugt, sonst *_Elements)
|
||||
const int maxSpeedSrc = static_cast<int>(SPEEDSOURCE_COUNT);
|
||||
const int maxGPSBaud = static_cast<int>(GPSBAUDRATE_COUNT);
|
||||
const int maxCANSrc = static_cast<int>(CANSOURCE_COUNT);
|
||||
|
||||
if (!index)
|
||||
{
|
||||
validext = (filename.indexOf(".ee.json") > -1);
|
||||
if (validext)
|
||||
{
|
||||
buffer = (char *)malloc(1536);
|
||||
buffer = (char *)malloc(kBufCap);
|
||||
read_ptr = 0;
|
||||
if (buffer == NULL)
|
||||
if (!buffer)
|
||||
{
|
||||
Debug_pushMessage("malloc() failed for EEPROM-Restore\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (buffer != NULL && len > 0)
|
||||
// Chunked receive mit Cap/Trunkierungsschutz
|
||||
if (buffer && len > 0)
|
||||
{
|
||||
memcpy(buffer + read_ptr, data, len);
|
||||
read_ptr = read_ptr + len;
|
||||
size_t remain = (read_ptr < kBufCap) ? (kBufCap - read_ptr) : 0;
|
||||
size_t to_copy = (len <= remain) ? len : remain;
|
||||
if (to_copy > 0)
|
||||
{
|
||||
memcpy(buffer + read_ptr, data, to_copy);
|
||||
read_ptr += to_copy;
|
||||
}
|
||||
else
|
||||
{
|
||||
Debug_pushMessage("EEPROM-Restore input exceeds buffer, truncating\n");
|
||||
}
|
||||
}
|
||||
|
||||
if (final)
|
||||
{
|
||||
if (buffer != NULL)
|
||||
if (buffer)
|
||||
{
|
||||
// Ensure zero-termination just in case
|
||||
if (read_ptr >= 1536)
|
||||
read_ptr = 1535;
|
||||
// Null-terminieren
|
||||
if (read_ptr == kBufCap)
|
||||
read_ptr = kBufCap - 1;
|
||||
buffer[read_ptr] = '\0';
|
||||
|
||||
Serial.print(buffer);
|
||||
JsonDocument json;
|
||||
// Parse
|
||||
JsonDocument json; // entspricht deinem bisherigen Stil
|
||||
error = deserializeJson(json, buffer);
|
||||
if (error)
|
||||
{
|
||||
Debug_pushMessage("deserializeJson() failed: %s\n", error.f_str());
|
||||
}
|
||||
else
|
||||
else if (validext)
|
||||
{
|
||||
// ---- Konfiguration sicher in RAM übernehmen ----
|
||||
// clamp-Helfer passend zu deinen Sanity-Grenzen
|
||||
auto clamp_u32 = [](uint32_t v, uint32_t lo, uint32_t hi)
|
||||
{ return (v < lo) ? lo : (v > hi ? hi : v); };
|
||||
auto clamp_u16 = [](uint16_t v, uint16_t lo, uint16_t hi)
|
||||
{ return (v < lo) ? lo : (v > hi ? hi : v); };
|
||||
auto clamp_u8 = [](uint8_t v, uint8_t lo, uint8_t hi)
|
||||
{ return (v < lo) ? lo : (v > hi ? hi : v); };
|
||||
|
||||
LubeConfig.DistancePerLube_Default = json["config"]["DistancePerLube_Default"].as<uint32_t>();
|
||||
LubeConfig.DistancePerLube_Rain = json["config"]["DistancePerLube_Rain"].as<uint32_t>();
|
||||
LubeConfig.tankCapacity_ml = json["config"]["tankCapacity_ml"].as<uint32_t>();
|
||||
LubeConfig.amountPerDose_microL = json["config"]["amountPerDose_microL"].as<uint32_t>();
|
||||
LubeConfig.TankRemindAtPercentage = json["config"]["TankRemindAtPercentage"].as<uint8_t>();
|
||||
LubeConfig.PulsePerRevolution = json["config"]["PulsePerRevolution"].as<uint8_t>();
|
||||
LubeConfig.TireWidth_mm = json["config"]["TireWidth_mm"].as<uint32_t>();
|
||||
LubeConfig.TireWidthHeight_Ratio = json["config"]["TireWidthHeight_Ratio"].as<uint32_t>();
|
||||
LubeConfig.RimDiameter_Inch = json["config"]["RimDiameter_Inch"].as<uint32_t>();
|
||||
LubeConfig.DistancePerRevolution_mm = json["config"]["DistancePerRevolution_mm"].as<uint32_t>();
|
||||
LubeConfig.BleedingPulses = json["config"]["BleedingPulses"].as<uint16_t>();
|
||||
LubeConfig.SpeedSource = (SpeedSource_t)json["config"]["SpeedSource"].as<int>();
|
||||
LubeConfig.GPSBaudRate = (GPSBaudRate_t)json["config"]["GPSBaudRate"].as<int>();
|
||||
LubeConfig.CANSource = (CANSource_t)json["config"]["CANSource"].as<int>();
|
||||
// config.*
|
||||
LubeConfig.DistancePerLube_Default = clamp_u32(json["config"]["DistancePerLube_Default"].as<uint32_t>(), 0, 50000);
|
||||
LubeConfig.DistancePerLube_Rain = clamp_u32(json["config"]["DistancePerLube_Rain"].as<uint32_t>(), 0, 50000);
|
||||
LubeConfig.tankCapacity_ml = clamp_u32(json["config"]["tankCapacity_ml"].as<uint32_t>(), 0, 5000);
|
||||
LubeConfig.amountPerDose_microL = clamp_u32(json["config"]["amountPerDose_microL"].as<uint32_t>(), 0, 100);
|
||||
LubeConfig.TankRemindAtPercentage = clamp_u8(json["config"]["TankRemindAtPercentage"].as<uint8_t>(), 0, 100);
|
||||
LubeConfig.PulsePerRevolution = clamp_u8(json["config"]["PulsePerRevolution"].as<uint8_t>(), 0, 255);
|
||||
LubeConfig.TireWidth_mm = clamp_u32(json["config"]["TireWidth_mm"].as<uint32_t>(), 0, 500);
|
||||
LubeConfig.TireWidthHeight_Ratio = clamp_u32(json["config"]["TireWidthHeight_Ratio"].as<uint32_t>(), 0, 150);
|
||||
LubeConfig.RimDiameter_Inch = clamp_u32(json["config"]["RimDiameter_Inch"].as<uint32_t>(), 0, 30);
|
||||
LubeConfig.DistancePerRevolution_mm = clamp_u32(json["config"]["DistancePerRevolution_mm"].as<uint32_t>(), 0, 10000);
|
||||
LubeConfig.BleedingPulses = clamp_u16(json["config"]["BleedingPulses"].as<uint16_t>(), 0, 1000);
|
||||
LubeConfig.WashMode_Distance = json["config"]["WashMode_Distance"].as<uint16_t>(); // ggf. Grenzen anpassen
|
||||
LubeConfig.WashMode_Interval = json["config"]["WashMode_Interval"].as<uint16_t>(); // ggf. Grenzen anpassen
|
||||
LubeConfig.LED_Mode_Flash = json["config"]["LED_Mode_Flash"].as<bool>();
|
||||
LubeConfig.LED_Max_Brightness = json["config"]["LED_Max_Brightness"].as<uint8_t>();
|
||||
LubeConfig.LED_Min_Brightness = json["config"]["LED_Min_Brightness"].as<uint8_t>();
|
||||
strncpy(LubeConfig.wifi_ap_ssid, json["config"]["wifi_ap_ssid"].as<const char *>(), sizeof(LubeConfig.wifi_ap_ssid));
|
||||
strncpy(LubeConfig.wifi_ap_password, json["config"]["wifi_ap_password"].as<const char *>(), sizeof(LubeConfig.wifi_ap_password));
|
||||
strncpy(LubeConfig.wifi_client_ssid, json["config"]["wifi_client_ssid"].as<const char *>(), sizeof(LubeConfig.wifi_client_ssid));
|
||||
strncpy(LubeConfig.wifi_client_password, json["config"]["wifi_client_password"].as<const char *>(), sizeof(LubeConfig.wifi_client_password));
|
||||
|
||||
// Enums nur nach Range-Check übernehmen
|
||||
{
|
||||
int v = json["config"]["SpeedSource"].as<int>();
|
||||
if (v >= 0 && v < maxSpeedSrc)
|
||||
LubeConfig.SpeedSource = (SpeedSource_t)v;
|
||||
else
|
||||
Debug_pushMessage("Restore: invalid SpeedSource=%d\n", v);
|
||||
}
|
||||
{
|
||||
int v = json["config"]["GPSBaudRate"].as<int>();
|
||||
if (v >= 0 && v < maxGPSBaud)
|
||||
LubeConfig.GPSBaudRate = (GPSBaudRate_t)v;
|
||||
else
|
||||
Debug_pushMessage("Restore: invalid GPSBaudRate=%d\n", v);
|
||||
}
|
||||
{
|
||||
int v = json["config"]["CANSource"].as<int>();
|
||||
if (v >= 0 && v < maxCANSrc)
|
||||
LubeConfig.CANSource = (CANSource_t)v;
|
||||
else
|
||||
Debug_pushMessage("Restore: invalid CANSource=%d\n", v);
|
||||
}
|
||||
|
||||
// Strings sicher kopieren (0-terminiert)
|
||||
safe_copy(LubeConfig.wifi_ap_ssid, sizeof(LubeConfig.wifi_ap_ssid), json["config"]["wifi_ap_ssid"]);
|
||||
safe_copy(LubeConfig.wifi_ap_password, sizeof(LubeConfig.wifi_ap_password), json["config"]["wifi_ap_password"]);
|
||||
safe_copy(LubeConfig.wifi_client_ssid, sizeof(LubeConfig.wifi_client_ssid), json["config"]["wifi_client_ssid"]);
|
||||
safe_copy(LubeConfig.wifi_client_password, sizeof(LubeConfig.wifi_client_password), json["config"]["wifi_client_password"]);
|
||||
|
||||
// persis.*
|
||||
PersistenceData.writeCycleCounter = json["persis"]["writeCycleCounter"].as<uint16_t>();
|
||||
PersistenceData.tankRemain_microL = json["persis"]["tankRemain_microL"].as<uint32_t>();
|
||||
PersistenceData.TravelDistance_highRes_mm = json["persis"]["TravelDistance_highRes_mm"].as<uint32_t>();
|
||||
@@ -427,24 +483,33 @@ void WebserverEERestore_Callback(AsyncWebServerRequest *request, const String &f
|
||||
PersistenceData.odometer = json["persis"]["odometer"].as<uint32_t>();
|
||||
PersistenceData.checksum = json["persis"]["checksum"].as<uint32_t>();
|
||||
|
||||
// Optional: Sanity-Autokorrektur im RAM (keine EEPROM-Writes hier!)
|
||||
{
|
||||
uint32_t sanity = ConfigSanityCheck(true);
|
||||
if (sanity > 0)
|
||||
{
|
||||
MaintainDTC(DTC_EEPROM_CFG_SANITY, true, sanity);
|
||||
Debug_pushMessage("Restore: ConfigSanity corrected (mask=0x%08lX)\n", sanity);
|
||||
}
|
||||
}
|
||||
|
||||
ee_done = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (buffer)
|
||||
{
|
||||
free(buffer);
|
||||
buffer = NULL;
|
||||
buffer = nullptr;
|
||||
}
|
||||
|
||||
AsyncWebServerResponse *response = request->beginResponse(302, "text/plain", "Please wait while the device reboots");
|
||||
// Browser zurückleiten & ggf. Shutdown
|
||||
AsyncWebServerResponse *response =
|
||||
request->beginResponse(302, "text/plain", "Please wait while the device reboots");
|
||||
response->addHeader("Refresh", "20");
|
||||
response->addHeader("Location", "/");
|
||||
request->send(response);
|
||||
|
||||
if (ee_done)
|
||||
{
|
||||
Debug_pushMessage("Update complete\n");
|
||||
Debug_pushMessage("EEPROM restore complete\n");
|
||||
globals.systemStatus = sysStat_Shutdown;
|
||||
}
|
||||
}
|
||||
@@ -479,8 +544,8 @@ void WebServerEEJSON_Callback(AsyncWebServerRequest *request)
|
||||
generateJsonObject_PersistenceData(persis);
|
||||
|
||||
JsonObject eepart = json["eepart"].to<JsonObject>();
|
||||
sprintf(buffer, "0x%04X", globals.eePersistanceAdress);
|
||||
eepart["PersistanceAddress"] = buffer;
|
||||
sprintf(buffer, "0x%04X", globals.eePersistenceAddress);
|
||||
eepart["PersistenceAddress"] = buffer;
|
||||
|
||||
serializeJsonPretty(json, *response);
|
||||
|
||||
@@ -648,24 +713,24 @@ void Websocket_HandleSettings(uint8_t *data)
|
||||
}
|
||||
else if (strcmp(identifier, "speedsource") == 0)
|
||||
{
|
||||
int index = findIndexByString(value, SpeedSourceString, (int)SpeedSourceString_Elements);
|
||||
if (validIndex(index, (int)SpeedSourceString_Elements))
|
||||
int index = findIndexByString(value, SpeedSourceString, (int)SPEEDSOURCE_COUNT);
|
||||
if (validIndex(index, (int)SPEEDSOURCE_COUNT))
|
||||
speedsourcePreselect = (SpeedSource_t)index;
|
||||
else
|
||||
Debug_pushMessage("Invalid speedsource '%s'\n", value);
|
||||
}
|
||||
else if (strcmp(identifier, "cansource") == 0)
|
||||
{
|
||||
int index = findIndexByString(value, CANSourceString, (int)CANSourceString_Elements);
|
||||
if (validIndex(index, (int)CANSourceString_Elements))
|
||||
int index = findIndexByString(value, CANSourceString, (int)CANSOURCE_COUNT);
|
||||
if (validIndex(index, (int)CANSOURCE_COUNT))
|
||||
LubeConfig.CANSource = (CANSource_t)index;
|
||||
else
|
||||
Debug_pushMessage("Invalid cansource '%s'\n", value);
|
||||
}
|
||||
else if (strcmp(identifier, "gpsbaud") == 0)
|
||||
{
|
||||
int index = findIndexByString(value, GPSBaudRateString, (int)GPSBaudRateString_Elements);
|
||||
if (validIndex(index, (int)GPSBaudRateString_Elements))
|
||||
int index = findIndexByString(value, GPSBaudRateString, (int)GPSBAUDRATE_COUNT);
|
||||
if (validIndex(index, (int)GPSBAUDRATE_COUNT))
|
||||
LubeConfig.GPSBaudRate = (GPSBaudRate_t)index;
|
||||
else
|
||||
Debug_pushMessage("Invalid gpsbaud '%s'\n", value);
|
||||
@@ -801,7 +866,7 @@ void Websocket_RefreshClientData_Status(uint32_t client_id, bool send_mapping)
|
||||
|
||||
String temp = "STATUS:";
|
||||
|
||||
temp.concat(String(nz(globals.systemStatustxt)) + ";");
|
||||
temp.concat(String(ToString(globals.systemStatus)) + ";");
|
||||
|
||||
// Guard against division by zero (capacity==0)
|
||||
uint32_t cap = LubeConfig.tankCapacity_ml;
|
||||
@@ -856,26 +921,26 @@ void Websocket_RefreshClientData_Static(uint32_t client_id, bool send_mapping)
|
||||
temp += String(LubeConfig.RimDiameter_Inch) + ";";
|
||||
|
||||
// speedsource + Optionen
|
||||
temp += tableStr(SpeedSourceString, (int)LubeConfig.SpeedSource, (int)SpeedSourceString_Elements) + ";";
|
||||
temp += String(ToString(LubeConfig.SpeedSource)) + ";";
|
||||
{
|
||||
String csv;
|
||||
appendCsv(csv, SpeedSourceString, SpeedSourceString_Elements);
|
||||
appendCsv(csv, SpeedSourceString, SPEEDSOURCE_COUNT);
|
||||
temp += csv + ";";
|
||||
}
|
||||
|
||||
// gpsbaud + Optionen
|
||||
temp += tableStr(GPSBaudRateString, (int)LubeConfig.GPSBaudRate, (int)GPSBaudRateString_Elements) + ";";
|
||||
temp += String(ToString(LubeConfig.GPSBaudRate)) + ";";
|
||||
{
|
||||
String csv;
|
||||
appendCsv(csv, GPSBaudRateString, GPSBaudRateString_Elements);
|
||||
appendCsv(csv, GPSBaudRateString, GPSBAUDRATE_COUNT);
|
||||
temp += csv + ";";
|
||||
}
|
||||
|
||||
// cansource + Optionen
|
||||
temp += tableStr(CANSourceString, (int)LubeConfig.CANSource, (int)CANSourceString_Elements) + ";";
|
||||
temp += String(ToString(LubeConfig.CANSource)) + ";";
|
||||
{
|
||||
String csv;
|
||||
appendCsv(csv, CANSourceString, CANSourceString_Elements);
|
||||
appendCsv(csv, CANSourceString, CANSOURCE_COUNT);
|
||||
temp += csv + ";";
|
||||
}
|
||||
|
||||
|
Reference in New Issue
Block a user