Added current factors. Various fixes for temperature monitor. Added partial implementation for can1 and can2

This commit is contained in:
Yury Shuvakin
2023-03-14 06:18:00 +03:00
parent 3f9bcbfebb
commit 418ad47c22
15 changed files with 678 additions and 137 deletions

View File

@@ -5,7 +5,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/> <provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-674487765729842554" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true"> <provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="892067782257261287" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/> <language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/> <language-scope id="org.eclipse.cdt.core.g++"/>
</provider> </provider>
@@ -16,8 +16,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/> <provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider copy-of="extension" id="org.eclipse.cdt.managedbuilder.core.GCCBuildCommandParser"/> <provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="892067782257261287" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-625935826514445666" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/> <language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/> <language-scope id="org.eclipse.cdt.core.g++"/>
</provider> </provider>

View File

@@ -1,6 +1,7 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?> <?xml version="1.0" encoding="UTF-8" standalone="no"?>
<launchConfiguration type="com.st.stm32cube.ide.mcu.debug.launch.launchConfigurationType"> <launchConfiguration type="com.st.stm32cube.ide.mcu.debug.launch.launchConfigurationType">
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.access_port_id" value="0"/> <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.access_port_id" value="0"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.cubeprog_external_loaders" value="[]"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.enable_live_expr" value="true"/> <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.enable_live_expr" value="true"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.enable_swv" value="false"/> <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.enable_swv" value="false"/>
<intAttribute key="com.st.stm32cube.ide.mcu.debug.launch.formatVersion" value="2"/> <intAttribute key="com.st.stm32cube.ide.mcu.debug.launch.formatVersion" value="2"/>
@@ -20,17 +21,29 @@
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.useRemoteTarget" value="true"/> <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.useRemoteTarget" value="true"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.vector_table" value=""/> <stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.vector_table" value=""/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.verify_flash_download" value="true"/> <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.verify_flash_download" value="true"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.openocd.CTI_ALLOW_HALT" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.openocd.CTI_SIGNAL_HALT" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.openocd.DBG_DEVICE_SHAREABLE_ALLOWED" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.openocd.DBG_INTERFACE" value="Swd"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.openocd.DBG_INTERFACE_FREQUENCY" value="8000000.0"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.openocd.DBG_LOW_POWER_MODE_ALLOWED" value="true"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.openocd.DBG_RESET_MODE" value="connect_under_reset"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.openocd.DBG_STOP_WATCHDOG_THEN_HALTED_ALLOWED" value="true"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.openocd.OPENOCD_GENERATOR_OPTION" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.openocd.OPENOCD_NAME" value="&quot;${stm32cubeide_openocd_path}\openocd.exe&quot;"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.openocd.OPENOCD_OTHER_OPTIONS" value=""/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.openocd.OPENOCD_RESTART_CONFIGURATIONS" value="{&quot;fVersion&quot;:1,&quot;fItems&quot;:[{&quot;fDisplayName&quot;:&quot;Reset halt&quot;,&quot;fIsSuppressible&quot;:false,&quot;fResetAttribute&quot;:&quot;Reset halt&quot;,&quot;fResetStrategies&quot;:[{&quot;fDisplayName&quot;:&quot;Reset halt&quot;,&quot;fLaunchAttribute&quot;:&quot;monitor reset halt&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset halt&quot;],&quot;fCmdOptions&quot;:[]},{&quot;fDisplayName&quot;:&quot;Reset init&quot;,&quot;fLaunchAttribute&quot;:&quot;monitor reset init&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset init&quot;],&quot;fCmdOptions&quot;:[]},{&quot;fDisplayName&quot;:&quot;None&quot;,&quot;fLaunchAttribute&quot;:&quot;&quot;,&quot;fGdbCommands&quot;:[],&quot;fCmdOptions&quot;:[]}],&quot;fGdbCommandGroup&quot;:{&quot;name&quot;:&quot;Additional commands&quot;,&quot;commands&quot;:[]},&quot;fStartApplication&quot;:true},{&quot;fDisplayName&quot;:&quot;Reset init&quot;,&quot;fIsSuppressible&quot;:false,&quot;fResetAttribute&quot;:&quot;Reset init&quot;,&quot;fResetStrategies&quot;:[{&quot;fDisplayName&quot;:&quot;Reset halt&quot;,&quot;fLaunchAttribute&quot;:&quot;monitor reset halt&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset halt&quot;],&quot;fCmdOptions&quot;:[]},{&quot;fDisplayName&quot;:&quot;Reset init&quot;,&quot;fLaunchAttribute&quot;:&quot;monitor reset init&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset init&quot;],&quot;fCmdOptions&quot;:[]},{&quot;fDisplayName&quot;:&quot;None&quot;,&quot;fLaunchAttribute&quot;:&quot;&quot;,&quot;fGdbCommands&quot;:[],&quot;fCmdOptions&quot;:[]}],&quot;fGdbCommandGroup&quot;:{&quot;name&quot;:&quot;Additional commands&quot;,&quot;commands&quot;:[]},&quot;fStartApplication&quot;:true}]}"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.openocd.OPENOCD_SCRIPT" value="${ProjDirPath}\BMS_v3 Debug.cfg"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.openocd.OPENOCD_SCRIPT_CHOICE" value="automated"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.openocdenable_rtos" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.cti_allow_halt" value="false"/> <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.cti_allow_halt" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.cti_signal_halt" value="false"/> <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.cti_signal_halt" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_external_loader" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_logging" value="false"/> <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_logging" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_max_halt_delay" value="false"/> <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_max_halt_delay" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_shared_stlink" value="false"/> <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_shared_stlink" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.external_loader" value=""/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.external_loader_init" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.frequency" value="0"/> <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.frequency" value="0"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.halt_all_on_reset" value="false"/> <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.halt_all_on_reset" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.log_file" value="E:\GIT\BMS\Debug\st-link_gdbserver_log.txt"/> <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.log_file" value="C:\Users\Public\bms_firmware\Debug\st-link_gdbserver_log.txt"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.low_power_debug" value="enable"/> <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.low_power_debug" value="enable"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.max_halt_delay" value="2"/> <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.max_halt_delay" value="2"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.reset_strategy" value="connect_under_reset"/> <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.reset_strategy" value="connect_under_reset"/>
@@ -38,7 +51,7 @@
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.stlink_txt_serial_number" value=""/> <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.stlink_txt_serial_number" value=""/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.watchdog_config" value="none"/> <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.watchdog_config" value="none"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkenable_rtos" value="false"/> <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkenable_rtos" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkrestart_configurations" value="{&quot;fItems&quot;:[{&quot;fDisplayName&quot;:&quot;Reset&quot;,&quot;fIsSuppressible&quot;:false,&quot;fResetAttribute&quot;:&quot;Software system reset&quot;,&quot;fResetStrategies&quot;:[{&quot;fDisplayName&quot;:&quot;Software system reset&quot;,&quot;fLaunchAttribute&quot;:&quot;system_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset\r\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Hardware reset&quot;,&quot;fLaunchAttribute&quot;:&quot;hardware_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset hardware\r\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Core reset&quot;,&quot;fLaunchAttribute&quot;:&quot;core_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset core\r\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;None&quot;,&quot;fLaunchAttribute&quot;:&quot;no_reset&quot;,&quot;fGdbCommands&quot;:[],&quot;fCmdOptions&quot;:[&quot;-g&quot;]}],&quot;fGdbCommandGroup&quot;:{&quot;name&quot;:&quot;Additional commands&quot;,&quot;commands&quot;:[]}}]}"/> <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkrestart_configurations" value="{&quot;fVersion&quot;:1,&quot;fItems&quot;:[{&quot;fDisplayName&quot;:&quot;Reset&quot;,&quot;fIsSuppressible&quot;:false,&quot;fResetAttribute&quot;:&quot;Software system reset&quot;,&quot;fResetStrategies&quot;:[{&quot;fDisplayName&quot;:&quot;Software system reset&quot;,&quot;fLaunchAttribute&quot;:&quot;system_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset\r\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Hardware reset&quot;,&quot;fLaunchAttribute&quot;:&quot;hardware_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset hardware\r\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Core reset&quot;,&quot;fLaunchAttribute&quot;:&quot;core_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset core\r\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;None&quot;,&quot;fLaunchAttribute&quot;:&quot;no_reset&quot;,&quot;fGdbCommands&quot;:[],&quot;fCmdOptions&quot;:[&quot;-g&quot;]}],&quot;fGdbCommandGroup&quot;:{&quot;name&quot;:&quot;Additional commands&quot;,&quot;commands&quot;:[]},&quot;fStartApplication&quot;:true}]}"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.enableRtosProxy" value="false"/> <booleanAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.enableRtosProxy" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyCustomProperties" value=""/> <stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyCustomProperties" value=""/>
<stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyDriver" value="threadx"/> <stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyDriver" value="threadx"/>
@@ -58,7 +71,7 @@
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setStopAt" value="true"/> <booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setStopAt" value="true"/>
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.stopAt" value="main"/> <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.stopAt" value="main"/>
<stringAttribute key="org.eclipse.cdt.dsf.gdb.DEBUG_NAME" value="arm-none-eabi-gdb"/> <stringAttribute key="org.eclipse.cdt.dsf.gdb.DEBUG_NAME" value="arm-none-eabi-gdb"/>
<booleanAttribute key="org.eclipse.cdt.dsf.gdb.NON_STOP" value="true"/> <booleanAttribute key="org.eclipse.cdt.dsf.gdb.NON_STOP" value="false"/>
<booleanAttribute key="org.eclipse.cdt.dsf.gdb.UPDATE_THREADLIST_ON_SUSPEND" value="false"/> <booleanAttribute key="org.eclipse.cdt.dsf.gdb.UPDATE_THREADLIST_ON_SUSPEND" value="false"/>
<intAttribute key="org.eclipse.cdt.launch.ATTR_BUILD_BEFORE_LAUNCH_ATTR" value="2"/> <intAttribute key="org.eclipse.cdt.launch.ATTR_BUILD_BEFORE_LAUNCH_ATTR" value="2"/>
<stringAttribute key="org.eclipse.cdt.launch.COREFILE_PATH" value=""/> <stringAttribute key="org.eclipse.cdt.launch.COREFILE_PATH" value=""/>
@@ -76,5 +89,5 @@
<listEntry value="4"/> <listEntry value="4"/>
</listAttribute> </listAttribute>
<stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&lt;memoryBlockExpressionList context=&quot;reserved-for-future-use&quot;/&gt;"/> <stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&lt;memoryBlockExpressionList context=&quot;reserved-for-future-use&quot;/&gt;"/>
<stringAttribute key="process_factory_id" value="org.eclipse.cdt.dsf.gdb.GdbProcessFactory"/> <stringAttribute key="process_factory_id" value="com.st.stm32cube.ide.mcu.debug.launch.HardwareDebugProcessFactory"/>
</launchConfiguration> </launchConfiguration>

56
Core/Inc/can.h Normal file
View File

@@ -0,0 +1,56 @@
//
// Created by enik on 02.06.22.
//
#ifndef USBCANV1_CAN_H
#define USBCANV1_CAN_H
//#include "libs.h"
#include <stdbool.h>
#include "stm32f1xx_hal.h"
typedef uint8_t u8_t; ///< 8-bit unsigned
typedef int8_t i8_t; ///< 8-bit signed
typedef uint16_t u16_t; ///< 16-bit unsigned
typedef int16_t i16_t; ///< 16-bit signed
typedef uint32_t u32_t; ///< 32-bit unsigned
typedef int32_t i32_t; ///< 32-bit signed
typedef float fl_t; ///< float type
#define __USR_BKPT() __asm__ __volatile__("BKPT")
/**
* @brief CAN Speed in KBit/s
*/
typedef enum CAN_SPEED {
CAN_SPD_1000,
CAN_SPD_800,
CAN_SPD_500,
CAN_SPD_250,
CAN_SPD_125,
CAN_SPD_100,
CAN_SPD_50,
CAN_SPD_20,
CAN_SPD_10,
} CAN_SPEED;
void CAN_SetSpeed(CAN_SPEED spd, CAN_HandleTypeDef *can); //
void CAN_SetMode(bool is_silent, CAN_HandleTypeDef *can); //
void CAN_SetLoopback(bool is_loopback, CAN_HandleTypeDef *can); //
bool CAN_IsOpened(CAN_HandleTypeDef *can); //
bool CAN_IsSilent(CAN_HandleTypeDef *can); //
bool CAN_IsLoopback(CAN_HandleTypeDef *can); //
void CAN_SetFilterMode(bool id_only, CAN_HandleTypeDef *can); //
void CAN_SetFilterID(u8_t *filt_id, CAN_HandleTypeDef *can); //
void CAN_SetFilterMask(u8_t *filt_mask, CAN_HandleTypeDef *can); //
void CAN_Transmit(u32_t id, u8_t *data, u8_t len, CAN_HandleTypeDef *can); //
void CAN_TransmitExt(u32_t id, u8_t *data, u8_t len, CAN_HandleTypeDef *can); //
void CAN_TransmitRTR(u32_t id, u8_t len, CAN_HandleTypeDef *can); //
void CAN_TransmitExtRTR(u32_t id, u8_t len, CAN_HandleTypeDef *can); //
#endif //USBCANV1_CAN_H

View File

@@ -21,7 +21,7 @@
//#define ADDR_FLASH_PAGE_2 ((uint32_t)0x0807E000) /* Base @ of Page 252, 2 Kbytes */ //#define ADDR_FLASH_PAGE_2 ((uint32_t)0x0807E000) /* Base @ of Page 252, 2 Kbytes */
#define ADDR_EEPROM_PAGE_0 ((uint32_t)0x0803E800) /* Base @ of Page 125, 2 Kbytes */ #define ADDR_EEPROM_PAGE_0 ((uint32_t)0x0803E000) /* Base @ of Page 125, 2 Kbytes */
#define ADDR_EEPROM_PAGE_1 ((uint32_t)0x0803F000) /* Base @ of Page 126, 2 Kbytes */ #define ADDR_EEPROM_PAGE_1 ((uint32_t)0x0803F000) /* Base @ of Page 126, 2 Kbytes */
//#define ADDR_EEPROM_PAGE_2 ((uint32_t)0x0802B000) /* Base @ of Page 86, 2 Kbytes */ //#define ADDR_EEPROM_PAGE_2 ((uint32_t)0x0802B000) /* Base @ of Page 86, 2 Kbytes */
@@ -287,7 +287,7 @@
//#define ADDR_FLASH_PAGE_255 ((uint32_t)0x0807F800) /* Base @ of Page 255, 2 Kbytes */ //#define ADDR_FLASH_PAGE_255 ((uint32_t)0x0807F800) /* Base @ of Page 255, 2 Kbytes */
/* Define the size of the sectors to be used */ /* Define the size of the sectors to be used */
#define PAGE_SIZE (uint32_t)FLASH_PAGE_SIZE /* Page size */ #define PAGE_SIZE (uint32_t)FLASH_PAGE_SIZE * 2 /* Page size */ // Extended to 4KB
/* EEPROM start address in Flash */ /* EEPROM start address in Flash */
#define EEPROM_START_ADDRESS ((uint32_t)ADDR_EEPROM_PAGE_0) /* EEPROM emulation start address */ #define EEPROM_START_ADDRESS ((uint32_t)ADDR_EEPROM_PAGE_0) /* EEPROM emulation start address */

View File

@@ -24,7 +24,7 @@
// Settings // Settings
#define PACKET_RX_TIMEOUT 10 #define PACKET_RX_TIMEOUT 10
#define PACKET_HANDLERS 2 #define PACKET_HANDLERS 1
#define PACKET_MAX_PL_LEN 1024 #define PACKET_MAX_PL_LEN 1024
// Functions // Functions

View File

@@ -152,6 +152,11 @@ typedef struct {
bool heatingOutputChecked; bool heatingOutputChecked;
int16_t heatingStartThreshold; int16_t heatingStartThreshold;
int16_t heatingStopThreshold; int16_t heatingStopThreshold;
float floatCurrentK1; // First factor of current calculation
float floatCurrentK2; // Second factor of current calculation
// uint8_t dummy;
} modConfigGeneralConfigStructTypedef; } modConfigGeneralConfigStructTypedef;
modConfigGeneralConfigStructTypedef* modConfigInit(void); modConfigGeneralConfigStructTypedef* modConfigInit(void);

View File

@@ -44,7 +44,7 @@
#define NoOfExpPossibleOnBMS 72 #define NoOfExpPossibleOnBMS 72
#define NoOfExpMonitorPossibleOnBMS 9 #define NoOfExpMonitorPossibleOnBMS 9
#define NoOfTempSensors 14 #define NoOfTempSensors 16
#define VinErrorThreshold 10 #define VinErrorThreshold 10
typedef enum { typedef enum {

View File

@@ -11,7 +11,7 @@
#include "libPacket.h" #include "libPacket.h"
#include "modCommands.h" #include "modCommands.h"
#define PACKET_HANDLER_UART 1 #define PACKET_HANDLER_UART 0
typedef enum { typedef enum {
MESSAGE_DEBUG = 0, MESSAGE_DEBUG = 0,

330
Core/Src/can.c Normal file
View File

@@ -0,0 +1,330 @@
//
// Created by enik on 02.06.22.
//
#include "can.h"
//#include "Settings.h"
#if CAN_TESTING_LOOPBACK
#define CAN_BASIC_MODE CAN_MODE_SILENT_LOOPBACK
#else
#define CAN_BASIC_MODE CAN_MODE_NORMAL
#endif
//#include "can_messenger.h"
extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;
CAN_FilterTypeDef can1Filter;
CAN_FilterTypeDef can2Filter;
CAN_RxHeaderTypeDef can1RX, can2RX;
u8_t can1Data[8] = {0,}, can2Data[8] = {0,};
typedef struct CAN_SPD_VAL {
u32_t Prescaler;
u32_t TimeSeg1;
u32_t TimeSeg2;
} CAN_SPD_VAL;
// PSC - 2 | Time1 - 15 | Time2 - 2
const CAN_SPD_VAL spd1000 = {.Prescaler = 2, .TimeSeg1 = CAN_BS1_15TQ, .TimeSeg2 = CAN_BS2_2TQ};
// PSC - 3 | Time1 - 12 | Time2 - 2
const CAN_SPD_VAL spd800 = {.Prescaler = 3, .TimeSeg1 = CAN_BS1_12TQ, .TimeSeg2 = CAN_BS2_2TQ};
// PSC - 4 | Time1 - 15 | Time2 - 2
const CAN_SPD_VAL spd500 = {.Prescaler = 4, .TimeSeg1 = CAN_BS1_15TQ, .TimeSeg2 = CAN_BS2_2TQ};
// PSC - 8 | Time1 - 15 | Time2 - 2
const CAN_SPD_VAL spd250 = {.Prescaler = 8, .TimeSeg1 = CAN_BS1_15TQ, .TimeSeg2 = CAN_BS2_2TQ};
// PSC - 16 | Time1 - 15 | Time2 - 2
const CAN_SPD_VAL spd125 = {.Prescaler = 16, .TimeSeg1 = CAN_BS1_15TQ, .TimeSeg2 = CAN_BS2_2TQ};
// PSC - 20 | Time1 - 15 | Time2 - 2
const CAN_SPD_VAL spd100 = {.Prescaler = 20, .TimeSeg1 = CAN_BS1_15TQ, .TimeSeg2 = CAN_BS2_2TQ};
// PSC - 40 | Time1 - 15 | Time2 - 2
const CAN_SPD_VAL spd50 = {.Prescaler = 40, .TimeSeg1 = CAN_BS1_15TQ, .TimeSeg2 = CAN_BS2_2TQ};
// PSC - 100 | Time1 - 15 | Time2 - 2
const CAN_SPD_VAL spd20 = {.Prescaler = 100, .TimeSeg1 = CAN_BS1_15TQ, .TimeSeg2 = CAN_BS2_2TQ};
// PSC - 200 | Time1 - 15 | Time2 - 2
const CAN_SPD_VAL spd10 = {.Prescaler = 200, .TimeSeg1 = CAN_BS1_15TQ, .TimeSeg2 = CAN_BS2_2TQ};
/**
* @brief Set Speed for CAN Bus
* @param[in] spd #CAN_SPEED enum
* @param[in] can Pointer to HAL CAN_HandleTypeDef
*/
void CAN_SetSpeed(CAN_SPEED spd, CAN_HandleTypeDef *can) {
//if (HAL_CAN_GetState(can) == HAL_CAN_STATE_LISTENING)
HAL_CAN_Stop(can);
can->Instance = (can == &hcan1) ? CAN1 : CAN2; // Can Instance
can->Init.SyncJumpWidth = CAN_SJW_1TQ; // Default
can->Init.TimeTriggeredMode = DISABLE; // Default
can->Init.AutoBusOff = ENABLE; // Default
can->Init.AutoWakeUp = ENABLE; // Default
can->Init.AutoRetransmission = DISABLE; // Default
can->Init.ReceiveFifoLocked = DISABLE; // Default
can->Init.TransmitFifoPriority = ENABLE; // Default
can->Init.Mode = CAN_BASIC_MODE; // Set Mode
switch (spd) {
case CAN_SPD_1000:
can->Init.Prescaler = spd1000.Prescaler;
can->Init.TimeSeg1 = spd1000.TimeSeg1;
can->Init.TimeSeg2 = spd1000.TimeSeg2;
break;
case CAN_SPD_800:
can->Init.Prescaler = spd800.Prescaler;
can->Init.TimeSeg1 = spd800.TimeSeg1;
can->Init.TimeSeg2 = spd800.TimeSeg2;
break;
case CAN_SPD_500:
can->Init.Prescaler = spd500.Prescaler;
can->Init.TimeSeg1 = spd500.TimeSeg1;
can->Init.TimeSeg2 = spd500.TimeSeg2;
break;
case CAN_SPD_250:
can->Init.Prescaler = spd250.Prescaler;
can->Init.TimeSeg1 = spd250.TimeSeg1;
can->Init.TimeSeg2 = spd250.TimeSeg2;
break;
case CAN_SPD_125:
can->Init.Prescaler = spd125.Prescaler;
can->Init.TimeSeg1 = spd125.TimeSeg1;
can->Init.TimeSeg2 = spd125.TimeSeg2;
break;
case CAN_SPD_100:
can->Init.Prescaler = spd100.Prescaler;
can->Init.TimeSeg1 = spd100.TimeSeg1;
can->Init.TimeSeg2 = spd100.TimeSeg2;
break;
case CAN_SPD_50:
can->Init.Prescaler = spd50.Prescaler;
can->Init.TimeSeg1 = spd50.TimeSeg1;
can->Init.TimeSeg2 = spd50.TimeSeg2;
break;
case CAN_SPD_20:
can->Init.Prescaler = spd20.Prescaler;
can->Init.TimeSeg1 = spd20.TimeSeg1;
can->Init.TimeSeg2 = spd20.TimeSeg2;
break;
case CAN_SPD_10:
can->Init.Prescaler = spd10.Prescaler;
can->Init.TimeSeg1 = spd10.TimeSeg1;
can->Init.TimeSeg2 = spd10.TimeSeg2;
break;
default:
break;
}
if (HAL_CAN_Init(can) != HAL_OK) {
Error_Handler();
}
CAN_FilterTypeDef *filt;
if (can == &hcan1) {
filt = &can1Filter;
filt->FilterBank = 0;
filt->FilterFIFOAssignment = CAN_RX_FIFO0;
} else if (can == &hcan2) {
filt = &can2Filter;
filt->FilterBank = 14;
filt->FilterFIFOAssignment = CAN_RX_FIFO1;
filt->SlaveStartFilterBank = 14;
} else {
return;
}
filt->FilterMode = CAN_FILTERMODE_IDMASK;
filt->FilterScale = CAN_FILTERSCALE_32BIT;
filt->FilterIdHigh = 0x0000;
filt->FilterIdLow = 0x0000;
filt->FilterMaskIdHigh = 0x0000;
filt->FilterMaskIdLow = 0x0000;
filt->FilterActivation = ENABLE;
if (HAL_CAN_ConfigFilter(can, filt) != HAL_OK) {
Error_Handler();
}
// HAL_CAN_Start(can);
volatile HAL_StatusTypeDef status = HAL_CAN_Start(can);
__NOP();
volatile HAL_CAN_StateTypeDef st = HAL_CAN_GetState(can);
if (st != HAL_CAN_STATE_LISTENING){
__USR_BKPT();
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_12, GPIO_PIN_SET);
} else {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_14, GPIO_PIN_SET);
}
HAL_CAN_ActivateNotification(can,
((can == &hcan1) ? CAN_IT_RX_FIFO0_MSG_PENDING : CAN_IT_RX_FIFO1_MSG_PENDING)
| CAN_IT_ERROR | CAN_IT_BUSOFF | CAN_IT_LAST_ERROR_CODE);
}
/**
* @brief Set Silent mode
* @param[in] is_silent 1 if silent mode needed
* @param[in] can Pointer to HAL CAN_HandleTypeDef
*/
void CAN_SetMode(bool is_silent, CAN_HandleTypeDef *can) {
can->Init.Mode = !is_silent ? CAN_MODE_NORMAL : CAN_MODE_SILENT;
}
/**
* @brief Set Silent Loopback mode
* @param[in] is_loopback 1 if loopback mode needed
* @param[in] can Pointer to HAL CAN_HandleTypeDef
*/
void CAN_SetLoopback(bool is_loopback, CAN_HandleTypeDef *can) {
can->Init.Mode = !is_loopback ? CAN_MODE_NORMAL : CAN_MODE_SILENT_LOOPBACK;
}
/**
* @brief Check if CAN is opened
* @param[in] can Pointer to HAL CAN_HandleTypeDef
* @return 1 if Opened / 0 if Closed
*/
bool CAN_IsOpened(CAN_HandleTypeDef *can) {
return HAL_CAN_GetState(can) == HAL_CAN_STATE_LISTENING ? 1 : 0;
}
/**
* @brief Check if CAN is in silent mode
* @param[in] can Pointer to HAL CAN_HandleTypeDef
* @return 1 if Silent / 0 if not
*/
bool CAN_IsSilent(CAN_HandleTypeDef *can) {
return (HAL_CAN_GetState(can) == HAL_CAN_STATE_LISTENING &&
(can->Init.Mode == CAN_MODE_SILENT ||
can->Init.Mode == CAN_MODE_SILENT_LOOPBACK)) ? 0 : 1;
}
/**
* @brief Check if CAN is in silent loopbsck mode
* @param[in] can Pointer to HAL CAN_HandleTypeDef
* @return 1 if Silent Loopback / 0 if else
*/
bool CAN_IsLoopback(CAN_HandleTypeDef *can) {
return (can->Init.Mode == CAN_MODE_SILENT_LOOPBACK) ? 1 : 0;
}
/**
* @brief Set filter mode
* @param[in] id_only 1 if ID_LIST mode / 0 if ID_MASK mode
* @param[in] can Pointer to HAL CAN_HandleTypeDef
*/
void CAN_SetFilterMode(bool id_only, CAN_HandleTypeDef *can) {
CAN_FilterTypeDef *filt;
if (can == &hcan1) {
filt = &can1Filter;
} else if (can == &hcan2) {
filt = &can2Filter;
} else {
return;
}
filt->FilterMode = id_only ? CAN_FILTERMODE_IDLIST : CAN_FILTERMODE_IDMASK;
}
/**
* @brief Set ID for filter
* @warning Not realized
* @param[in] filt_id Pointer to filter ID array
* @param[in] can Pointer to HAL CAN_HandleTypeDef
*/
void CAN_SetFilterID(u8_t *filt_id, CAN_HandleTypeDef *can) {
CAN_FilterTypeDef *filt;
if (can == &hcan1) {
filt = &can1Filter;
} else if (can == &hcan2) {
filt = &can2Filter;
} else {
return;
}
}
/**
* @brief Set Mask for filter
* @warning Not realized
* @param[in] filt_mask Pointer to filter mask
* @param[in] can Pointer to HAL CAN_HandleTypeDef
*/
void CAN_SetFilterMask(u8_t *filt_mask, CAN_HandleTypeDef *can) {
CAN_FilterTypeDef *filt;
if (can == &hcan1) {
filt = &can1Filter;
} else if (can == &hcan2) {
filt = &can2Filter;
} else {
return;
}
}
/**
* @brief Common transmit prototype
* @param[in] id CAN Packet ID
* @param[in] ext 0 if 11-bit ID / 1 if 29-bit ID
* @param[in] rtr 0 if standard frame / 1 if remote frame
* @param[in] len Length of payload
* @param[in] data Pointer to payload array
* @param[in] can Pointer to HAL CAN_HandleTypeDef
*/
void CAN_Transmit_Proto(u32_t id, bool ext, bool rtr, u8_t len, u8_t *data, CAN_HandleTypeDef *can) {
CAN_TxHeaderTypeDef tx;
tx.StdId = !ext ? id : 0;
tx.ExtId = ext ? id : 0;
tx.RTR = !rtr ? CAN_RTR_DATA : CAN_RTR_REMOTE;
tx.IDE = !ext ? CAN_ID_STD : CAN_ID_EXT;
tx.DLC = len;
tx.TransmitGlobalTime = 0;
u32_t mb;
HAL_CAN_AddTxMessage(can, &tx, data, &mb);
}
/**
* @brief Transmit standard (11-bit) data frame
* @param[in] id 11-bit frame ID
* @param[in] data Pointer to payload to be sent
* @param[in] len Length of the payload to be sent
* @param[in] can Pointer to HAL CAN_HandleTypeDef
*/
void CAN_Transmit(u32_t id, u8_t *data, u8_t len, CAN_HandleTypeDef *can) {
CAN_Transmit_Proto((u32_t) id, 0, 0, len, data, can);
}
/**
* @brief Transmit extended (29-bit) data frame
* @param[in] id 29-bit frame ID
* @param[in] data Pointer to payload to be sent
* @param[in] len Length of the payload to be sent
* @param[in] can Pointer to HAL CAN_HandleTypeDef
*/
void CAN_TransmitExt(u32_t id, u8_t *data, u8_t len, CAN_HandleTypeDef *can) {
CAN_Transmit_Proto(id, 1, 0, len, data, can);
}
/**
* @brief Transmit standard (11-bit) remote frame
* @param[in] id 11-bit frame ID
* @param[in] len Length of remote frame
* @param[in] can Pointer to HAL CAN_HandleTypeDef
*/
void CAN_TransmitRTR(u32_t id, u8_t len, CAN_HandleTypeDef *can) {
CAN_Transmit_Proto((u32_t) id, 0, 1, len, NULL, can);
}
/**
* @brief Transmit extended (29-bit) remote frame
* @param[in] id 29-bit frame ID
* @param[in] len Length of remote frame
* @param[in] can Pointer to HAL CAN_HandleTypeDef
*/
void CAN_TransmitExtRTR(u32_t id, u8_t len, CAN_HandleTypeDef *can) {
CAN_Transmit_Proto((u32_t) id, 1, 1, len, NULL, can);
}
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &can1RX, can1Data) == HAL_OK) {
can_irq_receive(&can1RX, can1Data, 1);
}
}
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) {
if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO1, &can2RX, can2Data) == HAL_OK) {
can_irq_receive(&can2RX, can2Data, 2);
}
}

View File

@@ -40,7 +40,7 @@ uint16_t driverHWEEPROMInit(uint16_t numberOfVars) {
/* Fill EraseInit structure*/ /* Fill EraseInit structure*/
s_eraseinit.TypeErase = FLASH_TYPEERASE_PAGES; s_eraseinit.TypeErase = FLASH_TYPEERASE_PAGES;
s_eraseinit.PageAddress = PAGE0_BASE_ADDRESS; s_eraseinit.PageAddress = PAGE0_BASE_ADDRESS;
s_eraseinit.NbPages = 1; s_eraseinit.NbPages = 2;
/* Check for invalid header states and repair if necessary */ /* Check for invalid header states and repair if necessary */
switch (pagestatus0) switch (pagestatus0)
@@ -127,7 +127,7 @@ uint16_t driverHWEEPROMInit(uint16_t numberOfVars) {
} }
s_eraseinit.TypeErase = FLASH_TYPEERASE_PAGES; s_eraseinit.TypeErase = FLASH_TYPEERASE_PAGES;
s_eraseinit.PageAddress = PAGE1_BASE_ADDRESS; s_eraseinit.PageAddress = PAGE1_BASE_ADDRESS;
s_eraseinit.NbPages = 1; s_eraseinit.NbPages = 2;
/* Erase Page1 */ /* Erase Page1 */
if(!driverHWEEPROMVerifyPageFullyErased(PAGE1_BASE_ADDRESS)) if(!driverHWEEPROMVerifyPageFullyErased(PAGE1_BASE_ADDRESS))
{ {
@@ -143,7 +143,7 @@ uint16_t driverHWEEPROMInit(uint16_t numberOfVars) {
{ {
s_eraseinit.TypeErase = FLASH_TYPEERASE_PAGES; s_eraseinit.TypeErase = FLASH_TYPEERASE_PAGES;
s_eraseinit.PageAddress = PAGE1_BASE_ADDRESS; s_eraseinit.PageAddress = PAGE1_BASE_ADDRESS;
s_eraseinit.NbPages = 1; s_eraseinit.NbPages = 2;
/* Erase Page1 */ /* Erase Page1 */
if(!driverHWEEPROMVerifyPageFullyErased(PAGE1_BASE_ADDRESS)) if(!driverHWEEPROMVerifyPageFullyErased(PAGE1_BASE_ADDRESS))
{ {
@@ -189,7 +189,7 @@ uint16_t driverHWEEPROMInit(uint16_t numberOfVars) {
{ {
s_eraseinit.TypeErase = FLASH_TYPEERASE_PAGES; s_eraseinit.TypeErase = FLASH_TYPEERASE_PAGES;
s_eraseinit.PageAddress = PAGE1_BASE_ADDRESS; s_eraseinit.PageAddress = PAGE1_BASE_ADDRESS;
s_eraseinit.NbPages = 1; s_eraseinit.NbPages = 2;
/* Erase Page1 */ /* Erase Page1 */
if(!driverHWEEPROMVerifyPageFullyErased(PAGE1_BASE_ADDRESS)) if(!driverHWEEPROMVerifyPageFullyErased(PAGE1_BASE_ADDRESS))
{ {
@@ -236,7 +236,7 @@ uint16_t driverHWEEPROMInit(uint16_t numberOfVars) {
} }
s_eraseinit.TypeErase = FLASH_TYPEERASE_PAGES; s_eraseinit.TypeErase = FLASH_TYPEERASE_PAGES;
s_eraseinit.PageAddress = PAGE0_BASE_ADDRESS; s_eraseinit.PageAddress = PAGE0_BASE_ADDRESS;
s_eraseinit.NbPages = 1; s_eraseinit.NbPages = 2;
/* Erase Page0 */ /* Erase Page0 */
if(!driverHWEEPROMVerifyPageFullyErased(PAGE0_BASE_ADDRESS)) if(!driverHWEEPROMVerifyPageFullyErased(PAGE0_BASE_ADDRESS))
{ {
@@ -419,7 +419,7 @@ static HAL_StatusTypeDef driverHWEEPROMFormat(void)
s_eraseinit.TypeErase = FLASH_TYPEERASE_PAGES; s_eraseinit.TypeErase = FLASH_TYPEERASE_PAGES;
s_eraseinit.PageAddress = PAGE0_BASE_ADDRESS; s_eraseinit.PageAddress = PAGE0_BASE_ADDRESS;
s_eraseinit.NbPages = 1; s_eraseinit.NbPages = 2;
/* Erase Page0 */ /* Erase Page0 */
if(!driverHWEEPROMVerifyPageFullyErased(PAGE0_BASE_ADDRESS)) if(!driverHWEEPROMVerifyPageFullyErased(PAGE0_BASE_ADDRESS))
{ {
@@ -669,7 +669,7 @@ static uint16_t driverHWEEPROMPageTransfer(uint16_t VirtAddress, uint16_t Data)
s_eraseinit.TypeErase = FLASH_TYPEERASE_PAGES; s_eraseinit.TypeErase = FLASH_TYPEERASE_PAGES;
s_eraseinit.PageAddress = oldpageid; s_eraseinit.PageAddress = oldpageid;
s_eraseinit.NbPages = 1; s_eraseinit.NbPages = 2;
/* Erase the old Page: Set old Page status to ERASED status */ /* Erase the old Page: Set old Page status to ERASED status */
flashstatus = HAL_FLASHEx_Erase(&s_eraseinit, &page_error); flashstatus = HAL_FLASHEx_Erase(&s_eraseinit, &page_error);

View File

@@ -783,7 +783,7 @@ float driverSWLTC6804ConvertTemperatureExt(uint16_t inputValue,uint32_t ntcNomin
steinhart -= 273.15f; // convert to degree steinhart -= 273.15f; // convert to degree
if(steinhart < -50.0f || (float)inputValue >= 30000.0f) if(steinhart < -50.0f || (float)inputValue >= 30000.0f)
steinhart = 100.0f; steinhart = 0.0f;
return steinhart; return steinhart;
} }

View File

@@ -38,6 +38,7 @@
#include "DPC_Timeout.h" #include "DPC_Timeout.h"
#include "GSM.h" #include "GSM.h"
#include "time.h" #include "time.h"
#include "can.h"
#include "SD_Card.h" #include "SD_Card.h"
@@ -272,9 +273,13 @@ uint16_t load_current = 0;
uint16_t charge_current = 0; uint16_t charge_current = 0;
float float_current = 0; float float_current = 0;
uint64_t output_control_clock = 0;
bool need_shunt_charging_contractor = false; bool need_shunt_charging_contractor = false;
uint64_t shunt_charging_contractor_clock = 0; uint64_t shunt_charging_contractor_clock = 0;
uint64_t can1_transmit_clock = 0;
uint64_t can2_transmit_clock = 0;
#define __MA_WIN_SIZE (500) #define __MA_WIN_SIZE (500)
#define __MA_ARRAY_QTY 2 #define __MA_ARRAY_QTY 2
@@ -289,7 +294,7 @@ static void MX_CAN1_Init(void);
static void MX_CAN2_Init(void); static void MX_CAN2_Init(void);
static void MX_DAC_Init(void); static void MX_DAC_Init(void);
static void MX_SPI1_Init(void); static void MX_SPI1_Init(void);
static void MX_SPI3_Init(void); //static void MX_SPI3_Init(void);
static void MX_USART2_UART_Init(void); static void MX_USART2_UART_Init(void);
static void MX_TIM6_Init(void); static void MX_TIM6_Init(void);
static void MX_TIM2_Init(void); static void MX_TIM2_Init(void);
@@ -447,42 +452,38 @@ void mainWatchDogInitAndStart(void) {
} }
void Get_Shunt_Current() { void Get_Shunt_Current() {
// static uint16_t adc;
// static uint16_t adc_average[100];
static uint16_t adc; // static uint8_t adc_average_cnt = 0;
static uint16_t adc_average[100]; // static uint32_t adc_average_sum = 0;
static uint8_t adc_average_cnt = 0;
static uint32_t adc_average_sum = 0;
static uint32_t adc_average_res = 0; static uint32_t adc_average_res = 0;
currentZero_config = generalConfig->shuntLCFactor; currentZero_config = generalConfig->shuntLCFactor;
HAL_ADC_Start(&hadc2); HAL_ADC_Start(&hadc2);
adc_average_res = MA_Filter((HAL_ADC_GetValue(&hadc2)), 1); adc_average_res = MA_Filter((HAL_ADC_GetValue(&hadc2)), 1);
export_adc_average_res = adc_average_res; export_adc_average_res = adc_average_res;
//export_adc_average_res = 3033; //export_adc_average_res = 3033;
if(adc_average_res >= _ADC_ZERO){
if((adc_average_res - _ADC_ZERO) > 10) { // load
//load_current = (adc_average_res - 3060) * 0.34;
float_current = (adc_average_res - _ADC_ZERO) * 0.777;
float_current = -float_current;
} else {
load_current = 0;
charge_current = 0;
float_current = 0;
}
} else {
if ((_ADC_ZERO - adc_average_res) > 10) {
//charge_current = ;
float_current = (_ADC_ZERO - adc_average_res) * 0.777;//charge_current;
} else {
load_current = 0;
charge_current = 0;
float_current = 0;
}
}
if(adc_average_res >= _ADC_ZERO){
if((adc_average_res - _ADC_ZERO) > 10) { // load
//load_current = (adc_average_res - 3060) * 0.34;
float_current = (adc_average_res - _ADC_ZERO + generalConfig->floatCurrentK1) * generalConfig->floatCurrentK2;
float_current = -float_current;
} else {
load_current = 0;
charge_current = 0;
float_current = 0;
}
} else {
if ((_ADC_ZERO - adc_average_res) > 10) {
//charge_current = ;
float_current = (_ADC_ZERO - adc_average_res + generalConfig->floatCurrentK1) * generalConfig->floatCurrentK2;
} else {
load_current = 0;
charge_current = 0;
float_current = 0;
}
}
} }
void USB_Check_Timeout() { void USB_Check_Timeout() {
@@ -998,32 +999,37 @@ void updateLimitsFromMem(){
void outputControl() void outputControl()
{ {
if ((TIM_Clock - output_control_clock) < 100)
{
return;
}
// 2 output handle // 2 output handle
if (generalConfig->chargeBatteryOutputChecked) if (generalConfig->chargeBatteryOutputChecked)
{ {
GPIO_PinState chargeContactorState = HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_0); GPIO_PinState chargeContactorState = HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_5);
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_2, chargeContactorState); HAL_GPIO_WritePin(GPIOE, GPIO_PIN_0, chargeContactorState);
} }
else else
{ {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_2, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOE, GPIO_PIN_0, GPIO_PIN_RESET);
} }
// 3 output handle // 3 output handle
if (!generalConfig->brushOrShuntMode) // brush mode if (!generalConfig->brushOrShuntMode) // brush mode
{ {
if (packState.SoC <= generalConfig->brushUsageSocThreshold) if (packState.SoC <= generalConfig->brushUsageSocThreshold)
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_3, generalConfig->brushOrShuntOutputChecked ? GPIO_PIN_RESET : GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOE, GPIO_PIN_1, generalConfig->brushOrShuntOutputChecked ? GPIO_PIN_RESET : GPIO_PIN_SET);
else else
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_3, generalConfig->brushOrShuntOutputChecked ? GPIO_PIN_SET : GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOE, GPIO_PIN_1, generalConfig->brushOrShuntOutputChecked ? GPIO_PIN_SET : GPIO_PIN_RESET);
Brush_Status = HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_3); Brush_Status = HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_1);
} }
else // shunt mode else // shunt mode
{ {
if (generalConfig->brushOrShuntOutputChecked) if (generalConfig->brushOrShuntOutputChecked)
{ {
GPIO_PinState loadContactorState = HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_1); GPIO_PinState loadContactorState = HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_4);
if (loadContactorState == GPIO_PIN_SET && !need_shunt_charging_contractor) if (loadContactorState == GPIO_PIN_SET && !need_shunt_charging_contractor)
{ {
need_shunt_charging_contractor = true; need_shunt_charging_contractor = true;
@@ -1033,18 +1039,18 @@ void outputControl()
if (loadContactorState == GPIO_PIN_SET && need_shunt_charging_contractor && if (loadContactorState == GPIO_PIN_SET && need_shunt_charging_contractor &&
(TIM_Clock - shunt_charging_contractor_clock) >= generalConfig->shuntChargingContactorDelay * 1000) (TIM_Clock - shunt_charging_contractor_clock) >= generalConfig->shuntChargingContactorDelay * 1000)
{ {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_3, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOE, GPIO_PIN_1, GPIO_PIN_SET);
} }
if (loadContactorState == GPIO_PIN_RESET) if (loadContactorState == GPIO_PIN_RESET)
{ {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_3, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOE, GPIO_PIN_1, GPIO_PIN_RESET);
need_shunt_charging_contractor = false; need_shunt_charging_contractor = false;
} }
} }
else else
{ {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_3, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOE, GPIO_PIN_1, GPIO_PIN_RESET);
need_shunt_charging_contractor = false; need_shunt_charging_contractor = false;
} }
} }
@@ -1053,31 +1059,121 @@ void outputControl()
if (generalConfig->coolingOutputChecked) if (generalConfig->coolingOutputChecked)
{ {
if (packState.tempBatteryHigh <= generalConfig->coolingStopThreshold) if (packState.tempBatteryHigh <= generalConfig->coolingStopThreshold)
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_4, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOE, GPIO_PIN_2, GPIO_PIN_RESET);
if (packState.tempBatteryHigh >= generalConfig->coolingStartThreshold) if (packState.tempBatteryHigh >= generalConfig->coolingStartThreshold)
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_4, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOE, GPIO_PIN_2, GPIO_PIN_SET);
} }
else else
{ {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_4, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOE, GPIO_PIN_2, GPIO_PIN_RESET);
} }
// 5 output handle // 5 output handle
if (generalConfig->heatingOutputChecked) if (generalConfig->heatingOutputChecked)
{ {
if (packState.tempBatteryLow >= generalConfig->heatingStopThreshold) if (packState.tempBatteryLow >= generalConfig->heatingStopThreshold)
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_5, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOE, GPIO_PIN_3, GPIO_PIN_RESET);
if (packState.tempBatteryLow <= generalConfig->heatingStartThreshold) if (packState.tempBatteryLow <= generalConfig->heatingStartThreshold)
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_5, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOE, GPIO_PIN_3, GPIO_PIN_SET);
} }
else else
{ {
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_5, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOE, GPIO_PIN_3, GPIO_PIN_RESET);
} }
output_control_clock = TIM_Clock;
} }
void transmitCan1Packet()
{
if ((TIM_Clock - can1_transmit_clock) < 1000)
{
return;
}
uint8_t data1[8] = {0x45};
CAN_Transmit(300, data1, 8, &hcan1);
return;
CAN_TxHeaderTypeDef header;
header.IDE = CAN_ID_STD;
header.RTR = CAN_RTR_DATA;
header.StdId = 0x444;
header.DLC = 1;
uint8_t data[32] = {};
uint32_t mailbox = 0;
// sending SOC, SOH and number of cells
uint16_t id = 0x101;
memcpy(data, &id, sizeof(id));
memcpy(data + 2, &packState.SoC, sizeof(packState.SoC));
memcpy(data + 6, &packState.SoCCapacityAh, sizeof(packState.SoCCapacityAh));
memcpy(data + 10, &generalConfig->noOfCellsSeries, sizeof(generalConfig->noOfCellsSeries));
header.DLC = 11;
HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);
// sending charge current, discharge current // TODO
id = 0x102;
memcpy(data, &id, sizeof(id));
memcpy(data + 2, &packState.packCurrent, sizeof(packState.packCurrent));
memcpy(data + 6, &packState.loCurrentLoadCurrent, sizeof(packState.loCurrentLoadCurrent));
header.DLC = 10;
HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);
// sending BMS state, input state, output state // TODO
id = 0x103;
memcpy(data, &id, sizeof(id));
memcpy(data + 2, &packState.cellVoltageLow, sizeof(packState.cellVoltageLow));
memcpy(data + 6, &packState.cellVoltageAverage, sizeof(packState.cellVoltageAverage));
memcpy(data + 10, &packState.cellVoltageHigh, sizeof(packState.cellVoltageHigh));
header.DLC = 12;
HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);
// sending cell voltages
id = 0x200;
for (int cellPointer = 0; cellPointer < generalConfig->noOfCellsSeries * generalConfig->noOfParallelModules; ++cellPointer)
{
++id;
memcpy(data, &id, sizeof(id));
float voltage = 0;
if (packState.cellVoltagesIndividual[cellPointer].cellBleedActive)
voltage = packState.cellVoltagesIndividual[cellPointer].cellVoltage * -1000;
else
voltage = packState.cellVoltagesIndividual[cellPointer].cellVoltage * 1000;
memcpy(data + 2, &voltage, sizeof(voltage));
header.DLC = 6;
HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);
}
// sending temperatures
id = 0x300;
for (int sensorPointer = 0; sensorPointer < NoOfTempSensors; ++sensorPointer)
{
++id;
memcpy(data, &id, sizeof(id));
float temperature = packState.temperatures[sensorPointer];
memcpy(data + 2, &temperature, sizeof(temperature));
header.DLC = 6;
HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);
}
can1_transmit_clock = TIM_Clock;
}
//
//void transmitCan2Packet()
//{
// if ((TIM_Clock - can2_transmit_clock) < 25)
// {
// return;
// }
//
// can2_transmit_clock = TIM_Clock;
//}
void modem_init(){ void modem_init(){
SIM800_Var_Init(&SIM800_Struct); SIM800_Var_Init(&SIM800_Struct);
@@ -1383,7 +1479,7 @@ uint8_t SS_status(uint8_t type, Time_Struct* Event_time) {
} }
void Update_parameters_for_sim(){ void Update_parameters_for_sim(){
float Service; // float Service;
RTC_Get_Values(&SimStatus_Struct.Actual_data.Ended_At_Time); RTC_Get_Values(&SimStatus_Struct.Actual_data.Ended_At_Time);
SimStatus_Struct.Actual_data.Battery_Level_At_End = (uint16_t) packState.SoC; SimStatus_Struct.Actual_data.Battery_Level_At_End = (uint16_t) packState.SoC;
@@ -1483,7 +1579,7 @@ void Parameters_Save(){
void Save_data_to_Backup(){ void Save_data_to_Backup(){
Time_Struct CurrentTime; Time_Struct CurrentTime;
Time_Struct Previous_time; // Time_Struct Previous_time;
struct tm tm_CurrenttimeStruct = {0}; struct tm tm_CurrenttimeStruct = {0};
struct tm tm_PrevtimeStruct = {0}; struct tm tm_PrevtimeStruct = {0};
time_t Unixtime1; time_t Unixtime1;
@@ -1491,7 +1587,7 @@ void Save_data_to_Backup(){
uint64_t Unixtime_delta; uint64_t Unixtime_delta;
uint16_t Service_var; uint16_t Service_var;
float Service_float; // float Service_float;
RTC_Get_Values(&CurrentTime); RTC_Get_Values(&CurrentTime);
@@ -1554,8 +1650,8 @@ void Restore_shutdown_data(){
uint16_t LoLoPart; uint16_t LoLoPart;
uint16_t LoHiPart; uint16_t LoHiPart;
uint32_t LoPart; // uint32_t LoPart;
uint32_t HiPart; // uint32_t HiPart;
time_t Unixtime; time_t Unixtime;
uint32_t Time_delta; uint32_t Time_delta;
struct tm tm_timeStruct = {0}; struct tm tm_timeStruct = {0};
@@ -1893,9 +1989,9 @@ void Total_DeInit(){
int main(void) int main(void)
{ {
uint8_t result; // uint8_t result;
// uint8_t Pinstate; // uint8_t Pinstate;
static uint32_t last_ss = 0; // static uint32_t last_ss = 0;
/* USER CODE BEGIN 1 */ /* USER CODE BEGIN 1 */
//SCB->VTOR = 0x0800F000; //SCB->VTOR = 0x0800F000;
@@ -1945,9 +2041,13 @@ int main(void)
MX_ADC2_Init(); MX_ADC2_Init();
// MX_CAN1_Init(); //MX_CAN1_Init();
//MX_CAN2_Init();
// CAN_SetSpeed(CAN_SPD_100, &hcan1);
//HAL_GPIO_WritePin(HL4_GPIO_Port, HL4_Pin, 1); //HAL_GPIO_WritePin(HL4_GPIO_Port, HL4_Pin, 1);
// MX_CAN2_Init();
MX_DAC_Init(); MX_DAC_Init();
MX_SPI1_Init(); MX_SPI1_Init();
@@ -2190,6 +2290,9 @@ int main(void)
// If there is new data handle SoC estimation // If there is new data handle SoC estimation
// mainWatchDogReset(); // mainWatchDogReset();
// transmitCan1Packet();
//transmitCan2Packet();
outputControl(); outputControl();
} }
@@ -2387,21 +2490,26 @@ static void MX_CAN1_Init(void)
/* USER CODE END CAN1_Init 1 */ /* USER CODE END CAN1_Init 1 */
hcan1.Instance = CAN1; hcan1.Instance = CAN1;
hcan1.Init.Prescaler = 16; hcan1.Init.Prescaler = 20;
hcan1.Init.Mode = CAN_MODE_NORMAL; hcan1.Init.Mode = CAN_MODE_NORMAL;
hcan1.Init.SyncJumpWidth = CAN_SJW_1TQ; hcan1.Init.SyncJumpWidth = CAN_SJW_1TQ;
hcan1.Init.TimeSeg1 = CAN_BS1_1TQ; hcan1.Init.TimeSeg1 = CAN_BS1_15TQ;
hcan1.Init.TimeSeg2 = CAN_BS2_1TQ; hcan1.Init.TimeSeg2 = CAN_BS2_2TQ;
hcan1.Init.TimeTriggeredMode = DISABLE; hcan1.Init.TimeTriggeredMode = DISABLE;
hcan1.Init.AutoBusOff = DISABLE; hcan1.Init.AutoBusOff = ENABLE;
hcan1.Init.AutoWakeUp = DISABLE; hcan1.Init.AutoWakeUp = ENABLE;
hcan1.Init.AutoRetransmission = DISABLE; hcan1.Init.AutoRetransmission = ENABLE;
hcan1.Init.ReceiveFifoLocked = DISABLE; hcan1.Init.ReceiveFifoLocked = DISABLE;
hcan1.Init.TransmitFifoPriority = DISABLE; hcan1.Init.TransmitFifoPriority = DISABLE;
if (HAL_CAN_Init(&hcan1) != HAL_OK) if (HAL_CAN_Init(&hcan1) != HAL_OK)
{ {
Error_Handler(); Error_Handler();
} }
if (HAL_CAN_Start(&hcan1) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN CAN1_Init 2 */ /* USER CODE BEGIN CAN1_Init 2 */
/* USER CODE END CAN1_Init 2 */ /* USER CODE END CAN1_Init 2 */
@@ -2439,13 +2547,18 @@ static void MX_CAN2_Init(void)
hcan2.Init.TimeTriggeredMode = DISABLE; hcan2.Init.TimeTriggeredMode = DISABLE;
hcan2.Init.AutoBusOff = DISABLE; hcan2.Init.AutoBusOff = DISABLE;
hcan2.Init.AutoWakeUp = DISABLE; hcan2.Init.AutoWakeUp = DISABLE;
hcan2.Init.AutoRetransmission = DISABLE; hcan2.Init.AutoRetransmission = ENABLE;
hcan2.Init.ReceiveFifoLocked = DISABLE; hcan2.Init.ReceiveFifoLocked = DISABLE;
hcan2.Init.TransmitFifoPriority = DISABLE; hcan2.Init.TransmitFifoPriority = DISABLE;
if (HAL_CAN_Init(&hcan2) != HAL_OK) if (HAL_CAN_Init(&hcan2) != HAL_OK)
{ {
Error_Handler(); Error_Handler();
} }
if (HAL_CAN_Start(&hcan2) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN CAN2_Init 2 */ /* USER CODE BEGIN CAN2_Init 2 */
/* USER CODE END CAN2_Init 2 */ /* USER CODE END CAN2_Init 2 */
@@ -2533,38 +2646,38 @@ static void MX_SPI1_Init(void)
* @param None * @param None
* @retval None * @retval None
*/ */
static void MX_SPI3_Init(void) //static void MX_SPI3_Init(void)
{ //{
//
/* USER CODE BEGIN SPI3_Init 0 */ // /* USER CODE BEGIN SPI3_Init 0 */
//
/* USER CODE END SPI3_Init 0 */ // /* USER CODE END SPI3_Init 0 */
//
/* USER CODE BEGIN SPI3_Init 1 */ // /* USER CODE BEGIN SPI3_Init 1 */
//
/* USER CODE END SPI3_Init 1 */ // /* USER CODE END SPI3_Init 1 */
/* SPI3 parameter configuration*/ // /* SPI3 parameter configuration*/
hspi3.Instance = SPI3; // hspi3.Instance = SPI3;
hspi3.Init.Mode = SPI_MODE_MASTER; // hspi3.Init.Mode = SPI_MODE_MASTER;
hspi3.Init.Direction = SPI_DIRECTION_2LINES; // hspi3.Init.Direction = SPI_DIRECTION_2LINES;
hspi3.Init.DataSize = SPI_DATASIZE_8BIT; // hspi3.Init.DataSize = SPI_DATASIZE_8BIT;
hspi3.Init.CLKPolarity = SPI_POLARITY_LOW; // hspi3.Init.CLKPolarity = SPI_POLARITY_LOW;
hspi3.Init.CLKPhase = SPI_PHASE_2EDGE; // hspi3.Init.CLKPhase = SPI_PHASE_2EDGE;
hspi3.Init.NSS = SPI_NSS_HARD_OUTPUT; // hspi3.Init.NSS = SPI_NSS_HARD_OUTPUT;
hspi3.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256; // hspi3.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256;
hspi3.Init.FirstBit = SPI_FIRSTBIT_MSB; // hspi3.Init.FirstBit = SPI_FIRSTBIT_MSB;
hspi3.Init.TIMode = SPI_TIMODE_DISABLE; // hspi3.Init.TIMode = SPI_TIMODE_DISABLE;
hspi3.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; // hspi3.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
hspi3.Init.CRCPolynomial = 7; // hspi3.Init.CRCPolynomial = 7;
if (HAL_SPI_Init(&hspi3) != HAL_OK) // if (HAL_SPI_Init(&hspi3) != HAL_OK)
{ // {
Error_Handler(); // Error_Handler();
} // }
/* USER CODE BEGIN SPI3_Init 2 */ // /* USER CODE BEGIN SPI3_Init 2 */
//
/* USER CODE END SPI3_Init 2 */ // /* USER CODE END SPI3_Init 2 */
//
} //}
/** /**
* @brief USART2 Initialization Function * @brief USART2 Initialization Function

View File

@@ -289,6 +289,9 @@ void modCommandsProcessPacket(unsigned char *data, unsigned int len) {
modCommandsGeneralConfig->heatingStartThreshold = libBufferGet_int16(data, &ind); modCommandsGeneralConfig->heatingStartThreshold = libBufferGet_int16(data, &ind);
modCommandsGeneralConfig->heatingStopThreshold = libBufferGet_int16(data, &ind); modCommandsGeneralConfig->heatingStopThreshold = libBufferGet_int16(data, &ind);
modCommandsGeneralConfig->floatCurrentK1 = libBufferGet_float32_auto(data,&ind);
modCommandsGeneralConfig->floatCurrentK2 = libBufferGet_float32_auto(data,&ind);
// RawSerialPtr = modCommandsGeneralConfig->displayTimeoutBatteryDead; // RawSerialPtr = modCommandsGeneralConfig->displayTimeoutBatteryDead;
// RawSerial = *RawSerialPtr; // RawSerial = *RawSerialPtr;
// RawSerial = RawSerial<<32; // RawSerial = RawSerial<<32;
@@ -435,6 +438,9 @@ void modCommandsProcessPacket(unsigned char *data, unsigned int len) {
libBufferAppend_int16(modCommandsSendBuffer, modCommandsToBeSendConfig->heatingStartThreshold, &ind); libBufferAppend_int16(modCommandsSendBuffer, modCommandsToBeSendConfig->heatingStartThreshold, &ind);
libBufferAppend_int16(modCommandsSendBuffer, modCommandsToBeSendConfig->heatingStopThreshold, &ind); libBufferAppend_int16(modCommandsSendBuffer, modCommandsToBeSendConfig->heatingStopThreshold, &ind);
libBufferAppend_float32_auto( modCommandsSendBuffer,modCommandsToBeSendConfig->floatCurrentK1 ,&ind);
libBufferAppend_float32_auto( modCommandsSendBuffer,modCommandsToBeSendConfig->floatCurrentK2 ,&ind);
modCommandsSendPacket(modCommandsSendBuffer, ind); modCommandsSendPacket(modCommandsSendBuffer, ind);
break; break;

View File

@@ -172,7 +172,7 @@ void modConfigLoadDefaultConfig(modConfigGeneralConfigStructTypedef *configLocat
configLocation->tempEnableMaskBMS = 0x0001; // Bitwise select what sensor to enable for the BMS (internal sensors). configLocation->tempEnableMaskBMS = 0x0001; // Bitwise select what sensor to enable for the BMS (internal sensors).
configLocation->tempEnableMaskBattery = 0xFFFF; // Bitwise select what sensor to enable for the battery (external sensors). configLocation->tempEnableMaskBattery = 0xFFFF; // Bitwise select what sensor to enable for the battery (external sensors).
configLocation->tempEnableMaskExpansion = 0xFFFF; // Bitwise select what sensor to enable for the expansion boards(external sensors). configLocation->tempEnableMaskExpansion = 0xFFFF; // Bitwise select what sensor to enable for the expansion boards(external sensors).
configLocation->noOfTempSensorPerModule = 2; // Number of temperature sensors monitored per LTC68XX configLocation->noOfTempSensorPerModule = 4; // Number of temperature sensors monitored per LTC68XX
configLocation->noOfExpansionBoard = 0; // Number of expansion board configLocation->noOfExpansionBoard = 0; // Number of expansion board
configLocation->noOfTempSensorPerExpansionBoard = 0; // Number of temperature sensors monitored per expansion board configLocation->noOfTempSensorPerExpansionBoard = 0; // Number of temperature sensors monitored per expansion board
configLocation->LCUseDischarge = enabled; // Enable or disable the solid state output configLocation->LCUseDischarge = enabled; // Enable or disable the solid state output
@@ -233,6 +233,8 @@ void modConfigLoadDefaultConfig(modConfigGeneralConfigStructTypedef *configLocat
configLocation->heatingStartThreshold = 0; configLocation->heatingStartThreshold = 0;
configLocation->heatingStopThreshold = 20; configLocation->heatingStopThreshold = 20;
configLocation->floatCurrentK1 = 0.0f; // First factor of current calculation
configLocation->floatCurrentK2 = 0.777f; // Second factor of current calculation
#elif ENNOID_HV #elif ENNOID_HV
configLocation->noOfCellsSeries = 8; // Total number of cells in series in the battery pack configLocation->noOfCellsSeries = 8; // Total number of cells in series in the battery pack

View File

@@ -108,10 +108,10 @@ void modPowerElectronicsInit(modPowerElectronicsPackStateTypedef *packState, mod
modPowerElectronicsPackStateHandle->packInSOADischarge = true; modPowerElectronicsPackStateHandle->packInSOADischarge = true;
modPowerElectronicsPackStateHandle->watchDogTime = 255; modPowerElectronicsPackStateHandle->watchDogTime = 255;
modPowerElectronicsPackStateHandle->packOperationalCellState = PACK_STATE_NORMAL; modPowerElectronicsPackStateHandle->packOperationalCellState = PACK_STATE_NORMAL;
modPowerElectronicsPackStateHandle->temperatures[0] = -50.0f; // modPowerElectronicsPackStateHandle->temperatures[0] = 0.0f;
modPowerElectronicsPackStateHandle->temperatures[1] = -50.0f; // modPowerElectronicsPackStateHandle->temperatures[1] = 0.0f;
modPowerElectronicsPackStateHandle->temperatures[2] = -50.0f; // modPowerElectronicsPackStateHandle->temperatures[2] = 0.0f;
modPowerElectronicsPackStateHandle->temperatures[3] = -50.0f; // modPowerElectronicsPackStateHandle->temperatures[3] = 0.0f;
modPowerElectronicsPackStateHandle->tempBatteryHigh = 0.0f; modPowerElectronicsPackStateHandle->tempBatteryHigh = 0.0f;
modPowerElectronicsPackStateHandle->tempBatteryLow = 0.0f; modPowerElectronicsPackStateHandle->tempBatteryLow = 0.0f;
modPowerElectronicsPackStateHandle->tempBatteryAverage = 0.0f; modPowerElectronicsPackStateHandle->tempBatteryAverage = 0.0f;
@@ -122,6 +122,11 @@ void modPowerElectronicsInit(modPowerElectronicsPackStateTypedef *packState, mod
modPowerElectronicsPackStateHandle->buzzerOn = false; modPowerElectronicsPackStateHandle->buzzerOn = false;
modPowerElectronicsPackStateHandle->powerDownDesired = false; modPowerElectronicsPackStateHandle->powerDownDesired = false;
modPowerElectronicsPackStateHandle->powerOnLongButtonPress = false; modPowerElectronicsPackStateHandle->powerOnLongButtonPress = false;
for (uint8_t tempPointer = 0; tempPointer < NoOfTempSensors; ++tempPointer)
{
modPowerElectronicsPackStateHandle->temperatures[tempPointer] = 0;
}
// init the cell module variables empty // init the cell module variables empty
for( uint8_t modulePointer = 0; modulePointer < NoOfCellMonitorsPossibleOnBMS; modulePointer++) { for( uint8_t modulePointer = 0; modulePointer < NoOfCellMonitorsPossibleOnBMS; modulePointer++) {
@@ -592,30 +597,41 @@ void modPowerElectronicsCalcTempStats(void) {
// BMS temperatures // BMS temperatures
for(uint8_t sensorPointer = 1; sensorPointer < 16; sensorPointer++){ for(uint8_t sensorPointer = 0; sensorPointer < 16; sensorPointer++){
if(modPowerElectronicsGeneralConfigHandle->tempEnableMaskBMS & (1 << sensorPointer)){ if(modPowerElectronicsGeneralConfigHandle->tempEnableMaskBMS & (1 << sensorPointer)){
modPowerElectronicsPackStateHandle->temperatures[sensorPointer] = modPowerElectronicsPackStateHandle->auxVoltagesIndividual[sensorPointer-1].auxVoltage; modPowerElectronicsPackStateHandle->temperatures[sensorPointer] = modPowerElectronicsPackStateHandle->auxVoltagesIndividual[sensorPointer-1].auxVoltage;
} }
} }
for(uint8_t sensorPointer = 0; sensorPointer < 16; sensorPointer++){ // for(uint8_t sensorPointer = 0; sensorPointer < 16; sensorPointer++){
if(modPowerElectronicsGeneralConfigHandle->tempEnableMaskBMS & (1 << sensorPointer)){ // if(modPowerElectronicsGeneralConfigHandle->tempEnableMaskBMS & (1 << sensorPointer)){
if(modPowerElectronicsPackStateHandle->temperatures[sensorPointer] > tempBMSMax) // if(modPowerElectronicsPackStateHandle->temperatures[sensorPointer] > tempBMSMax)
tempBMSMax = modPowerElectronicsPackStateHandle->temperatures[sensorPointer]; // tempBMSMax = modPowerElectronicsPackStateHandle->temperatures[sensorPointer];
if(modPowerElectronicsPackStateHandle->temperatures[sensorPointer] < tempBMSMin) // if(modPowerElectronicsPackStateHandle->temperatures[sensorPointer] < tempBMSMin)
tempBMSMin = modPowerElectronicsPackStateHandle->temperatures[sensorPointer]; // tempBMSMin = modPowerElectronicsPackStateHandle->temperatures[sensorPointer];
tempBMSSum += modPowerElectronicsPackStateHandle->temperatures[sensorPointer]; // tempBMSSum += modPowerElectronicsPackStateHandle->temperatures[sensorPointer];
tempBMSSumCount++; // tempBMSSumCount++;
} // }
} // }
// Battery temperatures statistics for LTC aux channels without taking into account the first slave board temp measurement // Battery temperatures statistics for LTC aux channels without taking into account the first slave board temp measurement
for(uint8_t sensorModulePointer = 0; sensorModulePointer < modPowerElectronicsGeneralConfigHandle->cellMonitorICCount; sensorModulePointer++) { for(uint8_t sensorModulePointer = 0; sensorModulePointer < modPowerElectronicsGeneralConfigHandle->cellMonitorICCount; sensorModulePointer++) {
for(uint8_t sensorPointer = 0; sensorPointer < modPowerElectronicsGeneralConfigHandle->noOfTempSensorPerModule; sensorPointer++) { for(uint8_t sensorPointer = 0; sensorPointer < modPowerElectronicsGeneralConfigHandle->noOfTempSensorPerModule; sensorPointer++) {
if(modPowerElectronicsGeneralConfigHandle->tempEnableMaskBMS & (1 << sensorPointer)){
if(modPowerElectronicsPackStateHandle->temperatures[sensorPointer] > tempBMSMax)
tempBMSMax = modPowerElectronicsPackStateHandle->temperatures[sensorPointer];
if(modPowerElectronicsPackStateHandle->temperatures[sensorPointer] < tempBMSMin)
tempBMSMin = modPowerElectronicsPackStateHandle->temperatures[sensorPointer];
tempBMSSum += modPowerElectronicsPackStateHandle->temperatures[sensorPointer];
tempBMSSumCount++;
}
if(modPowerElectronicsGeneralConfigHandle->tempEnableMaskBattery & (1 << sensorPointer)){ if(modPowerElectronicsGeneralConfigHandle->tempEnableMaskBattery & (1 << sensorPointer)){
if(modPowerElectronicsPackStateHandle->auxVoltagesIndividual[sensorPointer].auxVoltage > tempBatteryMax) if(modPowerElectronicsPackStateHandle->auxVoltagesIndividual[sensorPointer].auxVoltage > tempBatteryMax)
tempBatteryMax = modPowerElectronicsPackStateHandle->auxVoltagesIndividual[sensorPointer].auxVoltage; tempBatteryMax = modPowerElectronicsPackStateHandle->auxVoltagesIndividual[sensorPointer].auxVoltage;
@@ -921,8 +937,9 @@ void modPowerElectronicsCellMonitorsCheckConfigAndReadAnalogData(void){
// Read aux voltages // Read aux voltages
driverSWLTC6804ReadAuxVoltagesArray(modPowerElectronicsPackStateHandle->auxModuleVoltages,modPowerElectronicsGeneralConfigHandle->NTC25DegResistance[modConfigNTCGroupLTCExt],modPowerElectronicsGeneralConfigHandle->NTCTopResistor[modConfigNTCGroupLTCExt],modPowerElectronicsGeneralConfigHandle->NTCBetaFactor[modConfigNTCGroupLTCExt],25.0f); driverSWLTC6804ReadAuxVoltagesArray(modPowerElectronicsPackStateHandle->auxModuleVoltages,modPowerElectronicsGeneralConfigHandle->NTC25DegResistance[modConfigNTCGroupLTCExt],modPowerElectronicsGeneralConfigHandle->NTCTopResistor[modConfigNTCGroupLTCExt],modPowerElectronicsGeneralConfigHandle->NTCBetaFactor[modConfigNTCGroupLTCExt],25.0f);
modPowerElectronicsAuxMonitorsArrayTranslate(); modPowerElectronicsAuxMonitorsArrayTranslate();
//driverSWLTC6804ReadAuxSensors(modPowerElectronicsAuxVoltageArray);
//modPowerElectronicsPackStateHandle->temperatures[0] = modPowerElectronicsPackStateHandle->temperatures[1] = driverSWLTC6804ConvertTemperatureExt(modPowerElectronicsAuxVoltageArray[1],modPowerElectronicsGeneralConfigHandle->NTC25DegResistance[modConfigNTCGroupLTCExt],modPowerElectronicsGeneralConfigHandle->NTCTopResistor[modConfigNTCGroupLTCExt],modPowerElectronicsGeneralConfigHandle->NTCBetaFactor[modConfigNTCGroupLTCExt],25.0f); // driverSWLTC6804ReadAuxSensors(modPowerElectronicsAuxVoltageArray);
// modPowerElectronicsPackStateHandle->temperatures[0] = modPowerElectronicsPackStateHandle->temperatures[1] = driverSWLTC6804ConvertTemperatureExt(modPowerElectronicsAuxVoltageArray[1],modPowerElectronicsGeneralConfigHandle->NTC25DegResistance[modConfigNTCGroupLTCExt],modPowerElectronicsGeneralConfigHandle->NTCTopResistor[modConfigNTCGroupLTCExt],modPowerElectronicsGeneralConfigHandle->NTCBetaFactor[modConfigNTCGroupLTCExt],25.0f);
//Read exp voltages //Read exp voltages
// driverSWADC128D818ReadExpVoltagesArray(modPowerElectronicsPackStateHandle->expModuleVoltages,modPowerElectronicsGeneralConfigHandle->NTC25DegResistance[modConfigNTCGroupExp],modPowerElectronicsGeneralConfigHandle->NTCTopResistor[modConfigNTCGroupExp],modPowerElectronicsGeneralConfigHandle->NTCBetaFactor[modConfigNTCGroupExp],25.0f); // driverSWADC128D818ReadExpVoltagesArray(modPowerElectronicsPackStateHandle->expModuleVoltages,modPowerElectronicsGeneralConfigHandle->NTC25DegResistance[modConfigNTCGroupExp],modPowerElectronicsGeneralConfigHandle->NTCTopResistor[modConfigNTCGroupExp],modPowerElectronicsGeneralConfigHandle->NTCBetaFactor[modConfigNTCGroupExp],25.0f);