From 21a40a9c3d27cd6ab6d919b6796a40733eb5afee Mon Sep 17 00:00:00 2001 From: csouzapaz <102175399+csouzapaz@users.noreply.github.com> Date: Mon, 18 Nov 2024 17:52:47 +0100 Subject: [PATCH] feat: first implementation of leaf wetness module --- .../libraries/RmapCommon/stima_config.h | 1 + .../master/include/canard_class_master.hpp | 2 + .../stima_v4/master/include/canard_config.hpp | 5 + .../stima_v4/master/include/sensors_config.h | 3 + .../stima_v4/master/include/tasks/lcd_task.h | 2 + .../stima_v4/master/include/tasks/mqtt_task.h | 5 + .../master/src/canard_class_master.cpp | 3 + platformio/stima_v4/master/src/rpc_class.cpp | 18 + .../stima_v4/master/src/tasks/can_task.cpp | 125 ++ .../stima_v4/master/src/tasks/lcd_task.cpp | 7 + .../stima_v4/master/src/tasks/mqtt_task.cpp | 158 ++ platformio/stima_v4/slave-leaf/include/README | 39 + .../include/STM32FreeRTOSConfig_extra.h | 55 + .../slave-leaf/include/canard_class_leaf.hpp | 422 ++++ .../slave-leaf/include/canard_config.hpp | 120 + .../stima_v4/slave-leaf/include/config.h | 255 +++ .../stima_v4/slave-leaf/include/debug.h | 162 ++ .../stima_v4/slave-leaf/include/debug_F.h | 84 + .../slave-leaf/include/debug_config.h | 51 + .../slave-leaf/include/drivers/AT25SF161.h | 132 ++ .../include/drivers/accelerometer.h | 678 ++++++ .../slave-leaf/include/drivers/eeprom.h | 66 + .../slave-leaf/include/drivers/flash.h | 142 ++ .../include/drivers/module_slave_hal.hpp | 214 ++ .../slave-leaf/include/local_typedef.h | 168 ++ .../slave-leaf/include/local_typedef_config.h | 29 + platformio/stima_v4/slave-leaf/include/main.h | 72 + .../slave-leaf/include/os_port_config.h | 34 + .../slave-leaf/include/register_class.hpp | 152 ++ .../slave-leaf/include/sensors_config.h | 406 ++++ .../include/tasks/accelerometer_task.h | 125 ++ .../slave-leaf/include/tasks/can_task.h | 192 ++ .../include/tasks/elaborate_data_task.h | 129 ++ .../include/tasks/leaf_sensor_task.h | 147 ++ .../include/tasks/supervisor_task.h | 109 + .../slave-leaf/include/tasks/wdt_task.h | 82 + platformio/stima_v4/slave-leaf/lib/README | 46 + platformio/stima_v4/slave-leaf/platformio.ini | 54 + .../slave-leaf/src/canard_class_leaf.cpp | 1112 ++++++++++ platformio/stima_v4/slave-leaf/src/debug.cpp | 113 + .../slave-leaf/src/drivers/accelerometer.cpp | 1962 +++++++++++++++++ .../slave-leaf/src/drivers/eeprom.cpp | 219 ++ .../stima_v4/slave-leaf/src/drivers/flash.cpp | 1013 +++++++++ .../src/drivers/freeRTOS_callback.cpp | 231 ++ .../src/drivers/freeRTOS_lptimTick.c | 852 +++++++ .../src/drivers/module_slave_hal.cpp | 658 ++++++ .../slave-leaf/src/drivers/syscalls.c | 89 + platformio/stima_v4/slave-leaf/src/main.cpp | 316 +++ .../slave-leaf/src/register_class.cpp | 486 ++++ .../src/tasks/accelerometer_task.cpp | 501 +++++ .../slave-leaf/src/tasks/can_task.cpp | 1804 +++++++++++++++ .../src/tasks/elaborate_data_task.cpp | 429 ++++ .../slave-leaf/src/tasks/leaf_sensor_task.cpp | 460 ++++ .../slave-leaf/src/tasks/supervisor_task.cpp | 603 +++++ .../slave-leaf/src/tasks/wdt_task.cpp | 212 ++ platformio/stima_v4/slave-leaf/test/README | 11 + .../slave-leaf/user_custom/bootloader.ld | 176 ++ 57 files changed, 15741 insertions(+) create mode 100644 platformio/stima_v4/slave-leaf/include/README create mode 100644 platformio/stima_v4/slave-leaf/include/STM32FreeRTOSConfig_extra.h create mode 100644 platformio/stima_v4/slave-leaf/include/canard_class_leaf.hpp create mode 100644 platformio/stima_v4/slave-leaf/include/canard_config.hpp create mode 100644 platformio/stima_v4/slave-leaf/include/config.h create mode 100644 platformio/stima_v4/slave-leaf/include/debug.h create mode 100644 platformio/stima_v4/slave-leaf/include/debug_F.h create mode 100644 platformio/stima_v4/slave-leaf/include/debug_config.h create mode 100644 platformio/stima_v4/slave-leaf/include/drivers/AT25SF161.h create mode 100644 platformio/stima_v4/slave-leaf/include/drivers/accelerometer.h create mode 100644 platformio/stima_v4/slave-leaf/include/drivers/eeprom.h create mode 100644 platformio/stima_v4/slave-leaf/include/drivers/flash.h create mode 100644 platformio/stima_v4/slave-leaf/include/drivers/module_slave_hal.hpp create mode 100644 platformio/stima_v4/slave-leaf/include/local_typedef.h create mode 100644 platformio/stima_v4/slave-leaf/include/local_typedef_config.h create mode 100644 platformio/stima_v4/slave-leaf/include/main.h create mode 100644 platformio/stima_v4/slave-leaf/include/os_port_config.h create mode 100644 platformio/stima_v4/slave-leaf/include/register_class.hpp create mode 100644 platformio/stima_v4/slave-leaf/include/sensors_config.h create mode 100644 platformio/stima_v4/slave-leaf/include/tasks/accelerometer_task.h create mode 100644 platformio/stima_v4/slave-leaf/include/tasks/can_task.h create mode 100644 platformio/stima_v4/slave-leaf/include/tasks/elaborate_data_task.h create mode 100644 platformio/stima_v4/slave-leaf/include/tasks/leaf_sensor_task.h create mode 100644 platformio/stima_v4/slave-leaf/include/tasks/supervisor_task.h create mode 100644 platformio/stima_v4/slave-leaf/include/tasks/wdt_task.h create mode 100644 platformio/stima_v4/slave-leaf/lib/README create mode 100644 platformio/stima_v4/slave-leaf/platformio.ini create mode 100644 platformio/stima_v4/slave-leaf/src/canard_class_leaf.cpp create mode 100644 platformio/stima_v4/slave-leaf/src/debug.cpp create mode 100644 platformio/stima_v4/slave-leaf/src/drivers/accelerometer.cpp create mode 100644 platformio/stima_v4/slave-leaf/src/drivers/eeprom.cpp create mode 100644 platformio/stima_v4/slave-leaf/src/drivers/flash.cpp create mode 100644 platformio/stima_v4/slave-leaf/src/drivers/freeRTOS_callback.cpp create mode 100644 platformio/stima_v4/slave-leaf/src/drivers/freeRTOS_lptimTick.c create mode 100644 platformio/stima_v4/slave-leaf/src/drivers/module_slave_hal.cpp create mode 100644 platformio/stima_v4/slave-leaf/src/drivers/syscalls.c create mode 100644 platformio/stima_v4/slave-leaf/src/main.cpp create mode 100644 platformio/stima_v4/slave-leaf/src/register_class.cpp create mode 100644 platformio/stima_v4/slave-leaf/src/tasks/accelerometer_task.cpp create mode 100644 platformio/stima_v4/slave-leaf/src/tasks/can_task.cpp create mode 100644 platformio/stima_v4/slave-leaf/src/tasks/elaborate_data_task.cpp create mode 100644 platformio/stima_v4/slave-leaf/src/tasks/leaf_sensor_task.cpp create mode 100644 platformio/stima_v4/slave-leaf/src/tasks/supervisor_task.cpp create mode 100644 platformio/stima_v4/slave-leaf/src/tasks/wdt_task.cpp create mode 100644 platformio/stima_v4/slave-leaf/test/README create mode 100644 platformio/stima_v4/slave-leaf/user_custom/bootloader.ld diff --git a/platformio/stima_v4/libraries/RmapCommon/stima_config.h b/platformio/stima_v4/libraries/RmapCommon/stima_config.h index fbe1467ee..d4d630d3e 100644 --- a/platformio/stima_v4/libraries/RmapCommon/stima_config.h +++ b/platformio/stima_v4/libraries/RmapCommon/stima_config.h @@ -252,6 +252,7 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. #define STIMA_RPC_SENSOR_NAME_DSA ("DSA") #define STIMA_RPC_SENSOR_NAME_LVM ("LEV") #define STIMA_RPC_SENSOR_NAME_VWC ("SVW") +#define STIMA_RPC_SENSOR_NAME_BFT ("BFT") /*! \brief LCD descriptions list diff --git a/platformio/stima_v4/master/include/canard_class_master.hpp b/platformio/stima_v4/master/include/canard_class_master.hpp index c11e95145..d13afd3cc 100644 --- a/platformio/stima_v4/master/include/canard_class_master.hpp +++ b/platformio/stima_v4/master/include/canard_class_master.hpp @@ -55,6 +55,8 @@ #include #include #include +#include +#include #include #include #include diff --git a/platformio/stima_v4/master/include/canard_config.hpp b/platformio/stima_v4/master/include/canard_config.hpp index cc5d59a6a..249e0f94d 100644 --- a/platformio/stima_v4/master/include/canard_config.hpp +++ b/platformio/stima_v4/master/include/canard_config.hpp @@ -77,6 +77,7 @@ #define PORT_RMAP_MPPT 54 #define PORT_RMAP_VWC 55 #define PORT_RMAP_LEVEL 56 +#define PORT_RMAP_LEAF 57 #define PORT_RMAP_MASTER 100 /// @brief Parametri default per Modulo Slave (INIT_PARAMETER) @@ -98,6 +99,7 @@ #define USE_MODULE_FIXED_RAIN #define USE_MODULE_FIXED_WIND #define USE_MODULE_FIXED_RADIATION +#define USE_MODULE_FIXED_LEAF #define USE_MODULE_FIXED_VWC #define USE_MODULE_FIXED_LEVEL #define USE_MODULE_FIXED_POWER @@ -172,6 +174,9 @@ // Radiation #define SENSOR_METADATA_DSA 0 #define SENSOR_METADATA_RADIATION_COUNT 1 +// Leaf +#define SENSOR_METADATA_BFT 0 +#define SENSOR_METADATA_LEAF_COUNT 1 // VWC #define SENSOR_METADATA_VWC1 0 #define SENSOR_METADATA_VWC2 1 diff --git a/platformio/stima_v4/master/include/sensors_config.h b/platformio/stima_v4/master/include/sensors_config.h index 16f188b33..63de48c0d 100644 --- a/platformio/stima_v4/master/include/sensors_config.h +++ b/platformio/stima_v4/master/include/sensors_config.h @@ -215,6 +215,7 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. #define USE_WIND_SENSORS (USE_SENSOR_DWA + USE_SENSOR_DWB + USE_SENSOR_DWC + USE_SENSOR_DWD + USE_SENSOR_DWE + USE_SENSOR_DWF) #define USE_POWER_MPPT_SENSORS (USE_SENSOR_DEP) #define USE_LEVEL_SENSOR (USE_SENSOR_LVM) +#define USE_LEAF_SENSOR (USE_SENSOR_LWT) #if (USE_TH_SENSORS && (USE_RAIN_SENSORS == 0)) #define USE_MODULE_TH (true) @@ -230,6 +231,8 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. #define USE_MODULE_POWER_MPPT (true) #elif (USE_LEVEL_SENSOR) #define USE_MODULE_LEVEL (true) +#elif (USE_LEAF_SENSOR) +#define USE_MODULE_LEAF (true) #endif /// @brief Max count of all used sensor. diff --git a/platformio/stima_v4/master/include/tasks/lcd_task.h b/platformio/stima_v4/master/include/tasks/lcd_task.h index 6a968d233..65645505f 100644 --- a/platformio/stima_v4/master/include/tasks/lcd_task.h +++ b/platformio/stima_v4/master/include/tasks/lcd_task.h @@ -72,6 +72,8 @@ #define MIN_VALID_WIND_DIR (0.0) #define MAX_VALID_RADIATION (2000.0) #define MIN_VALID_RADIATION (0.0) +#define MAX_VALID_LEAF (100.0) +#define MIN_VALID_LEAF (0.0) #define MAX_VALID_LEVEL (60000.0) #define MIN_VALID_LEVEL (0.0) #define MAX_VALID_RAIN (1000.0) diff --git a/platformio/stima_v4/master/include/tasks/mqtt_task.h b/platformio/stima_v4/master/include/tasks/mqtt_task.h index 2118225a3..29d8d9773 100644 --- a/platformio/stima_v4/master/include/tasks/mqtt_task.h +++ b/platformio/stima_v4/master/include/tasks/mqtt_task.h @@ -93,6 +93,8 @@ #include #include #include +#include +#include #include #include #include @@ -286,6 +288,9 @@ class MqttTask : public cpp_freertos::Thread { error_t publishSensorRadiation(MqttClientContext *context, MqttQosLevel qos, rmap_sensors_Radiation_1_0 sensor, DateTime dateTime, configuration_t *configuration, char *topic, size_t topic_length, char *sensors_topic, size_t sensors_topic_length, char *message, size_t message_length); error_t makeSensorMessageRadiation(rmap_measures_Radiation_1_0 radiation, DateTime dateTime, char *message, size_t message_length); + error_t publishSensorLeaf(MqttClientContext *context, MqttQosLevel qos, rmap_sensors_Leaf_1_0 sensor, DateTime dateTime, configuration_t *configuration, char *topic, size_t topic_length, char *sensors_topic, size_t sensors_topic_length, char *message, size_t message_length); + error_t makeSensorMessageLeaf(rmap_measures_Leaf_1_0 leaf, DateTime dateTime, char *message, size_t message_length); + error_t publishSensorLevel(MqttClientContext *context, MqttQosLevel qos, rmap_sensors_RiverLevel_1_0 sensor, DateTime dateTime, configuration_t *configuration, char *topic, size_t topic_length, char *sensors_topic, size_t sensors_topic_length, char *message, size_t message_length); error_t makeSensorMessageLevel(rmap_measures_RiverLevel_1_0 level, DateTime dateTime, char *message, size_t message_length); diff --git a/platformio/stima_v4/master/src/canard_class_master.cpp b/platformio/stima_v4/master/src/canard_class_master.cpp index b49b2fb22..cab3ad19c 100644 --- a/platformio/stima_v4/master/src/canard_class_master.cpp +++ b/platformio/stima_v4/master/src/canard_class_master.cpp @@ -1466,6 +1466,9 @@ void canardClass::slave::rmap_service::set_module_type(Module_Type module_type) case Module_Type::radiation: _response = malloc(sizeof(rmap_service_module_Radiation_Response_1_0)); break; + case Module_Type::leaf: + _response = malloc(sizeof(rmap_service_module_Leaf_Response_1_0)); + break; case Module_Type::level: _response = malloc(sizeof(rmap_service_module_RiverLevel_Response_1_0)); break; diff --git a/platformio/stima_v4/master/src/rpc_class.cpp b/platformio/stima_v4/master/src/rpc_class.cpp index a949083a0..01aef5282 100644 --- a/platformio/stima_v4/master/src/rpc_class.cpp +++ b/platformio/stima_v4/master/src/rpc_class.cpp @@ -438,6 +438,7 @@ int RegisterRPC::configure(JsonObject params, JsonObject result) case STIMA_MODULE_TYPE_TH: case STIMA_MODULE_TYPE_WIND: case STIMA_MODULE_TYPE_SOLAR_RADIATION: + case STIMA_MODULE_TYPE_LEAF: case STIMA_MODULE_TYPE_LEVEL: case STIMA_MODULE_TYPE_POWER_MPPT: // Set module index (defualt START from 0xFF to point 0 index at start) @@ -493,6 +494,9 @@ int RegisterRPC::configure(JsonObject params, JsonObject result) case STIMA_MODULE_TYPE_SOLAR_RADIATION: param.configuration->board_slave[slaveId].can_port_id = PORT_RMAP_RADIATION; break; + case STIMA_MODULE_TYPE_LEAF: + param.configuration->board_slave[slaveId].can_port_id = PORT_RMAP_LEAF; + break; case STIMA_MODULE_TYPE_LEVEL: param.configuration->board_slave[slaveId].can_port_id = PORT_RMAP_LEVEL; break; @@ -564,6 +568,14 @@ int RegisterRPC::configure(JsonObject params, JsonObject result) } else error_command = true; break; + case Module_Type::leaf: + if(strcmp(subject, "node.leaf") == 0) { + param.configurationLock->Take(); + param.configuration->board_slave[slaveId].can_publish_id = it.value().as(); + param.configurationLock->Give(); + } + else error_command = true; + break; case Module_Type::level: if(strcmp(subject, "node.lev") == 0) { param.configurationLock->Take(); @@ -683,6 +695,12 @@ int RegisterRPC::configure(JsonObject params, JsonObject result) } else error_command = true; break; + case Module_Type::leaf: + if (strcmp(it.value().as(), STIMA_RPC_SENSOR_NAME_BFT) == 0) { + sensorId = SENSOR_METADATA_BFT; + } + else error_command = true; + break; case Module_Type::level: if (strcmp(it.value().as(), STIMA_RPC_SENSOR_NAME_LVM) == 0) { sensorId = SENSOR_METADATA_LVM; diff --git a/platformio/stima_v4/master/src/tasks/can_task.cpp b/platformio/stima_v4/master/src/tasks/can_task.cpp index 3c7f3cd42..86938b3cc 100644 --- a/platformio/stima_v4/master/src/tasks/can_task.cpp +++ b/platformio/stima_v4/master/src/tasks/can_task.cpp @@ -537,6 +537,10 @@ void CanTask::processReceivedTransfer(canardClass &clCanard, const CanardRxTrans TRACE_VERBOSE_F(F("Anonimous module RADIATION")); defaultNodeId = clCanard.getPNPValidIdFromNodeType(Module_Type::radiation, msg.unique_id_hash); break; + case Module_Type::leaf: + TRACE_VERBOSE_F(F("Anonimous module LEAF")); + defaultNodeId = clCanard.getPNPValidIdFromNodeType(Module_Type::leaf, msg.unique_id_hash); + break; case Module_Type::level: TRACE_VERBOSE_F(F("Anonimous module LEVEL")); defaultNodeId = clCanard.getPNPValidIdFromNodeType(Module_Type::level, msg.unique_id_hash); @@ -955,6 +959,14 @@ void CanTask::processReceivedTransfer(canardClass &clCanard, const CanardRxTrans clCanard.slave[queueId].rmap_service.reset_pending(respCastRadiation, sizeof(*respCastRadiation)); } } + if(clCanard.slave[queueId].get_module_type() == Module_Type::leaf) { + rmap_service_module_Leaf_Response_1_0 *respCastLeaf = (rmap_service_module_Leaf_Response_1_0 *)castLocalBuffer; + size_t size = transfer->payload_size; + if (rmap_service_module_Leaf_Response_1_0_deserialize_(respCastLeaf, static_cast(transfer->payload), &size) >= 0) + { + clCanard.slave[queueId].rmap_service.reset_pending(respCastLeaf, sizeof(*respCastLeaf)); + } + } if(clCanard.slave[queueId].get_module_type() == Module_Type::level) { rmap_service_module_RiverLevel_Response_1_0 *respCastLevel = (rmap_service_module_RiverLevel_Response_1_0 *)castLocalBuffer; size_t size = transfer->payload_size; @@ -1236,6 +1248,7 @@ void CanTask::Run() { rmap_service_module_Rain_Response_1_0* retRainData; rmap_service_module_Wind_Response_1_0* retWindData; rmap_service_module_Radiation_Response_1_0* retRadiationData; + rmap_service_module_Leaf_Response_1_0* retLeafData; rmap_service_module_RiverLevel_Response_1_0* retLevelData; rmap_service_module_VWC_Response_1_0* retVwcData; rmap_service_module_Power_Response_1_0* retPwrData; @@ -1389,6 +1402,16 @@ void CanTask::Run() { for(uint8_t isCfg=0; isCfgboard_slave[idxFixed].is_configured[isCfg] = true; #endif + #ifdef USE_MODULE_FIXED_LEAF + idxFixed++; + param.configuration->board_slave[idxFixed].can_address = 63; + param.configuration->board_slave[idxFixed].module_type = Module_Type::leaf; + param.configuration->board_slave[idxFixed].can_port_id = PORT_RMAP_LEAF; + param.configuration->board_slave[idxFixed].can_publish_id = PORT_RMAP_LEAF; + param.configuration->board_slave[idxFixed].serial_number = 0; + for(uint8_t isCfg=0; isCfgboard_slave[idxFixed].is_configured[isCfg] = true; + #endif #ifdef USE_MODULE_FIXED_POWER idxFixed++; param.configuration->board_slave[idxFixed].can_address = 64; @@ -1612,6 +1635,17 @@ void CanTask::Run() { } } // Controllo le varie tipologie di request/service per il nodo + if(clCanard.slave[queueId].get_module_type() == Module_Type::leaf) { + // Alloco la stottoscrizione in funzione del tipo di modulo + // Service client: -> Risposta per ServiceDataModuleTH richiesta interna (come master) + if (!clCanard.rxSubscribe(CanardTransferKindResponse, + clCanard.slave[queueId].rmap_service.get_port_id(), + rmap_service_module_Leaf_Response_1_0_EXTENT_BYTES_, + CANARD_RMAPDATA_TRANSFER_ID_TIMEOUT_USEC)) { + LOCAL_ASSERT(false); + } + } + // Controllo le varie tipologie di request/service per il nodo if(clCanard.slave[queueId].get_module_type() == Module_Type::level) { // Alloco la stottoscrizione in funzione del tipo di modulo // Service client: -> Risposta per ServiceDataModuleTH richiesta interna (come master) @@ -1695,6 +1729,17 @@ void CanTask::Run() { } } // Controllo le varie tipologie di request/service per il nodo + if(clCanard.slave[queueId].get_module_type() == Module_Type::leaf) { + // Alloco la stottoscrizione in funzione del tipo di modulo + // Service client: -> Sottoscrizione per ModuleTH (come master) + if (!clCanard.rxSubscribe(CanardTransferKindMessage, + clCanard.slave[queueId].publisher.get_subject_id(), + rmap_module_Leaf_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC)) { + LOCAL_ASSERT(false); + } + } + // Controllo le varie tipologie di request/service per il nodo if(clCanard.slave[queueId].get_module_type() == Module_Type::level) { // Alloco la stottoscrizione in funzione del tipo di modulo // Service client: -> Sottoscrizione per ModuleTH (come master) @@ -2338,6 +2383,83 @@ void CanTask::Run() { } break; + case Module_Type::leaf: + // Cast to th module + retLeafData = (rmap_service_module_Leaf_Response_1_0*) clCanard.slave[queueId].rmap_service.get_response(); + // data RMAP type is ready to send into queue Archive Data for Saving on SD Memory + // Get parameter data + #if TRACE_LEVEL >= TRACE_INFO + getStimaNameByType(stimaName, clCanard.slave[queueId].get_module_type()); + #endif + // Put data in system_status with module_type and RMAP Ver.Rev if not equal (reload or updated) + if((param.system_status->data_slave[queueId].module_version!=retLeafData->version) || + (param.system_status->data_slave[queueId].module_revision!=retLeafData->revision)) { + param.systemStatusLock->Take(); + param.system_status->data_slave[queueId].module_version = retLeafData->version; + param.system_status->data_slave[queueId].module_revision = retLeafData->revision; + // Module type also setted on load config module CAN + param.system_status->data_slave[queueId].module_type = clCanard.slave[queueId].get_module_type(); + // Reset flag before recheck exixting firmware available + param.system_status->data_slave[queueId].fw_upgradable = false; + // Check if module can be updated + for(uint8_t checkId=0; checkIdboards_update_avaiable[checkId].module_type) { + if((param.system_status->boards_update_avaiable[checkId].version > retLeafData->version) || + ((param.system_status->boards_update_avaiable[checkId].version == retLeafData->version) && + (param.system_status->boards_update_avaiable[checkId].revision > retLeafData->revision))) { + // Found an upgradable boards + param.system_status->data_slave[queueId].fw_upgradable = true; + } + break; + } + } + param.systemStatusLock->Give(); + } + // TRACE Info data + TRACE_INFO_F(F("RMAP recived response data module from [ %s ], node id: %d. Response code: %d\r\n"), + stimaName, clCanard.slave[queueId].get_node_id(), retLeafData->state); + TRACE_VERBOSE_F(F("Value (DSA) Leaf %d\r\n"), retLeafData->BFT.leaf.val.value); + // Get security remote state on maintenance mode from relative state flags + param.system_status->data_slave[queueId].maintenance_mode = (retLeafData->state & CAN_FLAG_IS_MAINTENANCE_MODE); + retLeafData->state &= CAN_FLAG_MASK_MAINTENANCE_MODE; + if(param.system_status->data_slave[queueId].maintenance_mode) { + TRACE_INFO_F(F("Warning this module is in maintenance mode!!!\r\n")); + } + // Put istant data in system_status + if(retLeafData->state == rmap_service_setmode_1_0_get_istant) { + // Only istant request LCD or other device + param.systemStatusLock->Take(); + // Set data istant value (switch depends from request, istant = sample, Data = Avg.) + param.system_status->data_slave[queueId].data_value[0] = retLeafData->BFT.leaf.val.value; + param.system_status->data_slave[queueId].is_new_ist_data_ready = true; + param.systemStatusLock->Give(); + } else if(retLeafData->state == rmap_service_setmode_1_0_get_last) { + // data value id rmap_service_setmode_1_0_get_last into queue SD + // Copy Flag State + bit8Flag = 0; + if(retLeafData->is_adc_unit_error) bit8Flag|=0x01; + if(retLeafData->is_adc_unit_overflow) bit8Flag|=0x02; + param.systemStatusLock->Take(); + param.system_status->flags.new_data_to_send = true; + param.system_status->data_slave[queueId].bit8StateFlag = bit8Flag; + param.system_status->data_slave[queueId].byteStateFlag[0] = retLeafData->rbt_event; + param.system_status->data_slave[queueId].byteStateFlag[1] = retLeafData->wdt_event; + param.system_status->data_slave[queueId].byteStateFlag[2] = 0; + param.systemStatusLock->Give(); + // Copy Data + memset(&rmap_archive_data, 0, sizeof(rmap_archive_data_t)); + // Set Module Type, Date Time as Uint32 GetEpoch_Style, and Block Data Cast to RMAP Type + rmap_archive_data.module_type = clCanard.slave[queueId].get_module_type(); + rmap_archive_data.date_time = param.system_status->datetime.epoch_sensors_get_value; + memcpy(rmap_archive_data.block, retLeafData, sizeof(*retLeafData)); + // Send queue to SD for direct archive data + // Queue is dimensioned to accept all Data for one step pushing array data (MAX_BOARDS) + // Clean queue if is full to send alwayl the last data on getted value + if(param.dataRmapPutQueue->IsFull()) param.dataLogPutQueue->Dequeue(&rmap_archive_empty); + param.dataRmapPutQueue->Enqueue(&rmap_archive_data, Ticks::MsToTicks(CAN_PUT_QUEUE_RMAP_TIMEOUT_MS)); + } + break; + case Module_Type::level: // Cast to th module retLevelData = (rmap_service_module_RiverLevel_Response_1_0*) clCanard.slave[queueId].rmap_service.get_response(); @@ -3127,6 +3249,9 @@ void CanTask::Run() { case Module_Type::radiation: sensorCount = SENSOR_METADATA_RADIATION_COUNT; break; + case Module_Type::leaf: + sensorCount = SENSOR_METADATA_LEAF_COUNT; + break; case Module_Type::level: sensorCount = SENSOR_METADATA_LEVEL_COUNT; break; diff --git a/platformio/stima_v4/master/src/tasks/lcd_task.cpp b/platformio/stima_v4/master/src/tasks/lcd_task.cpp index 88c2fccb1..722ecf2ff 100644 --- a/platformio/stima_v4/master/src/tasks/lcd_task.cpp +++ b/platformio/stima_v4/master/src/tasks/lcd_task.cpp @@ -509,6 +509,10 @@ void LCDTask::display_print_channel_interface(uint8_t module_type) { value_display_A = param.system_status->data_slave[channel].data_value[0]; if ((value_display_A < MIN_VALID_RADIATION) || (value_display_A > MAX_VALID_RADIATION)) bMeasValid_A = false; break; + case Module_Type::leaf: + value_display_A = param.system_status->data_slave[channel].data_value[0]; + if ((value_display_A < MIN_VALID_LEAF) || (value_display_A > MAX_VALID_LEAF)) bMeasValid_A = false; + break; case Module_Type::level: value_display_A = param.system_status->data_slave[channel].data_value[0]; if ((value_display_A < MIN_VALID_LEVEL) || (value_display_A > MAX_VALID_LEVEL)) bMeasValid_A = false; @@ -829,6 +833,9 @@ void LCDTask::display_print_main_interface(void) { case Module_Type::level: strcat(errors, "level "); break; + case Module_Type::leaf: + strcat(errors, "leaf "); + break; case Module_Type::vwc: strcat(errors, "vwc "); break; diff --git a/platformio/stima_v4/master/src/tasks/mqtt_task.cpp b/platformio/stima_v4/master/src/tasks/mqtt_task.cpp index b6091bcc9..5ebe7d06b 100644 --- a/platformio/stima_v4/master/src/tasks/mqtt_task.cpp +++ b/platformio/stima_v4/master/src/tasks/mqtt_task.cpp @@ -138,6 +138,7 @@ void MqttTask::Run() rmap_service_module_TH_Response_1_0 *rmapDataTH; rmap_service_module_Rain_Response_1_0 *rmapDataRain; rmap_service_module_Radiation_Response_1_0 *rmapDataRadiation; + rmap_service_module_Leaf_Response_1_0 *rmapDataLeaf; rmap_service_module_RiverLevel_Response_1_0 *rmapDataLevel; rmap_service_module_Wind_Response_1_0 *rmapDataWind; rmap_service_module_VWC_Response_1_0 *rmapDataVWC; @@ -715,6 +716,42 @@ void MqttTask::Run() } } + break; + case Module_Type::leaf: + rmapDataLeaf = (rmap_service_module_Leaf_Response_1_0 *)rmap_get_response.rmap_data.block; + #if (ENABLE_STACK_USAGE) + TaskMonitorStack(); + #endif + + // check if the sensor was configured or not + for (uint8_t slaveId = 0; slaveId < BOARDS_COUNT_MAX; slaveId++) + { + if (param.configuration->board_slave[slaveId].module_type == Module_Type::leaf) + { + if (!error && param.configuration->board_slave[slaveId].is_configured[SENSOR_METADATA_DSA]) + { + error = publishSensorLeaf(&mqttClientContext, qos, rmapDataLeaf->BFT, rmap_date_time_val, param.configuration, topic, sizeof(topic), sensors_topic, sizeof(sensors_topic), message, sizeof(message)); + } + + if (error) + { + // Connection to MQTT server lost data? + state = MQTT_STATE_DISCONNECT_LOST_DATA; + TRACE_ERROR_F(F("MQTT_STATE_PUBLISH (Error) -> MQTT_STATE_DISCONNECT_LOST_DATA\r\n")); + break; + } + else + { + // Executed publishing of line data + param.systemStatusLock->Take(); + param.system_status->connection.is_mqtt_publishing = true; + param.system_status->connection.mqtt_data_published++; + param.systemStatusLock->Give(); + } + break; + } + } + break; case Module_Type::level: rmapDataLevel = (rmap_service_module_RiverLevel_Response_1_0 *)rmap_get_response.rmap_data.block; @@ -1916,6 +1953,127 @@ error_t MqttTask::publishSensorRadiation(MqttClientContext *context, MqttQosLeve return error; } +/// @brief create mqtt message for leaf value +/// @param leaf uavcan rmap measurement +/// @param dateTime datetime value +/// @param message text of the message +/// @param message_length length of the message +/// @return error_t +error_t MqttTask::makeSensorMessageLeaf(rmap_measures_Leaf_1_0 leaf, DateTime dateTime, char *message, size_t message_length) +{ + error_t error = NO_ERROR; + osMemset(message, 0, message_length); + + if (leaf.val.value <= rmap_tableb_B13212_1_0_MAX) + { + if (snprintf(message, message_length, "{\"v\":%ld,", leaf.val.value) <= 0) + { + error = ERROR_FAILURE; + } + } + else + { + if (snprintf(message, message_length, "{\"v\":null,") <= 0) + { + error = ERROR_FAILURE; + } + } + + if (!error) + { + error = makeDate(dateTime, &(message[strlen(message)]), message_length); + } + + if (!error) + { + if (leaf.confidence.value <= rmap_tableb_B33199_1_0_MAX) + { + if (snprintf(&(message[strlen(message)]), message_length, "\"a\":{\"B33199\":%u}", leaf.confidence.value) <= 0) + { + error = ERROR_FAILURE; + } + } + else + { + if (snprintf(&(message[strlen(message)]), message_length, "\"a\":{\"B33199\":null}") <= 0) + { + error = ERROR_FAILURE; + } + } + } + + if (!error) + { + if (snprintf(&(message[strlen(message)]), message_length, "}") <= 0) + { + error = ERROR_FAILURE; + } + } + + return error; +} + +/// @brief Publish radiation sensor +/// @param context mqtt client context +/// @param qos mqtt Qos level +/// @param sensor uavcan sensor +/// @param dateTime datetime +/// @param configuration struct of configuration +/// @param topic string of the topic +/// @param topic_length length of the topic +/// @param sensors_topic string of sensor topic +/// @param sensors_topic_length length of sensor topic +/// @param message string of message +/// @param message_length length of message +/// @return error_t +error_t MqttTask::publishSensorLeaf(MqttClientContext *context, MqttQosLevel qos, rmap_sensors_Leaf_1_0 sensor, DateTime dateTime, configuration_t *configuration, char *topic, size_t topic_length, char *sensors_topic, size_t sensors_topic_length, char *message, size_t message_length) +{ + uint8_t error_count; + error_t error = NO_ERROR; + + // ---------------------------------------------------------------------------- + // Leaf + // ---------------------------------------------------------------------------- + // make leaf topic + error = makeSensorTopic(sensor.metadata, "B13212", sensors_topic, sensors_topic_length); + // make leaf message + if (!error) + { + error = makeSensorMessageLeaf(sensor.leaf, dateTime, message, message_length); + } + // make common topic + if (!error) + { + error = makeCommonTopic(configuration, topic, sensors_topic, topic_length); + } + + // Prevents attempts to transmit other data after an error + if (error) return error; + error_count = 0; + + // Saving Data Backup Older Data Format (if SD Card Ready...) + if ((!error)&&(param.system_status->flags.sd_card_ready)) { + putRmapBackupArchiveData(dateTime, sensors_topic, message); + } + + // publish leaf value + do + { + TaskWatchDog(MQTT_NET_WAIT_TIMEOUT_PUBLISH); + error = mqttClientPublish(context, topic, message, strlen(message), qos, false, NULL); + TaskWatchDog(MQTT_TASK_WAIT_DELAY_MS); + if (error) + { + error_count++; + } + TaskWatchDog(MQTT_TASK_PUBLISH_DELAY_MS); + Delay(Ticks::MsToTicks(MQTT_TASK_PUBLISH_DELAY_MS)); + } while (error && (error_count < MQTT_TASK_PUBLISH_RETRY)); + TRACE_DEBUG_F(F("%s%s %s [ %s ]\r\n"), MQTT_PUB_CMD_DEBUG_PREFIX, topic, message, error ? ERROR_STRING : OK_STRING); + + return error; +} + /// @brief create mqtt message for level value /// @param level uavcan rmap measurement /// @param dateTime datetime value diff --git a/platformio/stima_v4/slave-leaf/include/README b/platformio/stima_v4/slave-leaf/include/README new file mode 100644 index 000000000..194dcd432 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/platformio/stima_v4/slave-leaf/include/STM32FreeRTOSConfig_extra.h b/platformio/stima_v4/slave-leaf/include/STM32FreeRTOSConfig_extra.h new file mode 100644 index 000000000..e54a375af --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/STM32FreeRTOSConfig_extra.h @@ -0,0 +1,55 @@ +#define configUSE_CMSIS_RTOS_V2 0 +#define configCHECK_FOR_STACK_OVERFLOW 1 + +// Enable for use LowPower +#define _USE_FREERTOS_LOW_POWER 1 + +// Enable For Exit Immediate from LowPower (Using for Debug) + + +#if USE_DEBUG +#define _EXIT_SLEEP_FOR_DEBUGGING +#endif + +#define SLEEP_NONE 0 // Unused LowPOWER +#define SLEEP_IDLE 1 // Power mode main regulator ON, mode 1 debug standard enabled +#define SLEEP_LOWPOWER 2 // Power mode low power regulator ON, debug Low Power +#define SLEEP_STOP2 3 // Stop2 mode all power regulator OFF + +// Define For LowPower Method +#ifdef _USE_FREERTOS_LOW_POWER + +#ifdef _EXIT_SLEEP_FOR_DEBUGGING + #define LOWPOWER_MODE SLEEP_NONE +#else + #define LOWPOWER_MODE SLEEP_STOP2 +#endif + +#define LOW_POWER_NONE 0 +#define LOW_POWER_DEFAULT 1 +#define LOW_POWER_PRIVATE_LPTIMx_TICK 2 + +// Freertos Tickless Mode (LOW_POWER_PRIVATE Enable LptimTick.c) +#define configUSE_TICKLESS_IDLE LOW_POWER_PRIVATE_LPTIMx_TICK + +// Define for lptimTick.c +#if (configUSE_TICKLESS_IDLE==LOW_POWER_PRIVATE_LPTIMx_TICK) + #define configTICK_USES_LSE // USE LSE CLOCK + #define configLPTIM_REF_CLOCK_HZ 32768UL // LSE CLOCK REFERENCE + #define configLPTIM_ENABLE_PRECISION 1 // ENABLE AUTO PRECISION + #define configLPTIM_SRC_LPTIM1 // LPTIM1 OR LPTIM2 CONFIG + #define configTICK_ENABLE_UWTICK_PRECISION 1 // ENABLE UPDATE SYSTICK +#endif + +// Time minimal for start LOW_POWER && SuppressTick... +#ifdef _EXIT_SLEEP_FOR_DEBUGGING + #define configEXPECTED_IDLE_TIME_BEFORE_SLEEP 60000 +#else + #define configEXPECTED_IDLE_TIME_BEFORE_SLEEP 50 +#endif + +// Macro Pre && Post Sleep/Wake method +#define configPRE_SLEEP_PROCESSING( x ) xTaskSleepPrivate ( &x ) +#define configPOST_SLEEP_PROCESSING( x ) xTaskWakeUpPrivate ( x ) + +#endif \ No newline at end of file diff --git a/platformio/stima_v4/slave-leaf/include/canard_class_leaf.hpp b/platformio/stima_v4/slave-leaf/include/canard_class_leaf.hpp new file mode 100644 index 000000000..cc83d7cf9 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/canard_class_leaf.hpp @@ -0,0 +1,422 @@ +/** + ****************************************************************************** + * @file canard_class_leaf.hpp + * @author Moreno Gasperini + * @brief Uavcan Canard Class LibCanard, bxCan, o1Heap + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +// Configurazione modulo, definizioni ed utility generiche +#include "canard_config.hpp" +#include "local_typedef.h" +// Arduino +#include +// Libcanard +#include +#include +// Namespace UAVCAN +#include +#include +#include +#include +#include +#include +#include +#include +#include +// Namespace RMAP +#include +#include + +#ifndef _CANARD_CLASS_H +#define _CANARD_CLASS_H + +class canardClass { + + // ***************** PUBLIC ACCESS ***************** + public: + + // ******************** Tipi di Dati ***************** + + // Tipologie elaborazioni/sensori modulo(i) + enum Sensor_Type : uint8_t { + bft + }; + + // Modalità di accesso a getMicros() + enum GetMonotonicTime_Type : uint8_t { + syncronized_time, + start_syncronization + }; + + // Gestione porte e subject di Canard + enum Introspection_Port : uint8_t { + PublisherSubjectID, + SubscriptionSubjectID, + ClientPortID, + ServicePortID + }; + + // Gestione comandi privati di Canard / Rmap + enum Command_Private : uint8_t { + download_file = 5, + calibrate_accelerometer = 6, + module_maintenance = 7, + reset_flags = 8, + enable_publish_rmap = 10, + disable_publish_rmap = 11, + enable_publish_port_list = 12, + disable_publish_port_list = 13, + remote_test = 99, + remote_test_value = 100 + }; + + // Interprete heartBeat VSC (Vendor Status Code) x Comunicazioni messaggi Master<->Slave + typedef union { + // Bit field + struct { + Power_Mode powerMode : 2; + uint8_t traceLog : 1; + uint8_t fwUploading : 1; + uint8_t dataReady : 1; + uint8_t moduleReady : 1; + uint8_t moduleError : 1; + }; + // uint8 value + uint8_t uint8_val; + } heartbeat_VSC; + + // Coda di ricezione dati per gestione rapida in Interrupt (STM32 IRQ_CB bxCan) + // Gestisce l'inserimento dei dati sulla coda locale, prima della gestione + // Con Canard con il passaggio dalla coda bufferizzata ai frame di Canard_Rx + typedef struct CanardRxQueue + { + // CanardTxQueue (Frame e Buffer x Interrupt gestione Coda FiFo) + uint8_t wr_ptr; + uint8_t rd_ptr; + struct msg { + CanardFrame frame; + uint8_t buf[CANARD_MTU_MAX]; + } msg[CAN_RX_QUEUE_CAPACITY]; + } CanardRxQueue; + + // ******************** Dati e Funzioni ****************** + + // Constructor di classe + canardClass(); + + // Timings Function e Dati associati + static CanardMicrosecond getMicros(); + static CanardMicrosecond getMicros(GetMonotonicTime_Type syncro_type); + static uint32_t getUpTimeSecond(void); + + // ************************************************* + // Canard SendData + // ************************************************* + // Wrapper per send e sendresponse con Canard + void send(CanardMicrosecond tx_deadline_usec, + const CanardTransferMetadata* const metadata, + const size_t payload_size, + const void* const payload); + // Risposte con inversione meta.transfer_kind alle Request + void sendResponse(const CanardMicrosecond tx_deadline_usec, + const CanardTransferMetadata* const request_metadata, + const size_t payload_size, + const void* const payload); + + // Gestione messaggi e coda di trasmissione + bool transmitQueueDataPresent(void); + void transmitQueue(void); + + // ************************************************* + // Canard Local ISR Buffer ReceiveData + // ************************************************* + // Gestione messaggi e coda di ricezione in buffer FIFO + void receiveQueueEmpty(void); + bool receiveQueueDataPresent(void); + uint8_t receiveQueueElement(void); + uint8_t receiveQueueNextElement(uint8_t currElement); + + // Ricezione messaggi per elementi in RX, con logger opzionale + void receiveQueue(void); + void receiveQueue(char *logMessage); + + // CallBack esterna per rxCanardAccept dei messaggi dal buffer RX (valid message) + // Richiamata se canardAccept accetta il pacchetto ricomposto dei frame in RX + void setReceiveMessage_CB (void (*ptrFunction) (canardClass&, const CanardRxTransfer*, void *), void *param); + void enableReceiveMessage_CB (void); + void disableReceiveMessage_CB (void); + + // ************************************************* + // Canard Metodi di sottoscrizione + // ************************************************* + + // Sottoscrizione ai metodi Canard + bool rxSubscribe(const CanardTransferKind transfer_kind, + const CanardPortID port_id, + const size_t extent, + const CanardMicrosecond transfer_id_timeout_usec); + uint8_t rxSubscriptionAvaiable(void); + void rxUnSubscribe(const CanardTransferKind transfer_kind, + const CanardPortID port_id); + + // **************************************************************************************** + // Comandi Server per gestione del modulo locale tramite classe Master e pending o Diretti + // **************************************************************************************** + bool send_file_read_block(CanardNodeID server_id, char * fileName, uint64_t remoteOffset); + bool master_file_read_block_pending(uint32_t timeout_us); + + // ************************************************* + // Sottoclassi MASTER locale per gestione di Stato + // ************************************************* + class master + { + public: + + // Heartbeat Online e Stato + class heartbeat { + + public: + + bool is_online(bool is_heart_syncronized); + void set_online(uint32_t dead_line_us); + CanardMicrosecond last_online(void); + + private: + + uint64_t _timeout_us; + + } heartbeat; + + // Time stamp + class timestamp { + + public: + + bool check_valid_syncronization(uint8_t current_transfer_id, + CanardMicrosecond previous_tx_timestamp_us); + CanardMicrosecond get_timestamp_syncronized(CanardMicrosecond current_rx_timestamp_us, + CanardMicrosecond previous_tx_timestamp_us); + CanardMicrosecond get_timestamp_syncronized(); + CanardMicrosecond update_timestamp_message(CanardMicrosecond current_rx_timestamp_us, + CanardMicrosecond previous_tx_timestamp_us); + private: + + CanardMicrosecond _syncronized_timestamp_us; + CanardMicrosecond _previous_syncronized_timestamp_us; + CanardMicrosecond _previous_local_timestamp_us; + CanardMicrosecond _previous_msg_monotonic_us; + uint8_t _previous_transfer_id; + + } timestamp; + + // File upload (node_id può essere differente dal master, es. yakut e lo riporto) + class file + { + + public: + + void start_request(uint8_t remote_node, uint8_t *param_request_name, + uint8_t param_request_len, bool is_firmware); + char *get_name(void); + CanardNodeID get_server_node(void); + bool download_request(void); + bool is_download_complete(void); + void download_end(void); + bool is_firmware(void); + uint64_t get_offset_rx(void); + void set_offset_rx(uint64_t remote_file_offset); + bool is_first_data_block(void); + bool next_retry(void); + bool next_retry(uint8_t *avaiable_retry); + void start_pending(uint32_t timeout_us); + void reset_pending(void); + void reset_pending(size_t message_len); + bool event_timeout(void); + bool is_pending(void); + + private: + + uint8_t _node_id; + char _filename[CAN_FILE_NAME_SIZE_MAX]; + bool _is_firmware; + bool _updating; + uint8_t _updating_retry; + bool _updating_eof; + uint64_t _offset; + uint64_t _timeout_us; // Time command Remoto x Verifica deadLine Request + bool _is_pending; // Funzione in pending (inviato, attesa risposta o timeout) + + } file; + + } master; + + // **************************************************************************************** + // Comandi Server per gestione del modulo locale tramite classe locale e pending o Diretti + // **************************************************************************************** + bool slave_heartbeat_send_message(void); + bool slave_pnp_send_request(uint64_t serial_number); + bool slave_servicelist_send_message(void); + + // ************************************************* + // Sottoclassi e dati locali per gestione di Stato + // ************************************************* + + // Dati e Metadati del modulo locale + rmap_module_Leaf_1_0 module_leaf; + + // Subject ID porte e servizi modulo locale + class port_id + { + public: + + CanardPortID publisher_module_leaf; + CanardPortID service_module_leaf; + + } port_id; + + // Abilitazione delle pubblicazioni falcoltative sulla rete (ON/OFF a richiesta) + class publisher_enabled + { + public: + + bool module_leaf; + bool port_list; + + } publisher_enabled; + + // Tranfer ID (CAN Interfaccia ID -> uint8) servizi attivi del modulo locale + class next_transfer_id + { + public: + + uint8_t uavcan_node_heartbeat(void); + uint8_t uavcan_node_port_list(void); + uint8_t uavcan_pnp_allocation(void); + uint8_t uavcan_file_read_data(void); + uint8_t module_leaf(void); + + private: + + uint8_t _uavcan_node_heartbeat; + uint8_t _uavcan_node_port_list; + uint8_t _uavcan_pnp_allocation; + uint8_t _uavcan_file_read_data; + uint8_t _module_leaf; + + } next_transfer_id; + + // Flag di stato + class flag + { + + public: + + // Funzioni Locali per gestione sistema + void request_system_restart(void); + bool is_requested_system_restart(void); + void request_sleep(void); + bool is_module_sleep(void); + void disable_sleep(void); + void enable_sleep(void); + // Funzioni Locali per gestione interna e VSC x HeartBeat VendorStatusCode a Bit MASK + void set_local_power_mode(Power_Mode powerMode); + void set_local_fw_uploading(bool fwUploading); + void set_local_data_ready(bool dataReady); + void set_local_module_ready(bool moduleReady); + void set_local_module_error(bool moduleError); + Power_Mode get_local_power_mode(void); + bool get_local_fw_uploading(void); + bool get_local_data_ready(void); + bool get_local_module_ready(void); + bool get_local_module_error(void); + uint8_t get_local_value_heartbeat_VSC(void); + // Funzioni Locali per gestione interna e VSC x HeartBeat VendorStatusCode a Bit MASK + void set_local_node_mode(uint8_t heartLocalMODE); + uint8_t get_local_node_mode(void); + + private: + + bool _restart_required; // Forza reboot + bool _enter_sleep; // Avvia la procedura di sleep del modulo + bool _inibith_sleep; // Inibisce lo sleep del modulo + // Var per Status locale da utilizzare, RW in Heart_beat + heartbeat_VSC _heartLocalVSC; + uint8_t _heartLocalMODE; + + } flag; + + // Local address + void set_canard_node_id(CanardNodeID local_id); + CanardNodeID get_canard_node_id(void); + bool is_canard_node_anonymous(void); + // Master address + void set_canard_master_id(CanardNodeID remote_id); + CanardNodeID get_canard_master_id(void); + + // ***************** PRIVATE ACCESS ***************** + private: + + // Istanza del modulo canard + CanardInstance _canard; + + // Node ID del MASTER remoto utilizzato per riferimento Set Flag e Comandi locali + // automatici come gestione flag modalità power, Sleep, errori ecc. + CanardNodeID _master_id; + + // Buffer dati e trasmissione delle code di tx e quella di rx + CanardRxQueue _canard_rx_queue; + CanardTxQueue _canard_tx_queues[CAN_REDUNDANCY_FACTOR]; + + // Timings var per getMicros(); + inline static uint32_t _lastMicros; + inline static uint64_t _currMicros; + inline static uint64_t _syncMicros; + inline static uint64_t _mastMicros; + + // Funzioni di utility private (sezione publish list_message) + void _fillSubscriptions(const CanardTreeNode* const tree, uavcan_node_port_SubjectIDList_0_1* const obj); + void _fillServers(const CanardTreeNode* const tree, uavcan_node_port_ServiceIDList_0_1* const obj); + + // Canard O1HEAP, Gestita RAM e CallBack internamente alla classe + O1HeapInstance* _heap; + _Alignas(O1HEAP_ALIGNMENT) uint8_t _heap_arena[HEAP_ARENA_SIZE]; + + // Gestione O1Heap Static Funzioni x Canard Memory Allocate/Free + static void* _memAllocate(CanardInstance* const ins, const size_t amount); + static void _memFree(CanardInstance* const ins, void* const pointer); + + O1HeapDiagnostics _memGetDiagnostics(void); + + // Indirizzo della funzione di CallBack Esterna su Rx Messaggio Canard + bool _attach_rx_callback; + void (*_attach_rx_callback_PTR) (canardClass&, const CanardRxTransfer*, void *); + void *_attach_param_PTR; + + // Gestione subscription locali + CanardRxSubscription _rxSubscription[MAX_SUBSCRIPTION]; + uint8_t _rxSubscriptionIdx; +}; + +#endif diff --git a/platformio/stima_v4/slave-leaf/include/canard_config.hpp b/platformio/stima_v4/slave-leaf/include/canard_config.hpp new file mode 100644 index 000000000..59c0d63a9 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/canard_config.hpp @@ -0,0 +1,120 @@ +/** + ****************************************************************************** + * @file canard_config.hpp + * @author Moreno Gasperini + * @brief Uavcan Canard Configuration file + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +// Assert Locali (Enable / disable) +#define LOCAL_ASSERT assert +// #define LOCAL_ASSERT (void(0)); + +#define KILO 1000L +#define MEGA ((int64_t)KILO * KILO) + +// CODA, RIDONDANZA, TIMEDELAY TX & RX CANARD +#define CAN_REDUNDANCY_FACTOR 1 +#define CAN_TX_QUEUE_CAPACITY 100 +#define CAN_MAX_IFACE 1 +#define CAN_RX_QUEUE_CAPACITY 100 +#define IFACE_CAN_IDX 0 +#define CAN_DELAY_US_SEND 0 +#define MAX_SUBSCRIPTION 10 +#define HEAP_ARENA_SIZE (1024 * 16) + +// CAN SPEED RATE HZ +#define CAN_BIT_RATE 1000000ul +#define CAN_MTU_BASE 8 + +// Messaggi di lunghezza TimeOut Extra <> Standard +#define CANARD_REGISTERLIST_TRANSFER_ID_TIMEOUT_USEC 3500000UL +#define CANARD_READFILE_TRANSFER_ID_TIMEOUT_USEC 2500000UL +#define CANARD_RMAPDATA_TRANSFER_ID_TIMEOUT_USEC 2500000UL + +// Nodi Fissi per Moduli Master e Yakut +#define NODE_YAKUT_ID 127 +#define NODE_MASTER_ID 100 + +#define PORT_RMAP_TH 50 +#define PORT_RMAP_RAIN 51 +#define PORT_RMAP_WIND 52 +#define PORT_RMAP_RADIATION 53 +#define PORT_RMAP_MPPT 54 +#define PORT_RMAP_VWC 55 +#define PORT_RMAP_LEAF 56 +#define PORT_RMAP_MASTER 100 + +// Parametri default per Modulo Slave (INIT_PARAMETER) +#define NODE_VALUE_UNSET 255 +#define NODE_SLAVE_ID 63 +#define PORT_SERVICE_RMAP (PORT_RMAP_LEAF) +#define SUBJECTID_PUBLISH_RMAP (PORT_RMAP_LEAF) + + +// Maschera Check S.N. messaggio Hash Canard per PnP +#define HASH_SERNUMB_MASK 0x0000FFFFFFFFFF00u +#define HASH_EXCLUDING_BIT 16u + +// (FIXED CONFIGURATION, NO READING FROM REGISTER) +#if (FIXED_CONFIGURATION) +#define USE_NODE_MASTER_ID_FIXED +#define USE_NODE_SLAVE_ID_FIXED +#define USE_PORT_SERVICE_RMAP_FIXED +#define USE_SUBJECTID_PUBLISH_RMAP_FIXED +#endif + +// METADATA CONFIG AND DEFAULT VALUE +#define SENSOR_METADATA_BFT 0 +#define SENSOR_METADATA_COUNT 1 +// METADATA DEFAULT VALUE REGISTER INIT (P_IND, P1, P2 / TYPE_1, L1, TYPE_2, L2) +#define SENSOR_METADATA_LEVEL_1 65535 +#define SENSOR_METADATA_LEVEL_2 65535 +#define SENSOR_METADATA_LEVELTYPE_1 1 +#define SENSOR_METADATA_LEVELTYPE_2 65535 +#define SENSOR_METADATA_LEVEL_P1 0 +#define SENSOR_METADATA_LEVEL_P_IND_BFT 0 + +// SET Default value per risposte +#define GENERIC_STATE_UNDEFINED 0x0Fu +#define GENERIC_BVAL_UNDEFINED 0xFFu + +// Servizi Cypal attivi di default +#define DEFAULT_PUBLISH_PORT_LIST true +#define DEFAULT_PUBLISH_MODULE_DATA false + +// Time Publisher Servizi (secondi) +#define TIME_PUBLISH_MODULE_DATA 0.333 +#define TIME_PUBLISH_PNP_REQUEST 4 +#define TIME_PUBLISH_HEARTBEAT 1 +#define TIME_PUBLISH_PORT_LIST 20 + +// TimeOUT (millisecondi) +#define MASTER_OFFLINE_TIMEOUT_US 6000000 +#define MASTER_MAXSYNCRO_VALID_US 1250000 +#define NODE_GETFILE_TIMEOUT_US 1750000 +#define NODE_GETFILE_MAX_RETRY 3 + +// CODICI E STATUS AGGIORNAMENTO FILE REMOTI +#define CAN_FILE_NAME_SIZE_MAX 50 diff --git a/platformio/stima_v4/slave-leaf/include/config.h b/platformio/stima_v4/slave-leaf/include/config.h new file mode 100644 index 000000000..5388b37d8 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/config.h @@ -0,0 +1,255 @@ +/**@file config.h */ + +/********************************************************************* +

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+authors: +Marco Baldinetti + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +. +**********************************************************************/ + +#ifndef _CONFIG_H +#define _CONFIG_H + +#include "sensors_config.h" +#include "stima_config.h" + +/********************************************************************* +* MODULE +*********************************************************************/ +/// @brief Module main version. +#define MODULE_MAIN_VERSION (4) + +/// @brief Module minor version. +#define MODULE_MINOR_VERSION (0) + +/// @brief rmap protocol version +#define RMAP_PROCOTOL_VERSION (1) + +// Random generator value for Local Test +// #define USE_SIMULATOR + +// Fill buffer data wuith 900 data value init +// #define INIT_SIMULATOR + +/*! +\def MODULE_TYPE +\brief Type of module. It is defined in registers.h. +*/ +#if (USE_MODULE_THR) +#define MODULE_TYPE (STIMA_MODULE_TYPE_THR) +#elif (USE_MODULE_TH) +#define MODULE_TYPE (STIMA_MODULE_TYPE_TH) +#elif (USE_MODULE_RAIN) +#define MODULE_TYPE (STIMA_MODULE_TYPE_RAIN) +#elif (USE_MODULE_LEAF) +#define MODULE_TYPE (STIMA_MODULE_TYPE_LEAF) +#endif + +/// @brief Enable control Error for Debug +#define DEBUG_MODE (false) +#define ERROR_HANDLER_CB (false) + +/********************************************************************* +* HW DEVICES +*********************************************************************/ + +/// @brief Enable I2C1 interface +#define ENABLE_I2C1 (true) +/// @brief Enable I2C2 interface +#define ENABLE_I2C2 (true) +/// @brief Enable QSPI interface +#define ENABLE_QSPI (true) +/// @brief Enable CAN BUS interface +#define ENABLE_CAN (true) +/// @brief Enable I2C Accelerometer +#define ENABLE_ACCELEROMETER (false) + +/// @brief Enable HW Diag PIN redefine +#define ENABLE_DIAG_PIN (false) + +// Enable (Wdt Task and Module) and relative Function (Stack, Info ecc...) +/// @brief Enable WatchDog Task and Module +#define ENABLE_WDT (true) +/// @brief WatchDog Hardware microseconds timeout +#define WDT_TIMEOUT_BASE_US (8000000) +/// @brief Init WatchDog Task local milliseconds +#define WDT_STARTING_TASK_MS (60000) +/// @brief Task milliseconds minimal check +#define WDT_CONTROLLER_MS (2000) +/// @brief Enable stack usage +#define ENABLE_STACK_USAGE (true) +/// @brief Monitor Sub Position not used flag +#define UNUSED_SUB_POSITION (0) +/// @brief Monitor No Sleep / No Suspend +#define NORMAL_STATE (0) +/// @brief Sleep Task For Wdt or LowPower Check +#define SLEEP_STATE (1) +/// @brief Suspend Task from Wdt +#define SUSPEND_STATE (2) + +/********************************************************************* +* Generic Semaphore Time acquire RTC +*********************************************************************/ +/// @brief Enable RTC Interface +#define ENABLE_RTC (true) +/// @brief Delay for RTC in milliseconds +#define RTC_WAIT_DELAY_MS (100) + +/********************************************************************* +* Address EEProm for reserved bootloader flag param (and future used 2000 Bytes) +*********************************************************************/ +/// @brief Starting EEPROM address +#define START_EEPROM_ADDRESS (0) +/// @brief Size EEPROM reserved address. Must be > CONFIGURATION_EEPROM_END +#define SIZE_EEPROM_RESERVED (450) +/// @brief Bootloader start address +#define BOOT_LOADER_STRUCT_ADDR (START_EEPROM_ADDRESS) +/// @brief Bootloader struct size +#define BOOT_LOADER_STRUCT_SIZE (sizeof(bootloader_t)) +/// @brief Bootloader struct end address +#define BOOT_LOADER_STRUCT_END (START_EEPROM_ADDRESS + BOOT_LOADER_STRUCT_SIZE) + +/********************************************************************* +* Private configuration board direct +*********************************************************************/ +/// @brief Start Address EEPROM configuration +#define CONFIGURATION_EEPROM_ADDRESS (20) +/// @brief Start Standard UAVCAN Register +#define REGISTER_EEPROM_ADDRESS (START_EEPROM_ADDRESS + SIZE_EEPROM_RESERVED) + +/// @brief Monitor Debug Serial speed +#define SERIAL_DEBUG_BAUD_RATE (115200) + +// HW I2C Speed BUS and specific config +#if (ENABLE_I2C1 || ENABLE_I2C2) +#define I2C_MAX_DATA_LENGTH (32) +#define I2C_MAX_ERROR_COUNT (3) +#endif + +#if (ENABLE_I2C1) +#define I2C1_BUS_CLOCK_HZ (100000L) +#endif +#if (ENABLE_I2C2) +#define I2C2_BUS_CLOCK_HZ (100000L) +#endif + +/********************************************************************* +* Queue Lenght +*********************************************************************/ +/// @brief Request system message queue length +#define SYSTEM_MESSAGE_QUEUE_LENGTH (4) +/// @brief Elaborate data message queue length +#define ELABORATE_DATA_QUEUE_LENGTH (4) +/// @brief Request data message queue length +#define REQUEST_DATA_QUEUE_LENGTH (1) +/// @brief Report data message queue length +#define REPORT_DATA_QUEUE_LENGTH (1) + +/********************************************************************* +* Task system_status and queue ID message +*********************************************************************/ +/// @brief All task ID. Send message to ALL Task +#define ALL_TASK_ID (99) +/// @brief Supervisor task ID +#define SUPERVISOR_TASK_ID (0) +/// @brief CAN task ID +#define CAN_TASK_ID (1) +/// @brief Elaborate data task ID +#define ELABORATE_TASK_ID (2) +/// @brief Sensor acquire task ID +#define SENSOR_TASK_ID (3) +/// @brief Accelerometer task ID +#define ACCELEROMETER_TASK_ID (4) +/// @brief Watch Dog task ID +#define WDT_TASK_ID (5) +/// @brief Total Max Task for WDT Task Control +#define TOTAL_INFO_TASK (WDT_TASK_ID + 1) + +/********************************************************************* +* Global queue wait and other timeout +*********************************************************************/ +/// @brief Time to wait pushing data queue +#define WAIT_QUEUE_REQUEST_PUSHDATA_MS (500) +/// @brief Time to wait pushing command queue +#define WAIT_QUEUE_REQUEST_COMMAND_MS (500) + +/********************************************************************* +* Parameter of buffer data dimension and acquire +*********************************************************************/ +/// @brief Sample and default value for elaborate task +#define SAMPLES_COUNT_MAX (3600) +/// @brief Default observation (RMAP) time in second +#define OBSERVATIONS_TIME_S (60) +/// @brief Default report (RMAP) time in second +#define REPORTS_TIME_S (900) + +/********************************************************************* +* Parameter of sensor and elaboration function +*********************************************************************/ +/// @brief Default delay from two function acquire data +#define SENSORS_ACQUISITION_DELAY_MS (4000) + +// Index Sensor +#define LEAF_INDEX (0) + +// Caratteristic of input type selected chanel +#define ADC_VOLTAGE_MAX_V (14.4) +#define ADC_VOLTAGE_MIN_V (0.0) +#define ADC_VOLTAGE_MAX_MV (3300.0) +#define ADC_VOLTAGE_MIN_MV (0.0) +#define ADC_CURRENT_MAX_MA (20.0) +#define ADC_CURRENT_MIN_MA (0.0) + +/// @brief Limit MAX resolution adc value for module sensor +#define ADC_MAX (4096) +/// @brief Limit MIN resolution adc value for module sensor +#define ADC_MIN (0) + +/// @brief Limit MAX voltage adc range for module sensor +#define LEAF_VOLTAGE_MAX (5000.0) +/// @brief Limit MIN voltage adc range for module sensor +#define LEAF_VOLTAGE_MIN (0.0) + +/// @brief Limit Error voltage MAX valid range for module sensor +#define LEAF_ERROR_VOLTAGE_MAX (LEAF_VOLTAGE_MAX + 30.0) +/// @brief Limit Error voltage MIN valid range for module sensor +#define LEAF_ERROR_VOLTAGE_MIN (LEAF_VOLTAGE_MIN - 30.0) + +/// @brief Limit MAX valid range for module sensor (3600 seconds = 1 hour) +#define LEAF_MAX (3600.0) +/// @brief Limit MIN valid range for module sensor +#define LEAF_MIN (0.0) + +/// @brief Sensibility MIN sensor start range valid data +#define LEAF_SENSIBILITY (5.0) +/// @brief Limit MAX error valid range for module sensor +#define LEAF_ERROR_MAX (3600.0) +/// @brief Limit MIN error valid range for module sensor +#define LEAF_ERROR_MIN (0.0) + +/// @brief ADC MIN error on retrieve data for get a valid data +#define ADC_ERROR_PERCENTAGE_MIN (70) + +/// @brief Samples min percent valid on elaboration data +#define SAMPLE_ERROR_PERCENTAGE_MIN (90.0) +/// @brief Observation min percent valid on elaboration data +#define OBSERVATION_ERROR_PERCENTAGE_MIN (90.0) + +/// @brief Amount of cycles for reset sensor +#define ACQUISITION_COUNT_FOR_POWER_RESET (100) + +#endif diff --git a/platformio/stima_v4/slave-leaf/include/debug.h b/platformio/stima_v4/slave-leaf/include/debug.h new file mode 100644 index 000000000..b432c0cbf --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/debug.h @@ -0,0 +1,162 @@ +/**@file debug.h */ + +/********************************************************************* +

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+authors: +Marco Baldinetti + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +. +**********************************************************************/ + +#ifndef _DEBUG_H +#define _DEBUG_H + +//Dependencies +#include +#include +#include +#include "os_port.h" + +#define OK_STRING "OK" +#define NO_STRING "NO" +#define YES_STRING "YES" +#define FAIL_STRING "FAIL" +#define ERROR_STRING "ERROR" +#define REDUNDANT_STRING "REDUNDANT" +#define MAIN_STRING "MAIN" +#define ON_STRING "ON" +#define OFF_STRING "OFF" +#define SUSPEND_STRING "x" +#define FLAG_STRING "*" +#define SPACE_STRING " " + +//Trace level definitions +#define TRACE_LEVEL_OFF 0 +#define TRACE_LEVEL_FATAL 1 +#define TRACE_LEVEL_ERROR 2 +#define TRACE_LEVEL_WARNING 3 +#define TRACE_LEVEL_INFO 4 +#define TRACE_LEVEL_DEBUG 5 +#define TRACE_LEVEL_VERBOSE 6 + +//Default trace level +#ifndef TRACE_LEVEL + #define TRACE_LEVEL TRACE_LEVEL_DEBUG +#endif + +void print_debug(const char *fmt, ...); + +//Trace output redirection +#ifndef TRACE_PRINTF +#define TRACE_PRINTF(...) osSuspendAllTasks(), print_debug(__VA_ARGS__), osResumeAllTasks() +#endif + +#ifndef TRACE_ARRAY +#define TRACE_ARRAY(p, a, n) osSuspendAllTasks(), print_debug_array(p, a, n), osResumeAllTasks() +#endif + +#ifndef TRACE_MPI + #define TRACE_MPI(p, a) osSuspendAllTasks(), mpiDump(stdout, p, a), osResumeAllTasks() +#endif + +//Debugging macros +#if (TRACE_LEVEL >= TRACE_LEVEL_FATAL) + #define TRACE_FATAL(...) TRACE_PRINTF(__VA_ARGS__) + #define TRACE_FATAL_ARRAY(p, a, n) TRACE_ARRAY(p, a, n) + #define TRACE_FATAL_MPI(p, a) TRACE_MPI(p, a) +#else + #define TRACE_FATAL(...) + #define TRACE_FATAL_ARRAY(p, a, n) + #define TRACE_FATAL_MPI(p, a) +#endif + +#if (TRACE_LEVEL >= TRACE_LEVEL_ERROR) + #define TRACE_ERROR(...) TRACE_PRINTF(__VA_ARGS__) + #define TRACE_ERROR_ARRAY(p, a, n) TRACE_ARRAY(p, a, n) + #define TRACE_ERROR_MPI(p, a) TRACE_MPI(p, a) +#else + #define TRACE_ERROR(...) + #define TRACE_ERROR_ARRAY(p, a, n) + #define TRACE_ERROR_MPI(p, a) +#endif + +#if (TRACE_LEVEL >= TRACE_LEVEL_WARNING) + #define TRACE_WARNING(...) TRACE_PRINTF(__VA_ARGS__) + #define TRACE_WARNING_ARRAY(p, a, n) TRACE_ARRAY(p, a, n) + #define TRACE_WARNING_MPI(p, a) TRACE_MPI(p, a) +#else + #define TRACE_WARNING(...) + #define TRACE_WARNING_ARRAY(p, a, n) + #define TRACE_WARNING_MPI(p, a) +#endif + +#if (TRACE_LEVEL >= TRACE_LEVEL_INFO) + #define TRACE_INFO(...) TRACE_PRINTF(__VA_ARGS__) + #define TRACE_INFO_ARRAY(p, a, n) TRACE_ARRAY(p, a, n) + #define TRACE_INFO_NET_BUFFER(p, b, o, n) + #define TRACE_INFO_MPI(p, a) TRACE_MPI(p, a) +#else + #define TRACE_INFO(...) + #define TRACE_INFO_ARRAY(p, a, n) + #define TRACE_INFO_NET_BUFFER(p, b, o, n) + #define TRACE_INFO_MPI(p, a) +#endif + +#if (TRACE_LEVEL >= TRACE_LEVEL_DEBUG) + #define TRACE_DEBUG(...) TRACE_PRINTF(__VA_ARGS__) + #define TRACE_DEBUG_ARRAY(p, a, n) TRACE_ARRAY(p, a, n) + #define TRACE_DEBUG_NET_BUFFER(p, b, o, n) + #define TRACE_DEBUG_MPI(p, a) TRACE_MPI(p, a) +#else + #define TRACE_DEBUG(...) + #define TRACE_DEBUG_ARRAY(p, a, n) + #define TRACE_DEBUG_NET_BUFFER(p, b, o, n) + #define TRACE_DEBUG_MPI(p, a) +#endif + +#if (TRACE_LEVEL >= TRACE_LEVEL_VERBOSE) + #define TRACE_VERBOSE(...) TRACE_PRINTF(__VA_ARGS__) + #define TRACE_VERBOSE_ARRAY(p, a, n) TRACE_ARRAY(p, a, n) + #define TRACE_VERBOSE_NET_BUFFER(p, b, o, n) + #define TRACE_VERBOSE_MPI(p, a) TRACE_MPI(p, a) +#else + #define TRACE_VERBOSE(...) + #define TRACE_VERBOSE_ARRAY(p, a, n) + #define TRACE_VERBOSE_NET_BUFFER(p, b, o, n) + #define TRACE_VERBOSE_MPI(p, a) +#endif + +#define printError(error, ok_str, error_str) (error == NO_ERROR ? ok_str : error_str) + +//C++ guard +#ifdef __cplusplus + extern "C" { +#endif + +//Debug related functions +void init_debug(uint32_t baudrate); + +void print_debug_array(const char *prepend, const void *data, size_t length); + +//Deprecated definitions +#define TRACE_LEVEL_NO_TRACE TRACE_LEVEL_OFF + +// C++ guard +#ifdef __cplusplus +} +#endif + +#endif diff --git a/platformio/stima_v4/slave-leaf/include/debug_F.h b/platformio/stima_v4/slave-leaf/include/debug_F.h new file mode 100644 index 000000000..c354940b4 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/debug_F.h @@ -0,0 +1,84 @@ +/**@file debug_F.h */ + +/********************************************************************* +

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+authors: +Marco Baldinetti + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +. +**********************************************************************/ + +#ifndef _DEBUG_F_H +#define _DEBUG_F_H + +//Dependencies +#include "debug.h" + +void print_debug_F(const __FlashStringHelper *fmt, ...); + +//Trace output redirection +#ifndef TRACE_PRINTF_F +#define TRACE_PRINTF_F(...) print_debug_F(__VA_ARGS__) +#endif + +//Debugging macros +#if (TRACE_LEVEL >= TRACE_LEVEL_FATAL) + #define TRACE_FATAL_F(...) TRACE_PRINTF_F(__VA_ARGS__) +#else + #define TRACE_FATAL_F(...) +#endif + +#if (TRACE_LEVEL >= TRACE_LEVEL_ERROR) + #define TRACE_ERROR_F(...) TRACE_PRINTF_F(__VA_ARGS__) +#else + #define TRACE_ERROR_F(...) +#endif + +#if (TRACE_LEVEL >= TRACE_LEVEL_WARNING) + #define TRACE_WARNING_F(...) TRACE_PRINTF_F(__VA_ARGS__) +#else + #define TRACE_WARNING_F(...) +#endif + +#if (TRACE_LEVEL >= TRACE_LEVEL_INFO) + #define TRACE_INFO_F(...) TRACE_PRINTF_F(__VA_ARGS__) +#else + #define TRACE_INFO_F(...) +#endif + +#if (TRACE_LEVEL >= TRACE_LEVEL_DEBUG) + #define TRACE_DEBUG_F(...) TRACE_PRINTF_F(__VA_ARGS__) +#else + #define TRACE_DEBUG_F(...) +#endif + +#if (TRACE_LEVEL >= TRACE_LEVEL_VERBOSE) + #define TRACE_VERBOSE_F(...) TRACE_PRINTF_F(__VA_ARGS__) +#else + #define TRACE_VERBOSE_F(...) +#endif + +//C++ guard +#ifdef __cplusplus +extern "C" { +#endif + +// C++ guard +#ifdef __cplusplus +} +#endif + +#endif diff --git a/platformio/stima_v4/slave-leaf/include/debug_config.h b/platformio/stima_v4/slave-leaf/include/debug_config.h new file mode 100644 index 000000000..5516a46b3 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/debug_config.h @@ -0,0 +1,51 @@ +/**@file debug_config.h */ + +/********************************************************************* +

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+authors: +Marco Baldinetti + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +. +**********************************************************************/ + +#ifndef _DEBUG_CONFIG_H +#define _DEBUG_CONFIG_H + +#if USE_DEBUG + +#define STIMA_TRACE_LEVEL TRACE_LEVEL_INFO +#define SUPERVISOR_TASK_TRACE_LEVEL TRACE_LEVEL_INFO +#define CAN_TASK_TRACE_LEVEL TRACE_LEVEL_INFO +#define ACCELEROMETER_TASK_TRACE_LEVEL TRACE_LEVEL_OFF +#define WDT_TASK_TRACE_LEVEL TRACE_LEVEL_INFO +#define SOLAR_RADIATION_SENSOR_TASK_TRACE_LEVEL TRACE_LEVEL_VERBOSE +#define LEAF_SENSOR_TASK_TRACE_LEVEL TRACE_LEVEL_VERBOSE +#define ELABORATE_DATA_TASK_TRACE_LEVEL TRACE_LEVEL_VERBOSE + +#else + +#define STIMA_TRACE_LEVEL TRACE_LEVEL_OFF +#define SUPERVISOR_TASK_TRACE_LEVEL TRACE_LEVEL_OFF +#define CAN_TASK_TRACE_LEVEL TRACE_LEVEL_OFF +#define ACCELEROMETER_TASK_TRACE_LEVEL TRACE_LEVEL_OFF +#define WDT_TASK_TRACE_LEVEL TRACE_LEVEL_OFF +#define SOLAR_RADIATION_SENSOR_TASK_TRACE_LEVEL TRACE_LEVEL_OFF +#define LEAF_SENSOR_TASK_TRACE_LEVEL TRACE_LEVEL_OFF +#define ELABORATE_DATA_TASK_TRACE_LEVEL TRACE_LEVEL_OFF + +#endif + +#endif diff --git a/platformio/stima_v4/slave-leaf/include/drivers/AT25SF161.h b/platformio/stima_v4/slave-leaf/include/drivers/AT25SF161.h new file mode 100644 index 000000000..e21408e26 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/drivers/AT25SF161.h @@ -0,0 +1,132 @@ +/** + ****************************************************************************** + * @file AT25SF161.h (Flash Register and Config 2_MByte) + * @author Moreno Gasperini + * @brief This file contains all the description of the AT25SF161 QSPI memory. + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __AT25SF161_H +#define __AT25SF161_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ + +/** + * @brief AT25SF161 Configuration + */ +#define AT25SF161_FLASH_SIZE 0x200000 /* 16 MBits => 2MBytes */ +#define AT25SF161_SECTOR64_SIZE 0x10000 /* 32 sectors of 64KBytes */ +#define AT25SF161_SECTOR_SIZE 0x8000 /* 64 sectors of 32KBytes */ +#define AT25SF161_BLOCK_SIZE 0x1000 /* 512 blocks of 4kBytes */ +#define AT25SF161_PAGE_SIZE 0x100 /* 8192 pages of 256 bytes */ + +#define AT25SF161_DUMMY_CYCLES_READ 1 +#define AT25SF161_DUMMY_CYCLES_READ_QUAD 4 + +#define AT25SF161_BULK_ERASE_MAX_TIME 25000 +#define AT25SF161_SECTOR_ERASE_MAX_TIME 3000 +#define AT25SF161_BLOCK_ERASE_MAX_TIME 300 + +/** + * @brief AT25SF161 Commands + */ +/* Reset Operations */ +#ifndef __AT25SF161_H +#define RESET_ENABLE_CMD 0x66 +#define RESET_MEMORY_CMD 0x99 + +/* Identification Commands */ +#define READ_ID_CMD 0x9E +#define READ_ID_CMD2 0x9F +#define MULTIPLE_IO_READ_ID_CMD 0xAF +#define READ_SERIAL_FLASH_DISCO_PARAM_CMD 0x5A +#endif + +/* Read Commands */ +#define READ_CMD 0x03 +#define FAST_READ_CMD 0x0B +#define DUAL_OUT_FAST_READ_CMD 0x3B +#define DUAL_INOUT_FAST_READ_CMD 0xBB +#define QUAD_OUT_FAST_READ_CMD 0x6B +#define QUAD_INOUT_FAST_READ_CMD 0xEB +#define QUAD_CONTINUOUS_READ_MODE_RESET 0xFF + +/* Protection Commands */ +#define WRITE_ENABLE_CMD 0x06 +#define WRITE_DISABLE_CMD 0x04 + +/* Status Register Commands */ +#define READ_STATUS_REG_CMD 0x05 +#define READ_STATUS2_REG_CMD 0x35 +#define WRITE_STATUS_REG_CMD 0x01 +#define WRITE_EN_VOLAT_STATUS_REG_CMD 0x50 + +/* Program Command */ +#define PAGE_PROG_CMD 0x02 + +/* Erase Commands */ +#define BLOCK_ERASE_CMD 0x20 +#define SECTOR_ERASE_CMD 0x52 +#define SECTOR64_ERASE_CMD 0xD8 +#define BULK_ERASE_CMD 0xC7 +#define PROG_ERASE_RESUME_CMD 0x7A +#define PROG_ERASE_SUSPEND_CMD 0x75 + +/* Security Commands */ +#define READ_SEC_REG_PAGE_CMD 0x48 +#define WRITE_SEC_REG_PAGE_CMD 0x42 + +/* Miscellaneous Commands */ +#define READ_ID_CMD 0x90 +#define READ_ID_CMD2 0x9F +#define DEEP_POWER_DOWN_CMD 0xB9 +#define RESUME_FROM_DEEP_PWD_CMD 0xAB + + +/** + * @brief AT25SF161 Registers + */ +/* Status Register byte 1 (command READ_STATUS_REG_CMD) */ +#define AT25SF161_SR_BUSY ((uint32_t)0x0001) /*!< Device busy */ +#define AT25SF161_SR_WEL ((uint32_t)0x0002) /*!< Write enable latch */ +#define AT25SF161_SR_BLOCKPR ((uint32_t)0x005C) /*!< Block protected against program and erase operations */ +#define AT25SF161_SR_PRBOTTOM ((uint32_t)0x0020) /*!< Protected memory area defined by BLOCKPR starts from top or bottom */ +#define AT25SF161_SR_SRP0 ((uint32_t)0x0080) /*!< Status register protection bit 0 */ +/* Status Register byte 2 (command READ_STATUS2_REG_CMD) */ +#define AT25SF161_SR_SRP1 ((uint32_t)0x0100) /*!< Status register protection bit 1 */ +#define AT25SF161_SR_QE ((uint32_t)0x0200) /*!< Quad Enable */ +#define AT25SF161_SR_LB ((uint32_t)0x3800) /*!< Lock security register */ +#define AT25SF161_SR_CMP ((uint32_t)0x4000) /*!< Complement Block Protection */ +#define AT25SF161_FS_ERSUS ((uint32_t)0x8000) /*!< Erase operation suspended */ + +#ifdef __cplusplus +} +#endif + +#endif /* __AT25SF161_H */ \ No newline at end of file diff --git a/platformio/stima_v4/slave-leaf/include/drivers/accelerometer.h b/platformio/stima_v4/slave-leaf/include/drivers/accelerometer.h new file mode 100644 index 000000000..76e55d9d3 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/drivers/accelerometer.h @@ -0,0 +1,678 @@ +/** + ****************************************************************************** + * @file accelerometer.h + * @author Moreno Gasperini + * @brief accelerometer IIS328DQ header file + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef _ACCELEROMETER_H +#define _ACCELEROMETER_H + + +/* Includes ------------------------------------------------------------------*/ +#include +#include "ticks.hpp" +#include "thread.hpp" +#include "semaphore.hpp" +#include "Wire.h" + +using namespace cpp_freertos; + +#define ACCELEROMETER_SEMAPHORE_MAX_WAITING_TIME_MS (1000) + +/** IIS328DQ Accelerometer + * I2C Device Address 8 bit format depends from BIT SA0 -> 0x18 - 0x19 */ +#define ACCELEROMETER_IIS328DQ_I2C_ADDR_DEFAULT (0x19) +#define ACCELEROMETER_WAIT_CHECK_HARDWARE (5) +#define ACCELEROMETER_MAX_CHECK_ATTEMPT (5) +/** Device Identification (Who am I) **/ +#define IIS328DQ_ID 0x32 + +/** BIT Mode definitions */ +#define DRV_LITTLE_ENDIAN __ORDER_LITTLE_ENDIAN__ +#define DRV_BIG_ENDIAN __ORDER_BIG_ENDIAN__ +#define DRV_BYTE_ORDER __BYTE_ORDER__ +#define PROPERTY_DISABLE (0U) +#define PROPERTY_ENABLE (1U) + +#define ARR_REG_FILTER 10 + +// Class Accelerometer implementation +class Accelerometer { + +public: + + // ********* Typedef Structure and register ******** // + + // Local COORD for gestion raw_data + enum coordinate { + X, + Y, + Z + }; + + typedef struct + { + #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t bit0 : 1; + uint8_t bit1 : 1; + uint8_t bit2 : 1; + uint8_t bit3 : 1; + uint8_t bit4 : 1; + uint8_t bit5 : 1; + uint8_t bit6 : 1; + uint8_t bit7 : 1; + #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t bit7 : 1; + uint8_t bit6 : 1; + uint8_t bit5 : 1; + uint8_t bit4 : 1; + uint8_t bit3 : 1; + uint8_t bit2 : 1; + uint8_t bit1 : 1; + uint8_t bit0 : 1; + #endif /* DRV_BYTE_ORDER */ + } bitwise_t; + + #define IIS328DQ_WHO_AM_I 0x0FU + #define IIS328DQ_CTRL_REG1 0x20U + typedef struct + { + #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t xen : 1; + uint8_t yen : 1; + uint8_t zen : 1; + uint8_t dr : 2; + uint8_t pm : 3; + #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t pm : 3; + uint8_t dr : 2; + uint8_t zen : 1; + uint8_t yen : 1; + uint8_t xen : 1; + #endif /* DRV_BYTE_ORDER */ + } iis328dq_ctrl_reg1_t; + + #define IIS328DQ_CTRL_REG2 0x21U + typedef struct + { + #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t hpcf : 2; + uint8_t hpen : 2; + uint8_t fds : 1; + uint8_t hpm : 2; + uint8_t boot : 1; + #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t boot : 1; + uint8_t hpm : 2; + uint8_t fds : 1; + uint8_t hpen : 2; + uint8_t hpcf : 2; + #endif /* DRV_BYTE_ORDER */ + } iis328dq_ctrl_reg2_t; + + #define IIS328DQ_CTRL_REG3 0x22U + typedef struct + { + #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t i1_cfg : 2; + uint8_t lir1 : 1; + uint8_t i2_cfg : 2; + uint8_t lir2 : 1; + uint8_t pp_od : 1; + uint8_t ihl : 1; + #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ihl : 1; + uint8_t pp_od : 1; + uint8_t lir2 : 1; + uint8_t i2_cfg : 2; + uint8_t lir1 : 1; + uint8_t i1_cfg : 2; + #endif /* DRV_BYTE_ORDER */ + } iis328dq_ctrl_reg3_t; + + #define IIS328DQ_CTRL_REG4 0x23U + typedef struct + { + #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sim : 1; + uint8_t st : 3; /* STsign + ST */ + uint8_t fs : 2; + uint8_t ble : 1; + uint8_t bdu : 1; + #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t bdu : 1; + uint8_t ble : 1; + uint8_t fs : 2; + uint8_t st : 3; /* STsign + ST */ + uint8_t sim : 1; + #endif /* DRV_BYTE_ORDER */ + } iis328dq_ctrl_reg4_t; + + #define IIS328DQ_CTRL_REG5 0x24U + typedef struct + { + #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t turnon : 2; + uint8_t not_used_01 : 6; + #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 6; + uint8_t turnon : 2; + #endif /* DRV_BYTE_ORDER */ + } iis328dq_ctrl_reg5_t; + + #define IIS328DQ_HP_FILTER_RESET 0x25U + #define IIS328DQ_REFERENCE 0x26U + #define IIS328DQ_STATUS_REG 0x27U + typedef struct + { + #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t xda : 1; + uint8_t yda : 1; + uint8_t zda : 1; + uint8_t zyxda : 1; + uint8_t _xor : 1; + uint8_t yor : 1; + uint8_t zor : 1; + uint8_t zyxor : 1; + #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t zyxor : 1; + uint8_t zor : 1; + uint8_t yor : 1; + uint8_t _xor : 1; + uint8_t zyxda : 1; + uint8_t zda : 1; + uint8_t yda : 1; + uint8_t xda : 1; + #endif /* DRV_BYTE_ORDER */ + } iis328dq_status_reg_t; + + #define IIS328DQ_OUT_X_L 0x28U + #define IIS328DQ_OUT_X_H 0x29U + #define IIS328DQ_OUT_Y_L 0x2AU + #define IIS328DQ_OUT_Y_H 0x2BU + #define IIS328DQ_OUT_Z_L 0x2CU + #define IIS328DQ_OUT_Z_H 0x2DU + #define IIS328DQ_INT1_CFG 0x30U + typedef struct + { + #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t xlie : 1; + uint8_t xhie : 1; + uint8_t ylie : 1; + uint8_t yhie : 1; + uint8_t zlie : 1; + uint8_t zhie : 1; + uint8_t _6d : 1; + uint8_t aoi : 1; + #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t aoi : 1; + uint8_t _6d : 1; + uint8_t zhie : 1; + uint8_t zlie : 1; + uint8_t yhie : 1; + uint8_t ylie : 1; + uint8_t xhie : 1; + uint8_t xlie : 1; + #endif /* DRV_BYTE_ORDER */ + } iis328dq_int1_cfg_t; + + #define IIS328DQ_INT1_SRC 0x31U + typedef struct + { + #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t xl : 1; + uint8_t xh : 1; + uint8_t yl : 1; + uint8_t yh : 1; + uint8_t zl : 1; + uint8_t zh : 1; + uint8_t ia : 1; + uint8_t not_used_01 : 1; + #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 1; + uint8_t ia : 1; + uint8_t zh : 1; + uint8_t zl : 1; + uint8_t yh : 1; + uint8_t yl : 1; + uint8_t xh : 1; + uint8_t xl : 1; + #endif /* DRV_BYTE_ORDER */ + } iis328dq_int1_src_t; + + #define IIS328DQ_INT1_THS 0x32U + typedef struct + { + #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ths : 7; + uint8_t not_used_01 : 1; + #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 1; + uint8_t ths : 7; + #endif /* DRV_BYTE_ORDER */ + } iis328dq_int1_ths_t; + + #define IIS328DQ_INT1_DURATION 0x33U + typedef struct + { + #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t d : 7; + uint8_t not_used_01 : 1; + #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 1; + uint8_t d : 7; + #endif /* DRV_BYTE_ORDER */ + } iis328dq_int1_duration_t; + + #define IIS328DQ_INT2_CFG 0x34U + typedef struct + { + #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t xlie : 1; + uint8_t xhie : 1; + uint8_t ylie : 1; + uint8_t yhie : 1; + uint8_t zlie : 1; + uint8_t zhie : 1; + uint8_t _6d : 1; + uint8_t aoi : 1; + #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t aoi : 1; + uint8_t _6d : 1; + uint8_t zhie : 1; + uint8_t zlie : 1; + uint8_t yhie : 1; + uint8_t ylie : 1; + uint8_t xhie : 1; + uint8_t xlie : 1; + #endif /* DRV_BYTE_ORDER */ + } iis328dq_int2_cfg_t; + + #define IIS328DQ_INT2_SRC 0x35U + typedef struct + { + #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t xl : 1; + uint8_t xh : 1; + uint8_t yl : 1; + uint8_t yh : 1; + uint8_t zl : 1; + uint8_t zh : 1; + uint8_t ia : 1; + uint8_t not_used_01 : 1; + #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 1; + uint8_t ia : 1; + uint8_t zh : 1; + uint8_t zl : 1; + uint8_t yh : 1; + uint8_t yl : 1; + uint8_t xh : 1; + uint8_t xl : 1; + #endif /* DRV_BYTE_ORDER */ + } iis328dq_int2_src_t; + + #define IIS328DQ_INT2_THS 0x36U + typedef struct + { + #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ths : 7; + uint8_t not_used_01 : 1; + #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 1; + uint8_t ths : 7; + #endif /* DRV_BYTE_ORDER */ + } iis328dq_int2_ths_t; + + #define IIS328DQ_INT2_DURATION 0x37U + typedef struct + { + #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t d : 7; + uint8_t not_used_01 : 1; + #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used_01 : 1; + uint8_t d : 7; + #endif /* DRV_BYTE_ORDER */ + } iis328dq_int2_duration_t; + + // Accelerometer IIS328DQ_Register_Union + typedef union + { + iis328dq_ctrl_reg1_t ctrl_reg1; + iis328dq_ctrl_reg2_t ctrl_reg2; + iis328dq_ctrl_reg3_t ctrl_reg3; + iis328dq_ctrl_reg4_t ctrl_reg4; + iis328dq_ctrl_reg5_t ctrl_reg5; + iis328dq_status_reg_t status_reg; + iis328dq_int1_cfg_t int1_cfg; + iis328dq_int1_src_t int1_src; + iis328dq_int1_ths_t int1_ths; + iis328dq_int1_duration_t int1_duration; + iis328dq_int2_cfg_t int2_cfg; + iis328dq_int2_src_t int2_src; + iis328dq_int2_ths_t int2_ths; + iis328dq_int2_duration_t int2_duration; + bitwise_t bitwise; + uint8_t byte; + } iis328dq_reg_t; + + // *************** Class Constructor *************** // + + Accelerometer(); + Accelerometer(TwoWire *_wire, BinarySemaphore *_wireLock, uint8_t _i2c_address = ACCELEROMETER_IIS328DQ_I2C_ADDR_DEFAULT); + + // ************ Library implemenmtation ************ // + + int32_t iis328dq_read_reg(uint8_t reg, uint8_t *data, uint16_t len); + int32_t iis328dq_write_reg(uint8_t reg, uint8_t *data, uint16_t len); + + void push_raw_data(int16_t *data_raw); + int16_t get_raw_mean(coordinate request); + + float_t iis328dq_from_fs2_to_mg(int16_t lsb); + float_t iis328dq_from_fs2_to_mg(coordinate request); + float_t iis328dq_from_fs4_to_mg(int16_t lsb); + float_t iis328dq_from_fs4_to_mg(coordinate request); + float_t iis328dq_from_fs8_to_mg(int16_t lsb); + float_t iis328dq_from_fs8_to_mg(coordinate request); + + float_t iis328dq_from_fsx_to_inc(int16_t lsb); + float_t iis328dq_from_fsx_to_inc(coordinate request); + + int32_t iis328dq_axis_x_data_set(uint8_t val); + int32_t iis328dq_axis_x_data_get(uint8_t *val); + + int32_t iis328dq_axis_y_data_set(uint8_t val); + int32_t iis328dq_axis_y_data_get(uint8_t *val); + + int32_t iis328dq_axis_z_data_set(uint8_t val); + int32_t iis328dq_axis_z_data_get(uint8_t *val); + + typedef enum + { + IIS328DQ_ODR_OFF = 0x00, + IIS328DQ_ODR_ON = 0x01, + IIS328DQ_ODR_Hz5 = 0x02, + IIS328DQ_ODR_1Hz = 0x03, + IIS328DQ_ODR_5Hz2 = 0x04, + IIS328DQ_ODR_5Hz = 0x05, + IIS328DQ_ODR_10Hz = 0x06, + IIS328DQ_ODR_50Hz = 0x01, + IIS328DQ_ODR_100Hz = 0x11, + IIS328DQ_ODR_400Hz = 0x21, + IIS328DQ_ODR_1kHz = 0x31, + } iis328dq_dr_t; + int32_t iis328dq_data_rate_set(iis328dq_dr_t val); + int32_t iis328dq_data_rate_get(iis328dq_dr_t *val); + + typedef enum + { + IIS328DQ_NORMAL_MODE = 0, + IIS328DQ_REF_MODE_ENABLE = 1, + } iis328dq_hpm_t; + int32_t iis328dq_reference_mode_set(iis328dq_hpm_t val); + int32_t iis328dq_reference_mode_get(iis328dq_hpm_t *val); + + typedef enum + { + IIS328DQ_2g = 0, + IIS328DQ_4g = 1, + IIS328DQ_8g = 3, + } iis328dq_fs_t; + int32_t iis328dq_full_scale_set(iis328dq_fs_t val); + int32_t iis328dq_full_scale_get(iis328dq_fs_t *val); + + int32_t iis328dq_block_data_update_set(uint8_t val); + int32_t iis328dq_block_data_update_get(uint8_t *val); + + int32_t iis328dq_status_reg_get(iis328dq_status_reg_t *val); + + int32_t iis328dq_flag_data_ready_get(uint8_t *val); + + int32_t iis328dq_acceleration_raw_get(int16_t *val); + + int32_t iis328dq_device_id_get(uint8_t *buff); + + int32_t iis328dq_boot_set(uint8_t val); + int32_t iis328dq_boot_get(uint8_t *val); + + typedef enum + { + IIS328DQ_ST_DISABLE = 0, + IIS328DQ_ST_POSITIVE = 1, + IIS328DQ_ST_NEGATIVE = 5, + } iis328dq_st_t; + int32_t iis328dq_self_test_set(iis328dq_st_t val); + int32_t iis328dq_self_test_get(iis328dq_st_t *val); + + typedef enum + { + IIS328DQ_LSB_AT_LOW_ADD = 0, + IIS328DQ_MSB_AT_LOW_ADD = 1, + } iis328dq_ble_t; + int32_t iis328dq_data_format_set(iis328dq_ble_t val); + int32_t iis328dq_data_format_get(iis328dq_ble_t *val); + + typedef enum + { + IIS328DQ_CUT_OFF_8Hz = 0, + IIS328DQ_CUT_OFF_16Hz = 1, + IIS328DQ_CUT_OFF_32Hz = 2, + IIS328DQ_CUT_OFF_64Hz = 3, + } iis328dq_hpcf_t; + int32_t iis328dq_hp_bandwidth_set(iis328dq_hpcf_t val); + int32_t iis328dq_hp_bandwidth_get(iis328dq_hpcf_t *val); + + typedef enum + { + IIS328DQ_HP_DISABLE = 0, + IIS328DQ_HP_ON_OUT = 4, + IIS328DQ_HP_ON_INT1 = 1, + IIS328DQ_HP_ON_INT2 = 2, + IIS328DQ_HP_ON_INT1_INT2 = 3, + IIS328DQ_HP_ON_INT1_INT2_OUT = 7, + IIS328DQ_HP_ON_INT2_OUT = 6, + IIS328DQ_HP_ON_INT1_OUT = 5, + } iis328dq_hpen_t; + int32_t iis328dq_hp_path_set(iis328dq_hpen_t val); + int32_t iis328dq_hp_path_get(iis328dq_hpen_t *val); + + int32_t iis328dq_hp_reset_get(void); + + int32_t iis328dq_hp_reference_value_set(uint8_t val); + int32_t iis328dq_hp_reference_value_get(uint8_t *val); + + typedef enum + { + IIS328DQ_SPI_4_WIRE = 0, + IIS328DQ_SPI_3_WIRE = 1, + } iis328dq_sim_t; + int32_t iis328dq_spi_mode_set(iis328dq_sim_t val); + int32_t iis328dq_spi_mode_get(iis328dq_sim_t *val); + + typedef enum + { + IIS328DQ_PAD1_INT1_SRC = 0, + IIS328DQ_PAD1_INT1_OR_INT2_SRC = 1, + IIS328DQ_PAD1_DRDY = 2, + IIS328DQ_PAD1_BOOT = 3, + } iis328dq_i1_cfg_t; + int32_t iis328dq_pin_int1_route_set(iis328dq_i1_cfg_t val); + int32_t iis328dq_pin_int1_route_get(iis328dq_i1_cfg_t *val); + + typedef enum + { + IIS328DQ_INT1_PULSED = 0, + IIS328DQ_INT1_LATCHED = 1, + } iis328dq_lir1_t; + int32_t iis328dq_int1_notification_set(iis328dq_lir1_t val); + int32_t iis328dq_int1_notification_get(iis328dq_lir1_t *val); + + typedef enum + { + IIS328DQ_PAD2_INT2_SRC = 0, + IIS328DQ_PAD2_INT1_OR_INT2_SRC = 1, + IIS328DQ_PAD2_DRDY = 2, + IIS328DQ_PAD2_BOOT = 3, + } iis328dq_i2_cfg_t; + int32_t iis328dq_pin_int2_route_set(iis328dq_i2_cfg_t val); + int32_t iis328dq_pin_int2_route_get(iis328dq_i2_cfg_t *val); + + typedef enum + { + IIS328DQ_INT2_PULSED = 0, + IIS328DQ_INT2_LATCHED = 1, + } iis328dq_lir2_t; + int32_t iis328dq_int2_notification_set(iis328dq_lir2_t val); + int32_t iis328dq_int2_notification_get(iis328dq_lir2_t *val); + + typedef enum + { + IIS328DQ_PUSH_PULL = 0, + IIS328DQ_OPEN_DRAIN = 1, + } iis328dq_pp_od_t; + int32_t iis328dq_pin_mode_set(iis328dq_pp_od_t val); + int32_t iis328dq_pin_mode_get(iis328dq_pp_od_t *val); + + typedef enum + { + IIS328DQ_ACTIVE_HIGH = 0, + IIS328DQ_ACTIVE_LOW = 1, + } iis328dq_ihl_t; + int32_t iis328dq_pin_polarity_set(iis328dq_ihl_t val); + int32_t iis328dq_pin_polarity_get(iis328dq_ihl_t *val); + + typedef struct + { + uint8_t int1_xlie : 1; + uint8_t int1_xhie : 1; + uint8_t int1_ylie : 1; + uint8_t int1_yhie : 1; + uint8_t int1_zlie : 1; + uint8_t int1_zhie : 1; + } int1_on_th_conf_t; + int32_t iis328dq_int1_on_threshold_conf_set(int1_on_th_conf_t val); + int32_t iis328dq_int1_on_threshold_conf_get(int1_on_th_conf_t *val); + + typedef enum + { + IIS328DQ_INT1_ON_THRESHOLD_OR = 0, + IIS328DQ_INT1_ON_THRESHOLD_AND = 1, + } iis328dq_int1_aoi_t; + int32_t iis328dq_int1_on_threshold_mode_set(iis328dq_int1_aoi_t val); + int32_t iis328dq_int1_on_threshold_mode_get(iis328dq_int1_aoi_t *val); + + int32_t iis328dq_int1_src_get(iis328dq_int1_src_t *val); + + int32_t iis328dq_int1_treshold_set(uint8_t val); + int32_t iis328dq_int1_treshold_get(uint8_t *val); + + int32_t iis328dq_int1_dur_set(uint8_t val); + int32_t iis328dq_int1_dur_get(uint8_t *val); + + typedef struct + { + uint8_t int2_xlie : 1; + uint8_t int2_xhie : 1; + uint8_t int2_ylie : 1; + uint8_t int2_yhie : 1; + uint8_t int2_zlie : 1; + uint8_t int2_zhie : 1; + } int2_on_th_conf_t; + int32_t iis328dq_int2_on_threshold_conf_set(int2_on_th_conf_t val); + int32_t iis328dq_int2_on_threshold_conf_get(int2_on_th_conf_t *val); + + typedef enum + { + IIS328DQ_INT2_ON_THRESHOLD_OR = 0, + IIS328DQ_INT2_ON_THRESHOLD_AND = 1, + } iis328dq_int2_aoi_t; + int32_t iis328dq_int2_on_threshold_mode_set(iis328dq_int2_aoi_t val); + int32_t iis328dq_int2_on_threshold_mode_get(iis328dq_int2_aoi_t *val); + + int32_t iis328dq_int2_src_get(iis328dq_int2_src_t *val); + + int32_t iis328dq_int2_treshold_set(uint8_t val); + int32_t iis328dq_int2_treshold_get(uint8_t *val); + + int32_t iis328dq_int2_dur_set(uint8_t val); + int32_t iis328dq_int2_dur_get(uint8_t *val); + + int32_t iis328dq_wkup_to_sleep_set(uint8_t val); + int32_t iis328dq_wkup_to_sleep_get(uint8_t *val); + + typedef enum + { + IIS328DQ_6D_INT1_DISABLE = 0, + IIS328DQ_6D_INT1_MOVEMENT = 1, + IIS328DQ_6D_INT1_POSITION = 3, + } iis328dq_int1_6d_t; + int32_t iis328dq_int1_6d_mode_set(iis328dq_int1_6d_t val); + int32_t iis328dq_int1_6d_mode_get(iis328dq_int1_6d_t *val); + + int32_t iis328dq_int1_6d_src_get(iis328dq_int1_src_t *val); + + int32_t iis328dq_int1_6d_treshold_set(uint8_t val); + int32_t iis328dq_int1_6d_treshold_get(uint8_t *val); + + typedef enum + { + IIS328DQ_6D_INT2_DISABLE = 0, + IIS328DQ_6D_INT2_MOVEMENT = 1, + IIS328DQ_6D_INT2_POSITION = 3, + } iis328dq_int2_6d_t; + int32_t iis328dq_int2_6d_mode_set(iis328dq_int2_6d_t val); + int32_t iis328dq_int2_6d_mode_get(iis328dq_int2_6d_t *val); + + int32_t iis328dq_int2_6d_src_get(iis328dq_int2_src_t *val); + + int32_t iis328dq_int2_6d_treshold_set(uint8_t val); + int32_t iis328dq_int2_6d_treshold_get(uint8_t *val); + + // ********** Private Variables Semaphore ********** // + +protected: +private: + TwoWire *_wire; + BinarySemaphore *_wireLock; + uint8_t _i2c_address; + int16_t _raw_scroll[3][ARR_REG_FILTER]; +}; + +typedef struct +{ + uint8_t config_valid; //!< Inizialization Byte Config + Accelerometer::iis328dq_dr_t module_power; //!< module updating frequency (enabled) + float offset_x; //!< offset_x to 0 + float offset_y; //!< offset_y to 0 + float offset_z; //!< offset_z to 0 +} accelerometer_t; + +#endif /* _ACCELEROMETR_H */ + + diff --git a/platformio/stima_v4/slave-leaf/include/drivers/eeprom.h b/platformio/stima_v4/slave-leaf/include/drivers/eeprom.h new file mode 100644 index 000000000..b446fc73b --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/drivers/eeprom.h @@ -0,0 +1,66 @@ +/** + ****************************************************************************** + * @file Eeprom.cpp + * @author Moreno Gasperini + * @brief eeprom AT24C64 header file + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + + +#ifndef _EEPROM_H +#define _EEPROM_H + +#include +#include "ticks.hpp" +#include "thread.hpp" +#include "semaphore.hpp" +#include "Wire.h" + +using namespace cpp_freertos; + +#define EEPROM_AT24C64_DEFAULT_ADDRESS (0x50) +#define EEPROMSIZE 8192 +#define EEPAGESIZE 32 +#define PAGEMASK (EEPROMSIZE-EEPAGESIZE) +#define WR_TIME_MS 5 +#define EEPROM_SEMAPHORE_MAX_WAITING_TIME_MS (1000) + +class EEprom { + +public: + EEprom(); + EEprom(TwoWire *wire, BinarySemaphore *wireLock, uint8_t i2c_address = EEPROM_AT24C64_DEFAULT_ADDRESS); + bool Write(uint16_t address, uint8_t *buffer, uint16_t length); + bool Write(uint16_t address, uint8_t value); + bool Read(uint16_t address, uint8_t *buffer, uint16_t length); + bool Read(uint16_t address, uint8_t *value); + +protected: +private: + TwoWire *_wire; + BinarySemaphore *_wireLock; + uint8_t _i2c_address; +}; + +#endif diff --git a/platformio/stima_v4/slave-leaf/include/drivers/flash.h b/platformio/stima_v4/slave-leaf/include/drivers/flash.h new file mode 100644 index 000000000..ecc170c04 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/drivers/flash.h @@ -0,0 +1,142 @@ +/** + ****************************************************************************** + * @file flash.h + * @author Moreno Gasperini + * @brief flash QSPI ETH452 AT256F161 header file + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#ifndef _FLASH_H +#define _FLASH_H + +/* Includes ------------------------------------------------------------------*/ +#include +#include "ticks.hpp" +#include "thread.hpp" +#include "stm32l4xx_hal.h" +#include "FreeRTOS.h" +#include "AT25SF161.h" + +using namespace cpp_freertos; + +#define FLASH_MEMORYMAP_BASE 0x90000000 + +// Security Check W->R Data +#define CHECK_FLASH_WRITE + +#define FLASH_FW_POSITION (0ul) +#define FLASH_FW_BACKUP (262144ul) // 000 - 256 KBytes For Program Flash +#define FLASH_FILE_POSITION (524288ul) // 256 - 512 KBytes For Program Flash (Backup) +#define FLASH_FREE_ACCESS (1048756ul) // 512 - 1024 KBytes For Extra File (... -> Free) +#define FLASH_BUFFER_SIZE (256) +#define FLASH_INFO_SIZE_LEN (256) +#define FLASH_FILE_SIZE_LEN (128) +#define FLASH_SIZE_ADDR(X) (X + FLASH_FILE_SIZE_LEN + 1) +#define FLASH_INFO_SIZE_U64 (8) + +class Flash { + + public: + + /* QSPI Error codes */ + typedef enum + { + QSPI_KO_INIT = -1, + QSPI_OK = HAL_OK, + QSPI_ERROR = HAL_ERROR, + QSPI_BUSY = HAL_BUSY, + QSPI_TIMEOUT = HAL_TIMEOUT, + QSPI_NOT_SUPPORTED, + QSPI_SUSPENDED, + QSPI_READY, + QSPI_RESET + } QSPI_StatusTypeDef; + + /* QSPI IT EventFlag */ + typedef union { + struct { + uint8_t flag_TE : 1; /*!< CallBack Register_Flag TE */ + uint8_t flag_TC : 1; /*!< CallBack Register_Flag TC */ + uint8_t flag_SM : 1; /*!< CallBack Register_Flag SM */ + uint8_t match_TE : 1; /*!< Enable Match Register_Flag TE */ + uint8_t match_TC : 1; /*!< Enable Match Register_Flag TC */ + uint8_t match_SM : 1; /*!< Enable Match Register_Flag SM */ + } flagBit; + uint8_t flag; + } QSPI_IT_EventFlag; + + /* QSPI Info */ + typedef struct + { + uint32_t FlashSize; /*!< Size of the flash */ + uint32_t EraseBlockSize; /*!< Size of blocks for the erase operation */ + uint32_t EraseBlockNumber; /*!< Number of blocks for the erase operation */ + uint32_t ProgPageSize; /*!< Size of pages for the program operation */ + uint32_t ProgPagesNumber; /*!< Number of pages for the program operation */ + uint32_t StatusRegister; /*!< Flash status register(byte 1 & byte 2) */ + __IO QSPI_StatusTypeDef State; /*!< QSPI state */ + } QSPI_Info; + + Flash(); + Flash(QSPI_HandleTypeDef *hqspi); + QSPI_StatusTypeDef BSP_QSPI_Init(void); + QSPI_StatusTypeDef BSP_QSPI_DeInit(void); + QSPI_StatusTypeDef BSP_QSPI_Read(uint8_t *pData, uint32_t ReadAddr, uint32_t Size); + QSPI_StatusTypeDef BSP_QSPI_Write(uint8_t *pData, uint32_t WriteAddr, uint32_t Size); + QSPI_StatusTypeDef BSP_QSPI_Erase_Block(uint32_t BlockAddress); + QSPI_StatusTypeDef BSP_QSPI_Erase_Sector(uint32_t Sector); + QSPI_StatusTypeDef BSP_QSPI_Erase_Chip(void); + QSPI_StatusTypeDef BSP_QSPI_ReadStatus(uint32_t *Reg); + QSPI_StatusTypeDef BSP_QSPI_WriteStatus(uint32_t Reg); + QSPI_StatusTypeDef BSP_QSPI_GetStatus(void); + QSPI_StatusTypeDef BSP_QSPI_GetInfo(QSPI_Info *pInfo); + QSPI_StatusTypeDef BSP_QSPI_EnableMemoryMappedMode(void); + QSPI_StatusTypeDef BSP_QSPI_DisableMemoryMappedMode(void); + uint8_t BSP_QSPI_SuspendErase(void); + uint8_t BSP_QSPI_ResumeErase(void); + +protected: +private: + + QSPI_StatusTypeDef BSP_QSPI_Receive(uint8_t *pData, uint32_t Timeout); + QSPI_StatusTypeDef BSP_QSPI_Transmit(uint8_t *pData, uint32_t Timeout); + QSPI_StatusTypeDef BSP_QSPI_AutoPolling(QSPI_CommandTypeDef *cmd, QSPI_AutoPollingTypeDef *cfg, uint32_t Timeout); + QSPI_StatusTypeDef BSP_QSPI_WaitingForEvent(uint32_t Timeout); + QSPI_StatusTypeDef QSPI_SetDeepPowerDown(void); + QSPI_StatusTypeDef QSPI_ExitDeepPowerDown(void); + QSPI_StatusTypeDef QSPI_ResetMemory(void); + QSPI_StatusTypeDef QSPI_DummyCyclesCfg(void); + QSPI_StatusTypeDef QSPI_WriteEnableVolat(void); + QSPI_StatusTypeDef QSPI_WriteEnable(void); + QSPI_StatusTypeDef QSPI_WriteDisable(void); + QSPI_StatusTypeDef QSPI_DisableContinuousMode(void); + QSPI_StatusTypeDef QSPI_AutoPollingMemReady(uint32_t Timeout); + + QSPI_HandleTypeDef *_hqspi; + inline static QSPI_Info _FlashInfo; + inline static QSPI_IT_EventFlag *_evtFlag; +}; + +#endif /* __ETH452_QSPI_H */ + diff --git a/platformio/stima_v4/slave-leaf/include/drivers/module_slave_hal.hpp b/platformio/stima_v4/slave-leaf/include/drivers/module_slave_hal.hpp new file mode 100644 index 000000000..50aa8880c --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/drivers/module_slave_hal.hpp @@ -0,0 +1,214 @@ +/** + ****************************************************************************** + * @file module_slave_hal.hpp + * @author Moreno Gasperini + * @brief Interface STM32 hardware_hal STIMAV4 Header config + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#include "config.h" +#include +#include + +// /* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef _MODULE_SLAVE_HAL_H +#define _MODULE_SLAVE_HAL_H + +// CPUID STM32 +#define UID_BASE_ADDRESS (0x1FFF7590UL) + +#if (ENABLE_I2C1 || ENABLE_I2C2) +#include +#endif + +#if (ENABLE_I2C2) +extern TwoWire Wire2; +#endif + +#if (ENABLE_QSPI) +extern QSPI_HandleTypeDef hqspi; +#define QSPI_NVIC_INT_PREMPT_PRIORITY 7 +#endif + +#if (ENABLE_CAN) +extern CAN_HandleTypeDef hcan1; +#define CAN_NVIC_INT_PREMPT_PRIORITY 8 +#endif + +// INIT HW PRIVATE BOARD/ISTANCE CFG + +// ****************************************************************************** + +// PIN NAMED STM32 ARDUINO GPIO_INIT + +// CAN BUS +#define PIN_CAN_RX1 PB12 +#define PIN_CAN_TX1 PB13 +#define PIN_CAN_EN PC12 +#define PIN_CAN_STB PB15 + +// POWER OUTPUT SENSOR OR DIGITAL OUT Protection & Controller +#define PIN_IN_ICTRL PA4 // Power Controller Output Consumation +#define PIN_OUT_DEN PC4 // Enable Output +#define PIN_OUT_DSEL0 PC5 // MUX Chanel 0 Power Consumation Controller +#define PIN_OUT_DSEL1 PC6 // MUX Chanel 1 Power Consumation Controller +#define PIN_OUT0 PC7 // Digital OUT 0 +#define PIN_OUT1 PC8 // Digital OUT 1 +#define PIN_OUT2 PC9 // Digital OUT 2 +#define PIN_OUT3 PC10 // Digital OUT 3 + +// INPUT DIGITAL +#define PIN_IN0 PB2 +#define PIN_IN1 PA8 +#define PIN_IN2 PB8 + +// INPUT BUTTON +#ifdef STIMAV4_SLAVE_HW_VER_01_01 +#define PIN_BTN PH0 // INPUT BUTTON (RESET PARAMETERS) +#endif + +// I2C +#define PIN_I2C2_EN PC11 +#define PIN_I2C2_SDA PB14 +#define PIN_I2C2_SCL PB10 + +#define PIN_I2C1_SDA PB7 +#define PIN_I2C1_SCL PB6 + +// PIN ENABLE SUPPLY +#define PIN_EN_SPLY PD2 +#define PIN_EN_5VS PB5 +#define PIN_EN_5VA PB9 +#define PIN_FAULT_SPLY PB4 + +// PIN SPI +#define PIN_SPI_SCK PA5 +#define PIN_SPI_MOSI PA12 +#define PIN_SPI_MISO PA11 + +#define PIN_SYS_JTMS PA13 + +// PIN UART1 (Monitor Debugger) +#define PIN_USART1_TX PA10 +#define PIN_USART1_RX PA9 + +// PIN UART2 +#define PIN_USART2_TX PA2 +#define PIN_USART2_RX PA15 +#define PIN_USART2_CTS PA0 +#define PIN_USART2_RTS PA1 + +// PIN ANALOG +#define PIN_ANALOG_01 PC0 +#define PIN_ANALOG_02 PC1 +#define PIN_ANALOG_03 PC2 +#define PIN_ANALOG_04 PC3 +#define PIN_ANALOG_09 PA4 // Power Controller Consumation + +#if (ENABLE_DIAG_PIN) +// DIAG PIN (LED + BUTTON COME TEST NUCLEO) +// Commentare per escludere la funzionalità +#define HFLT_PIN PIN_OUT1 // N.C. in Module_Power -> Output Signal Fault_Handler +#define LED1_PIN PIN_OUT2 // LED 1 Nucleo Simulator +#define LED2_PIN PIN_OUT3 // LED 2 Nucleo Simulator +#define USER_INP PIN_IN2 // BTN_I Nucleo Simulator +#endif + +// ****************************************************************************** + +// PIN NAMED STM32CUBE FOR GPIO_INIT + +#define DEN_Pin GPIO_PIN_4 +#define DEN_GPIO_Port GPIOC +#define DSEL0_Pin GPIO_PIN_5 +#define DSEL0_GPIO_Port GPIOC +#define IN0_Pin GPIO_PIN_2 +#define IN0_GPIO_Port GPIOB +#define STB_CAN_Pin GPIO_PIN_15 +#define STB_CAN_GPIO_Port GPIOB +#define DSEL1_Pin GPIO_PIN_6 +#define DSEL1_GPIO_Port GPIOC +#define PW0_Pin GPIO_PIN_7 +#define PW0_GPIO_Port GPIOC +#define PW1_Pin GPIO_PIN_8 +#define PW1_GPIO_Port GPIOC +#define PW2_Pin GPIO_PIN_9 +#define PW2_GPIO_Port GPIOC +#define IN1_Pin GPIO_PIN_8 +#define IN1_GPIO_Port GPIOA +#define PW3_Pin GPIO_PIN_10 +#define PW3_GPIO_Port GPIOC +#define I2C2_EN_Pin GPIO_PIN_11 +#define I2C2_EN_GPIO_Port GPIOC +#define EN_CAN_Pin GPIO_PIN_12 +#define EN_CAN_GPIO_Port GPIOC +#define EN_SPLY_Pin GPIO_PIN_2 +#define EN_SPLY_GPIO_Port GPIOD +#define FAULT_Pin GPIO_PIN_4 +#define FAULT_GPIO_Port GPIOB +#define EN_5VS_Pin GPIO_PIN_5 +#define EN_5VS_GPIO_Port GPIOB +#define IN2_Pin GPIO_PIN_8 +#define IN2_GPIO_Port GPIOB +#define EN_5VA_Pin GPIO_PIN_9 +#define EN_5VA_GPIO_Port GPIOB +#ifdef STIMAV4_SLAVE_HW_VER_01_01 +#define IN_BTN_Pin GPIO_PIN_0 +#define IN_BTN_GPIO_Port GPIOH +#endif + +// ****************************************************************************** + +/* Private Hardware_Handler istance initialization ---------------------------------------*/ + +#ifdef __cplusplus +extern "C" { +#endif + +// Basic Function Hardware +void SystemClock_Config(void); +void SetupSystemPeripheral(void); +void HAL_MspInit(void); +void MX_GPIO_Init(void); + +void STM32L4GetCPUID(uint8_t *ptrCpuId); +uint64_t StimaV4GetSerialNumber(void); + +#if (ENABLE_CAN) +void MX_CAN1_Init(void); +void HAL_CAN_MspInit(CAN_HandleTypeDef* hcan); +void HAL_CAN_MspDeInit(CAN_HandleTypeDef* hcan); +#endif + +#if (ENABLE_QSPI) +void MX_QUADSPI_Init(void); +void HAL_QSPI_MspInit(QSPI_HandleTypeDef* hqspi); +void HAL_QSPI_MspDeInit(QSPI_HandleTypeDef* hqspi); +#endif + +#ifdef __cplusplus +} +#endif + +#endif // __MODULE_SLAVE_HAL_H \ No newline at end of file diff --git a/platformio/stima_v4/slave-leaf/include/local_typedef.h b/platformio/stima_v4/slave-leaf/include/local_typedef.h new file mode 100644 index 000000000..ae2a55051 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/local_typedef.h @@ -0,0 +1,168 @@ +/**@file local_typedef.h */ + +/********************************************************************* +

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+authors: +Marco Baldinetti + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +. +**********************************************************************/ + +#ifndef _LOCAL_TYPEDEF_H +#define _LOCAL_TYPEDEF_H + +#include "local_typedef_config.h" +#include "typedef.h" + +#define MAX_ADC_CHANELS (1) + +/// @brief Power mode (Canard and general Node) +enum Power_Mode : uint8_t { + pwr_on, ///< Never (All ON, test o gestione locale) + pwr_nominal, ///< Every Second (Nominale base) + pwr_deep_save, ///< Deep mode (Very Low Power) + pwr_critical ///< Deep mode (Power Critical, Save data, Power->Off) +}; + +// Gestione modalità Power ( x Canard e Nodo in generale) +enum Adc_Mode : uint8_t { + mVolt, // Low Volt (0-3.3V) Input + Volt, // High Volt (0-14) Input + mA // mA (4-20) Input +}; + +/// @brief Sensor configuration +typedef struct +{ + float adc_offset; //!< adc offset + float adc_gain; //!< adc gain + float analog_min; //!< min sensor analog value for selected range + float analog_max; //!< max sensor analog value for selòected range + uint8_t is_active; //!< Chanel active, for Leaf is selected first active (One chanel) + Adc_Mode adc_type; //!< adc input type selection +} sensor_configuration_t; + +/// @brief System module configuration +typedef struct +{ + uint8_t module_main_version; //!< module main version + uint8_t module_minor_version; //!< module minor version + uint8_t configuration_version; //!< module configuration version + uint64_t serial_number; //!< module serial number + uint8_t module_type; //!< module type + uint8_t sensors_count; //!< number of configured sensors + sensor_configuration_t sensors[MAX_ADC_CHANELS]; //!< max input ADC + uint32_t sensor_acquisition_delay_ms; //!< sensor delay from next acquire +} configuration_t; + +/// @brief WatchDog Flag type +enum wdt_flag { + clear = 0, //!< Wdt Reset (From WDT Task Controller) + set = 1, //!< Set WDT (From Application TASK... All OK) + timer = 2 //!< Set Timered WDT (From Application long function WDT...) +}; + +/// @brief Task state Flag type +enum task_flag { + normal = 0, //!< Normal operation Task controller + sleepy = 1, //!< Task is in sleep mode or longer wait (Inform WDT controller) + suspended = 2 //!< Task is excluded from WDT Controller or Suspended complete +}; + +/// @brief Task Info structure +typedef struct +{ + wdt_flag watch_dog; //!< WatchDog of Task + int32_t watch_dog_ms; //!< WatchDog of Task Timer + uint16_t stack; //!< Stack Max Usage Monitor + task_flag state; //!< Long sleep Task + uint8_t running_pos; //!< !=0 (CREATE) Task Started (Generic state of Task) + uint8_t running_sub; //!< Optional SubState of Task +} task_t; + +/// @brief System module status +typedef struct +{ + ///< DateTime Operation + struct + { + uint32_t time_start_maintenance; //!< Starting time for maintenance mode + } datetime; + + /// Info Task && WDT + task_t tasks[TOTAL_INFO_TASK]; //!< Info flag structure + + ///< Module Flags + struct + { + bool is_cfg_loaded; //!< Is config loaded + bool is_maintenance; //!< Module is in maintenance mode + bool is_inibith_sleep; //!< Module sleep is inibithed + } flags; + + ///< Module error or alert + struct + { + #if(ENABLE_ACCELEROMETER) + bool is_accelerometer_error; //!< Accelerometer error + bool is_bubble_level_error; //!< Bubble software accelerometer error + #endif + bool is_adc_unit_error; //!< Adc unit error + bool is_adc_unit_overflow; //!< Adc unit overflow + } events; + +} system_status_t; + +/// @brief System message for queue +typedef struct +{ + uint8_t task_dest; //!< destination task for message + ///< Command struct + struct + { + uint8_t do_calib : 1; //!< Calibrate accelerometr + uint8_t do_inibith : 1; //!< Request inibith sleep (system_status) + uint8_t do_maint : 1; //!< Request maintenance (system_status) + uint8_t do_sleep : 1; //!< Optional param for difference level Sleep + uint8_t do_cmd : 1; //!< Using param to determine type of message command + uint8_t done_cmd : 1; //!< Using param to determine type of message response + } command; + uint32_t param; //!< 32 Bit for generic data or casting to pointer + +} system_message_t; + +/// @brief Report module +typedef struct +{ + rmapdata_t avg; //!< Measure for VWC input chanel + rmapdata_t quality; //!< Quality of measure for VWC input chanel +} report_t; + +/// @brief Backup && Upload Firmware TypeDef (BootLoader) +typedef struct +{ + bool request_upload; ///< Request an upload of firmware + bool backup_executed; ///< Firmware backup is executed + bool upload_executed; ///< An upload of firmware was executed + bool rollback_executed; ///< An rollback of firmware was executed + bool app_executed_ok; ///< Flag running APP (setted after new firmware, prevert a rollback operation) + bool app_forcing_start; ///< Force starting APP from Flash RUN APP Memory Position + uint8_t upload_error; ///< Error in upload firmware (ID of Error) + uint8_t tot_reset; ///< Number of module reset + uint8_t wdt_reset; ///< Number of WatchDog +} bootloader_t; + +#endif diff --git a/platformio/stima_v4/slave-leaf/include/local_typedef_config.h b/platformio/stima_v4/slave-leaf/include/local_typedef_config.h new file mode 100644 index 000000000..f90fc12a4 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/local_typedef_config.h @@ -0,0 +1,29 @@ +/**@file local_typedef_config.h */ + +/********************************************************************* +

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+authors: +Marco Baldinetti + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +. +**********************************************************************/ + +#ifndef _LOCAL_TYPEDEF_CONFIG_H +#define _LOCAL_TYPEDEF_CONFIG_H + +#include "config.h" + +#endif diff --git a/platformio/stima_v4/slave-leaf/include/main.h b/platformio/stima_v4/slave-leaf/include/main.h new file mode 100644 index 000000000..d1475b2a1 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/main.h @@ -0,0 +1,72 @@ +/**@file main.h */ + +/********************************************************************* +

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+authors: +Marco Baldinetti + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +. +**********************************************************************/ + +#ifndef _MAIN_H +#define _MAIN_H + +#define __STDC_LIMIT_MACROS + +#include "debug_config.h" +#include "canard_config.hpp" +#include "stima_utility.h" +#include "task_util.h" +#include "drivers/module_slave_hal.hpp" + +#include +#include "STM32LowPower.h" + +#include + +#include +#include "thread.hpp" +#include "ticks.hpp" +#include "semaphore.hpp" +#include "queue.hpp" + +#include "os_port.h" + +#if (ENABLE_ACCELEROMETER) +#include "tasks/accelerometer_task.h" +#endif + +#if (ENABLE_CAN) +#include "tasks/can_task.h" +#endif + +#if (MODULE_TYPE == STIMA_MODULE_TYPE_LEAF) +#include "tasks/leaf_sensor_task.h" +#endif + +#include "tasks/wdt_task.h" +#include "tasks/supervisor_task.h" +#include "tasks/elaborate_data_task.h" + +#include "debug_F.h" + +using namespace cpp_freertos; + +void init_pins(void); +void init_wire(void); +void init_rtc(bool init); + +#endif diff --git a/platformio/stima_v4/slave-leaf/include/os_port_config.h b/platformio/stima_v4/slave-leaf/include/os_port_config.h new file mode 100644 index 000000000..f38db36c8 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/os_port_config.h @@ -0,0 +1,34 @@ +/** + * @file os_port_config.h + * @brief RTOS port configuration file + * + * @section License + * + * SPDX-License-Identifier: GPL-2.0-or-later + * + * Copyright (C) 2010-2022 Oryx Embedded SARL. All rights reserved. + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * + * @author Oryx Embedded SARL (www.oryx-embedded.com) + * @version 2.1.4 + **/ + +#ifndef _OS_PORT_CONFIG_H +#define _OS_PORT_CONFIG_H + +#define USE_FREERTOS + +#endif diff --git a/platformio/stima_v4/slave-leaf/include/register_class.hpp b/platformio/stima_v4/slave-leaf/include/register_class.hpp new file mode 100644 index 000000000..2bf71a2aa --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/register_class.hpp @@ -0,0 +1,152 @@ +/** + ****************************************************************************** + * @file register_class.hpp + * @author Moreno Gasperini + * @brief Register class (Uavcan/Other) header file + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#include +#include +#include +#include +#include "drivers/eeprom.h" + +#ifndef _REGISTER_CLASS_H +#define _REGISTER_CLASS_H + +// *************************************************************************************** +// E2PROM STIMAV4 STM32 REGISTER ACCESS LOCATION AND SIZE CONFIG +// *************************************************************************************** + +// Start Address BASE UAVCAN/CYPAL Register (128 Bytes x Register) +#define MEM_UAVCAN_LEN EEPROMSIZE +#define MEM_UAVCAN_ADDR_START REGISTER_EEPROM_ADDRESS +#define MEM_UAVCAN_MAX_REG 60u +#define MEM_UAVCAN_LEN_SIZE_T_REG 1u +#define MEM_UAVCAN_LEN_INTEST_REG 60u +#define MEM_UAVCAN_LEN_VALUE_REG 66u +#define MEM_UAVCAN_POS_LEN_NAME 0u +#define MEM_UAVCAN_POS_STR_NAME MEM_UAVCAN_POS_LEN_NAME + MEM_UAVCAN_LEN_SIZE_T_REG +#define MEM_UAVCAN_POS_LEN_DATA MEM_UAVCAN_POS_STR_NAME + MEM_UAVCAN_LEN_INTEST_REG +#define MEM_UAVCAN_POS_VALUE_DATA MEM_UAVCAN_POS_LEN_DATA + MEM_UAVCAN_LEN_SIZE_T_REG +#define MEM_UAVCAN_LEN_NAME_REG (MEM_UAVCAN_LEN_SIZE_T_REG + MEM_UAVCAN_LEN_INTEST_REG) +#define MEM_UAVCAN_LEN_DATA_REG (MEM_UAVCAN_LEN_SIZE_T_REG + MEM_UAVCAN_LEN_VALUE_REG) +#define MEM_UAVCAN_LEN_REG (MEM_UAVCAN_LEN_NAME_REG + MEM_UAVCAN_LEN_DATA_REG) +#define MEM_UAVCAN_START_AREA_REG (MEM_UAVCAN_ADDR_START + MEM_UAVCAN_MAX_REG) +#define MEM_UAVCAN_REG_UNDEF 0xFF +#define MEM_UAVCAN_GET_ADDR_FLAG() (MEM_UAVCAN_ADDR_START) +#define MEM_UAVCAN_GET_ADDR_FLAG_REG(X) (MEM_UAVCAN_ADDR_START + X) +#define MEM_UAVCAN_GET_ADDR_NAME_LEN(X) (MEM_UAVCAN_START_AREA_REG + (MEM_UAVCAN_LEN_REG * X)) +#define MEM_UAVCAN_GET_ADDR_NAME(X) (MEM_UAVCAN_START_AREA_REG + (MEM_UAVCAN_LEN_REG * X) + MEM_UAVCAN_LEN_SIZE_T_REG) +#define MEM_UAVCAN_GET_ADDR_VALUE_LEN(X) (MEM_UAVCAN_START_AREA_REG + (MEM_UAVCAN_LEN_REG * X) + MEM_UAVCAN_LEN_NAME_REG) +#define MEM_UAVCAN_GET_ADDR_VALUE(X) (MEM_UAVCAN_START_AREA_REG + (MEM_UAVCAN_LEN_REG * X) + MEM_UAVCAN_LEN_NAME_REG + MEM_UAVCAN_LEN_SIZE_T_REG) +#define MEM_UAVCAN_GET_ADDR_BASE_REG(X) MEM_UAVCAN_GET_ADDR_NAME_LEN(X) +// Start Address Eeprom Application Free usage +#define MEM_UAVCAN_ADDR_END (MEM_UAVCAN_START_AREA_REG + (MEM_UAVCAN_LEN_REG * MEM_UAVCAN_MAX_REG)) +#define MEM_UAVCAN_USED_TOTAL (MEM_UAVCAN_ADDR_END - MEM_UAVCAN_ADDR_START) +// Error Size Memory limit +#if (MEM_UAVCAN_ADDR_END > MEM_UAVCAN_LEN) + #error Uavcan Register limit is more than MAX SIZE (MEM_UAVCAN_LEN). Please check Register configuration +#endif + +// Register base name for metadata +#define REGISTER_UAVCAN_MTU "uavcan.can.mtu" +#define REGISTER_UAVCAN_BITRATE "uavcan.can.bitrate" +#define REGISTER_UAVCAN_NODE_ID "uavcan.node.id" +#define REGISTER_UAVCAN_UNIQUE_ID "uavcan.node.unique_id" +#define REGISTER_UAVCAN_NODE_DESCR "uavcan.node.description" +#define REGISTER_UAVCAN_DATA_PUBLISH "uavcan.pub.rmap.publish.id" +#define REGISTER_UAVCAN_DATA_SERVICE "uavcan.srv.rmap.service.id" +#define REGISTER_METADATA_LEVEL_L1 "rmap.metadata.Level.L1" +#define REGISTER_METADATA_LEVEL_L2 "rmap.metadata.Level.L2" +#define REGISTER_METADATA_LEVEL_TYPE1 "rmap.metadata.Level.LevelType1" +#define REGISTER_METADATA_LEVEL_TYPE2 "rmap.metadata.Level.LevelType2" +#define REGISTER_METADATA_TIME_P1 "rmap.metadata.Timerange.P1" +#define REGISTER_METADATA_TIME_PIND "rmap.metadata.Timerange.Pindicator" +#define REGISTER_DATA_PUBLISH "rmap.publish" +#define REGISTER_DATA_SERVICE "rmap.service" +#define REGISTER_RMAP_MASTER_ID "rmap.master.id" + +// Class EEProm - Register Uavcan +class EERegister { + +public: + // Constructor + EERegister(); + EERegister(TwoWire *wire, BinarySemaphore *wireLock, uint8_t i2c_address = EEPROM_AT24C64_DEFAULT_ADDRESS); + + // Inizializza lo spazio RAM/ROM/FLASH/SD dei registri, ai valori di default + // N.B.! Azzera tutti registri e quelli non inizializzati devono essere impostati + // nel relativo modulo di utilizzo + void setup(void); + + /// Reads the specified register from the persistent storage into `inout_value`. + /// If the register does not exist or it cannot be automatically converted to the type of the provided argument, + /// the value will be stored in the persistent storage using registerWrite(), overriding existing value. + /// The default will not be initialized if the argument is empty. + void read(const char* const register_name, uavcan_register_Value_1_0* const inout_value); + + /// Store the given register value into the persistent storage. + void write(const char* const register_name, const uavcan_register_Value_1_0* const value); + + /// This function is mostly intended for implementing the standard RPC-service uavcan.register.List. + /// It returns the name of the register at the specified index (where the ordering is undefined but guaranteed + /// to be short-term stable), or empty name if the index is out of bounds. + uavcan_register_Name_1_0 getNameByIndex(const uint16_t index); + + /// Erase all registers such that the defaults are used at the next launch. + void doFactoryReset(void); + + /// Copy one value to the other if their types and dimensionality are the same or automatic conversion is possible. + /// If the destination is empty, it is simply replaced with the source (assignment always succeeds). + /// The return value is true if the assignment has been performed, false if it is not possible + /// (in the latter case the destination is NOT modified). + bool assign(uavcan_register_Value_1_0* const dst, const uavcan_register_Value_1_0* const src); + +protected: +private: + + void _memory_write_block(uint16_t address, uint8_t *data, uint8_t len); + void _memory_read_block(uint16_t address, uint8_t *data, uint8_t len); + void _memory_write_byte(uint16_t address, uint8_t data); + void _memory_read_byte(uint16_t address, uint8_t *data); + void _eeprom_register_factory(void); + void _eeprom_register_clear(uint8_t reg_numb); + size_t _eeprom_register_get_fast(uint8_t reg_numb, uint8_t *reg_name, uint8_t *reg_value); + size_t _eeprom_register_get_len_intest_fast(uint8_t reg_numb); + void _eeprom_register_get_intest_fast(uint8_t reg_numb, uint8_t *reg_name, uint8_t name_len); + bool _eeprom_register_get_name_from_index(uint8_t reg_numb, uint8_t *reg_name); + size_t _eeprom_register_get_from_name(uint8_t const *reg_name, uint8_t *reg_numb, uint8_t *data); + uint8_t _eeprom_register_get_index_from_name(uint8_t *reg_name); + void _eeprom_register_set(uint8_t reg_numb, uint8_t *reg_name, uint8_t *data, size_t len_data); + void _eeprom_register_get_next_id(uint8_t *current_register); + uint8_t _eeprom_register_add(uint8_t *reg_name, uint8_t *data, size_t data_len); + + // Private Memory E2prom Object + EEprom _Eprom; + +}; + +#endif \ No newline at end of file diff --git a/platformio/stima_v4/slave-leaf/include/sensors_config.h b/platformio/stima_v4/slave-leaf/include/sensors_config.h new file mode 100644 index 000000000..1401ba15d --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/sensors_config.h @@ -0,0 +1,406 @@ +/**@file sensors_config.h */ + +/********************************************************************* +

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+authors: +Marco Baldinetti +Paolo patruno + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +. +**********************************************************************/ + +#ifndef _SENSORS_CONFIG_H +#define _SENSORS_CONFIG_H + +/*! +\def USE_REDUNDANT_SENSOR +\brief Enable if you want use redundant sensor. Redundant sensor MUST be equal to main sensor. +*/ +#define USE_REDUNDANT_SENSOR (false) + +/*! +\def USE_JSON +\brief Enable if you want use json library for json response (getJson function in SensorDriver). +*/ +#define USE_JSON (false) + +#define USE_D_TEMPLATE (false) + +/*! +\def USE_THR +\brief Enable if you want use one module for TH and RAIN. Disable if you want use one module for TH and one for RAIN. +*/ +#define USE_THR (false) + +/*! +\def USE_SENSOR_ADT +\brief Enable if you want use ADT7420 sensor. +*/ +#define USE_SENSOR_ADT (false) + +/*! +\def USE_SENSOR_HIH +\brief Enable if you want use HIH6100 sensor. +*/ +#define USE_SENSOR_HIH (false) + +/*! +\def USE_SENSOR_HYT +\brief Enable if you want use HYT939, HYT271 or HYT221 sensor. +*/ +#define USE_SENSOR_HYT (false) + +/*! +\def USE_SENSOR_SHT +\brief Enable if you want use SHT35 sensor. +*/ +#define USE_SENSOR_SHT (false) + +/*! +\def USE_SENSOR_B28 +\brief Enable if you want use BMP280 sensor.s +*/ +#define USE_SENSOR_B28 (false) + +/*! +\def USE_SENSOR_DEP +\brief Enable if you want use DigitEco Power sensor. +*/ +#define USE_SENSOR_DEP (false) + +/*! +\def USE_SENSOR_DES +\brief Enable if you want use DigitEco Wind Speed sensor. +*/ +#define USE_SENSOR_DES (false) + +/*! +\def USE_SENSOR_DED +\brief Enable if you want use DigitEco Wind Direction sensor. +*/ +#define USE_SENSOR_DED (false) + +/*! +\def USE_SENSOR_GWS +\brief Enable if you want use Gill Windsonic sensor. +*/ +#define USE_SENSOR_GWS (false) + +/*! +\def USE_SENSOR_DSR +\brief Enable if you want use DigitEco Global Solar Radiation sensor. +*/ +#define USE_SENSOR_DSR (false) + +/*! +\def USE_SENSOR_VSR +\brief Enable if you want use 0-5V High Resolution Global Solar Radiation sensor. +*/ +#define USE_SENSOR_VSR (false) + +/*! +\def USE_SENSOR_DSA +\brief Enable if you want average Global Solar Radiation sensor. +*/ +#define USE_SENSOR_DSA (false) + +/*! +\def USE_SENSOR_DWA +\brief Enable if you want vectorial average Wind Speed and Direction over 10'. +*/ +#define USE_SENSOR_DWA (false) + +/*! +\def USE_SENSOR_DWB +\brief Enable if you want vectorial average Wind Speed and Direction over report time. +*/ +#define USE_SENSOR_DWB (false) + +/*! +\def USE_SENSOR_DWC +\brief Enable if you want gust Wind Speed and Direction over report time. +*/ +#define USE_SENSOR_DWC (false) + +/*! +\def USE_SENSOR_DWD +\brief Enable if you want average Wind Speed over report time. +*/ +#define USE_SENSOR_DWD (false) + +/*! +\def USE_SENSOR_DWE +\brief Enable if you want class Wind Speed over report time. +*/ +#define USE_SENSOR_DWE (false) + +/*! +\def USE_SENSOR_DWF +\brief Enable if you want class Wind Speed over report time. +*/ +#define USE_SENSOR_DWF (false) + +/*! +\def USE_SENSOR_OA2 +\brief Enable if you want use OPC PM1, PM2.5, PM10 continuous average value. +*/ +#define USE_SENSOR_OA2 (false) +#define USE_SENSOR_OA3 (false) + +/*! +\def USE_SENSOR_OB2 +\brief Enable if you want use OPC PM1, PM2.5, PM10 continuous standard deviation value. +*/ +#define USE_SENSOR_OB2 (false) +#define USE_SENSOR_OB3 (false) + +/*! +\def USE_SENSOR_OC3 +\brief Enable if you want use OPC BINS continuous average value. +*/ +#define USE_SENSOR_OCX_ODX_FULL_BIN (false) +#define USE_SENSOR_OC2 (false) +#define USE_SENSOR_OC3 (false) + +/*! +\def USE_SENSOR_OD3 +\brief Enable if you want use OPC BINS continuous standard deviation value. +*/ +#define USE_SENSOR_OD2 (false) +#define USE_SENSOR_OD3 (false) + +/*! +\def USE_SENSOR_OE3 +\brief Enable if you want use temperature, humidity or pressure average value. +*/ +#define USE_SENSOR_OE3 (false) + +#define USE_SENSOR_GAS (false) +/*! +\def USE_SENSOR_CO2 +\brief Enable if you want use CO2 gas sensor. +*/ +#define USE_SENSOR_CO2 (false) + +/*! +\def USE_SENSOR_NO2 +\brief Enable if you want use NO2 gas sensor. +*/ +#define USE_SENSOR_NO2 (false) + +/*! +\def USE_SENSOR_O3 +\brief Enable if you want use O3 gas sensor. +*/ +#define USE_SENSOR_O3 (false) + +/*! +\def USE_SENSOR_CO +\brief Enable if you want use CO gas sensor. +*/ +#define USE_SENSOR_CO (false) + +/*! +\def USE_SENSOR_EX +\brief Enable if you want use EX gas sensor. +*/ +#define USE_SENSOR_EX (false) + +/*! +\def USE_SENSOR_LWT +\brief Enable if you want use leaf wetness time continuous value. +*/ +#define USE_SENSOR_LWT (true) + +/*! +\def USE_SENSOR_HI7 +\brief Enable if you want use SI7021 sensor. +*/ +#define USE_SENSOR_HI7 (false) + +/*! +\def USE_SENSOR_BMP +\brief Enable if you want use Bmp085 sensor. +*/ +#define USE_SENSOR_BMP (false) + +/*! +\def USE_SENSOR_DW1 +\brief Enable if you want use DW1 sensor. +*/ +#define USE_SENSOR_DW1 (false) + +/*! +\def USE_SENSOR_TBS +\brief Enable if you want use Tipping bucket rain gauge sensor. +*/ +#define USE_SENSOR_TBS (false) + +/*! +\def USE_SENSOR_TBR +\brief Enable if you want use Tipping bucket rain gauge sensor. +*/ +#define USE_SENSOR_TBR (false) + +/*! +\def USE_SENSOR_STH +\brief Enable if you want use Temperature and humidity oneshot sensor. +*/ +#define USE_SENSOR_STH (false) + +/*! +\def USE_SENSOR_ITH +\brief Enable if you want use Temperature and humidity continuous istantaneous sensor. +*/ +#define USE_SENSOR_ITH (false) + +/*! +\def USE_SENSOR_NTH +\brief Enable if you want use Temperature and humidity continuous minium sensor. +*/ +#define USE_SENSOR_NTH (false) + +/*! +\def USE_SENSOR_MTH +\brief Enable if you want use Temperature and humidity continuous average sensor. +*/ +#define USE_SENSOR_MTH (false) + +/*! +\def USE_SENSOR_XTH +\brief Enable if you want use Temperature and humidity continuous maximum sensor. +*/ +#define USE_SENSOR_XTH (false) + +/*! +\def USE_SENSOR_SSD +\brief Enable if you want use SSD011 oneshot sensor. +*/ +#define USE_SENSOR_SSD (false) + +/*! +\def USE_SENSOR_ISD +\brief Enable if you want use SSD011 report istantaneous sensor. +*/ +#define USE_SENSOR_ISD (false) + +/*! +\def USE_SENSOR_NSD +\brief Enable if you want use SSD011 report minium sensor. +*/ +#define USE_SENSOR_NSD (false) + +/*! +\def USE_SENSOR_MSD +\brief Enable if you want use SSD011 report average sensor. +*/ +#define USE_SENSOR_MSD (false) + +/*! +\def USE_SENSOR_XSD +\brief Enable if you want use SSD011 report maximum sensor. +*/ +#define USE_SENSOR_XSD (false) + +/*! +\def USE_SENSOR_SMI +\brief Enable if you want use MICS4514 oneshot sensor. +*/ +#define USE_SENSOR_SMI (false) + +/*! +\def USE_SENSOR_IMI +\brief Enable if you want use MICS4514 report istantaneous sensor. +*/ +#define USE_SENSOR_IMI (false) + +/*! +\def USE_SENSOR_NMI +\brief Enable if you want use MICS4514 report minium sensor. +*/ +#define USE_SENSOR_NMI (false) + +/*! +\def USE_SENSOR_MMI +\brief Enable if you want use MICS4514 report average sensor. +*/ +#define USE_SENSOR_MMI (false) + +/*! +\def USE_SENSOR_XMI +\brief Enable if you want use MICS4514 report maximum sensor. +*/ +#define USE_SENSOR_XMI (false) + +/*! +\def USE_SENSOR_RF24 +\brief Enable if you want use Radio RF24 sensor. +*/ +#define USE_SENSOR_RF24 (false) + +/*! +\def RAIN_FOR_TIP +\brief How much mm of rain for one tip of tipping bucket rain gauge. +*/ +#define RAIN_FOR_TIP (1) + +// OPC +#define VALUES_TO_READ_FROM_SENSOR_COUNT (2) +#define JSONS_TO_READ_FROM_SENSOR_COUNT (2) + +// OPC +// #define VALUES_TO_READ_FROM_SENSOR_COUNT (24) +// #define JSONS_TO_READ_FROM_SENSOR_COUNT (1) + +#define USE_TH_SENSORS (USE_SENSOR_ADT + USE_SENSOR_HIH + USE_SENSOR_HYT + USE_SENSOR_SHT + USE_SENSOR_STH + USE_SENSOR_ITH + USE_SENSOR_MTH + USE_SENSOR_NTH + USE_SENSOR_XTH) +#define USE_RAIN_SENSORS (USE_SENSOR_TBR + USE_SENSOR_TBS) +#define USE_RADIAITION_SENSORS (USE_SENSOR_DSR + USE_SENSOR_VSR + USE_SENSOR_DSA) +#define USE_WIND_SENSORS (USE_SENSOR_DWA + USE_SENSOR_DWB + USE_SENSOR_DWC + USE_SENSOR_DWD + USE_SENSOR_DWE + USE_SENSOR_DWF) +#define USE_POWER_MPPT_SENSORS (USE_SENSOR_DEP) +#define USE_LEAF_SENSORS (USE_SENSOR_LWT) + +#if (USE_TH_SENSORS && (USE_RAIN_SENSORS == 0)) +#define USE_MODULE_TH (true) +#elif ((USE_TH_SENSORS == 0) && USE_RAIN_SENSORS) +#define USE_MODULE_RAIN (true) +#elif (USE_TH_SENSORS && USE_RAIN_SENSORS && USE_THR) +#define USE_MODULE_THR (true) +#elif (USE_RADIAITION_SENSORS) +#define USE_MODULE_SOLAR_RADIATION (true) +#elif (USE_WIND_SENSORS) +#define USE_MODULE_WIND (true) +#elif (USE_LEAF_SENSORS) +#define USE_MODULE_LEAF (true) +#elif (USE_POWER_MPPT_SENSORS) +#define USE_MODULE_POWER_MPPT (true) +#endif + +#define SENSORS_COUNT_MAX (USE_SENSOR_HYT + USE_SENSOR_SHT + USE_REDUNDANT_SENSOR) + +/*! +\def SENSORS_MAX +\brief Max number of sensor. +*/ +#define SENSORS_MAX (SENSORS_COUNT_MAX) + +/*! +\def SENSORS_UNIQUE_MAX +\brief Max number of unique sensor. +unique sensors are sensors that can have more driver but only one i2c address and only one setup and prepare +*/ +#define SENSORS_UNIQUE_MAX (1) + +#endif diff --git a/platformio/stima_v4/slave-leaf/include/tasks/accelerometer_task.h b/platformio/stima_v4/slave-leaf/include/tasks/accelerometer_task.h new file mode 100644 index 000000000..0fd7ee9a1 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/tasks/accelerometer_task.h @@ -0,0 +1,125 @@ +/** + ****************************************************************************** + * @file accelerometer_task.h + * @author Moreno Gasperini + * @brief accelerometer cpp_Freertos header file + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#ifndef _ACCELEROMETER_TASK_H +#define _ACCELEROMETER_TASK_H + +#include "debug_config.h" +#include "local_typedef.h" +#include "str.h" +#include "stima_utility.h" + +#if (ENABLE_ACCELEROMETER) + +#include +#include "thread.hpp" +#include "ticks.hpp" +#include "semaphore.hpp" +#include "queue.hpp" +#include "drivers/module_slave_hal.hpp" +#include "drivers/accelerometer.h" + +// Register EEprom +#include "register_class.hpp" + +#include "debug_F.h" + +using namespace cpp_freertos; + +// Main TASK Switch Delay +#define ACCELEROMETER_TASK_WAIT_DELAY_MS (20) +#define ACCELEROMETER_TASK_SLEEP_DELAY_MS (850) + +#define BUBBLE_ANGLE_ERROR (0.03) +#define BUBBLE_ANGLE_MIRROR (0.80) + +/// @brief struct local elaborate data parameter +typedef struct { + configuration_t *configuration; //!< system configuration pointer struct + system_status_t *system_status; //!< system status pointer struct + TwoWire *wire; //!< Local Wire access for sensor accelerometer + cpp_freertos::BinarySemaphore *systemStatusLock; //!< Semaphore to system status access + cpp_freertos::BinarySemaphore *registerAccessLock; //!< Semaphore to register Cyphal access + cpp_freertos::BinarySemaphore *wireLock; //!< Semaphore to Wire access for sensor accelerometer + cpp_freertos::Queue *systemMessageQueue; //!< Queue for system message + EERegister *clRegister; //!< Object Register C++ access +} AccelerometerParam_t; + +/// @brief ACCELEROMETER TASK cpp_freertos class +class AccelerometerTask : public cpp_freertos::Thread { + + /// @brief Enum for state switch of running method + typedef enum + { + ACCELEROMETER_STATE_CREATE, + ACCELEROMETER_STATE_INIT, + ACCELEROMETER_STATE_CHECK_HARDWARE, + ACCELEROMETER_STATE_LOAD_CONFIGURATION, + ACCELEROMETER_STATE_SETUP_MODULE, + ACCELEROMETER_STATE_CHECK_OPERATION, + ACCELEROMETER_STATE_WAIT_RESUME, + ACCELEROMETER_STATE_HARDWARE_FAIL + } AccelerometerState_t; + +public: + AccelerometerTask(const char *taskName, uint16_t stackSize, uint8_t priority, AccelerometerParam_t AccelerometerParam); + +protected: + virtual void Run(); + +private: + + #if (ENABLE_STACK_USAGE) + void TaskMonitorStack(); + #endif + void TaskWatchDog(uint32_t millis_standby); + void TaskState(uint8_t state_position, uint8_t state_subposition, task_flag state_operation); + + void printConfiguration(void); + void loadConfiguration(void); + void saveConfiguration(bool is_default); + void calibrate(bool is_default, bool save_register); + bool checkModule(void); + void setupModule(void); + bool readModule(void); + void powerDownModule(void); + + AccelerometerState_t state; + AccelerometerParam_t param; + Accelerometer accelerometer; + accelerometer_t accelerometer_configuration; + + // Value data + float value_x; + float value_y; + float value_z; +}; + +#endif +#endif diff --git a/platformio/stima_v4/slave-leaf/include/tasks/can_task.h b/platformio/stima_v4/slave-leaf/include/tasks/can_task.h new file mode 100644 index 000000000..47b91c1b2 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/tasks/can_task.h @@ -0,0 +1,192 @@ +/** + ****************************************************************************** + * @file can_task.h + * @author Moreno Gasperini + * @brief Uavcan over CanBus cpp_Freertos header file + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#ifndef _CAN_TASK_H +#define _CAN_TASK_H + +#include "debug_config.h" +#include "local_typedef.h" +#include "str.h" +#include "stima_utility.h" + +#include +#include "thread.hpp" +#include "ticks.hpp" +#include "semaphore.hpp" +#include "queue.hpp" +#include "drivers/module_slave_hal.hpp" + +#include +#include + +// Configurazione modulo, definizioni ed utility generiche +#include "canard_config.hpp" + +// Register EEprom +#include "register_class.hpp" + +// Flash Access +#include "drivers/flash.h" + +// Classe Canard +#include "canard_class_leaf.hpp" +// Libcanard +#include +#include "bxcan.h" +// Namespace UAVCAN +#include +#include +#include +#include +#include +#include +#include +#include +#include +// Namespace RMAP +#include +#include +// Standard Library +#include +#include +#include +#include +#include + +#include "debug_F.h" + +using namespace cpp_freertos; + +// Main TASK Switch Delay +#define CAN_TASK_WAIT_DELAY_MS (20) +#define CAN_TASK_WAIT_MAXSPEED_DELAY_MS (1) +#define CAN_TASK_SLEEP_DELAY_MS (850) + +// Task waiting Semaphore Driver access +#define CAN_SEMAPHORE_MAX_WAITING_TIME_MS (1000) +#define FLASH_SEMAPHORE_MAX_WAITING_TIME_MS (3000) + +// Bit flag return maintenance mode on state command +#define CAN_FLAG_IS_MAINTENANCE_MODE (0x08) +#define CAN_FLAG_MASK_MAINTENANCE_MODE (0x07) + +// Debug Check Enable Function +// #define LOG_RX_PACKET +// #define LED_ON_SYNCRO_TIME + +/// @brief Mode Power HW CanBus Controller state +enum CAN_ModePower { + CAN_INIT, //!< CAN is in init or configuration mode + CAN_NORMAL, //!< CAN is in normal state (TX and RX Ready) + CAN_LISTEN_ONLY, //!< CAN in only listen mode (turn off TX board) + CAN_SLEEP //!< Power CAN is OFF +}; + +/// @brief struct local elaborate data parameter +typedef struct +{ + configuration_t *configuration; //!< system configuration pointer struct + system_status_t *system_status; //!< system status pointer struct + bootloader_t *boot_request; //!< Boot struct pointer + cpp_freertos::BinarySemaphore *configurationLock; //!< Semaphore to configuration access + cpp_freertos::BinarySemaphore *systemStatusLock; //!< Semaphore to system status access + cpp_freertos::BinarySemaphore *registerAccessLock; //!< Semaphore to register Cyphal access + cpp_freertos::BinarySemaphore *canLock; //!< Semaphore to CAN Bus access + cpp_freertos::BinarySemaphore *qspiLock; //!< Semaphore to QSPI Memory flash access + cpp_freertos::BinarySemaphore *rtcLock; //!< Semaphore to RTC Access + cpp_freertos::Queue *systemMessageQueue; //!< Queue for system message + cpp_freertos::Queue *requestDataQueue; //!< Queue to request data + cpp_freertos::Queue *reportDataQueue; //!< Queue to report data + Flash *flash; //!< Object Flash C++ access + EEprom *eeprom; //!< Object EEprom C++ access + EERegister *clRegister; //!< Object Register C++ access +} CanParam_t; + +/// @brief CAN TASK cpp_freertos class +class CanTask : public cpp_freertos::Thread { + + /// @brief Enum for state switch of running method + typedef enum { + CAN_STATE_CREATE, + CAN_STATE_INIT, + CAN_STATE_SETUP, + CAN_STATE_CHECK + } State_t; + +public: + CanTask(const char *taskName, uint16_t stackSize, uint8_t priority, CanParam_t canParam); + +protected: + virtual void Run(); + +private: + + #if (ENABLE_STACK_USAGE) + void TaskMonitorStack(); + #endif + void TaskWatchDog(uint32_t millis_standby); + void TaskState(uint8_t state_position, uint8_t state_subposition, task_flag state_operation); + + static void HW_CAN_Power(CAN_ModePower ModeCan); + static void getUniqueID(uint8_t out[uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_], uint64_t serNumb); + static CanardPortID getModeAccessID(uint8_t modeAccessID, const char* const port_name, const char* const type_name); + static bool putFlashFile(const char* const file_name, const bool is_firmware, const bool rewrite, void* buf, size_t count); + static bool getFlashFwInfoFile(uint8_t *module_type, uint8_t *version, uint8_t *revision, uint64_t *len); + static void prepareSensorsDataValue(uint8_t const sensore, const report_t *report, rmap_module_Leaf_1_0 *rmap_data); + static void prepareSensorsDataValue(uint8_t const sensore, const report_t *report, rmap_service_module_Leaf_Response_1_0 *rmap_data); + static void publish_rmap_data(canardClass &clsCanard, CanParam_t *param); + static void processMessagePlugAndPlayNodeIDAllocation(canardClass &clsCanard, const uavcan_pnp_NodeIDAllocationData_1_0* const msg); + static uavcan_node_ExecuteCommand_Response_1_1 processRequestExecuteCommand(canardClass &clsCanard, const uavcan_node_ExecuteCommand_Request_1_1* req, uint8_t remote_node); + static rmap_service_module_Leaf_Response_1_0 processRequestGetModuleData(canardClass &clsCanard, rmap_service_module_Leaf_Request_1_0* req, CanParam_t *param); + static uavcan_register_Access_Response_1_0 processRequestRegisterAccess(const uavcan_register_Access_Request_1_0* req); + static uavcan_node_GetInfo_Response_1_0 processRequestNodeGetInfo(); + static void processRequestUpdateRTC(canardClass &clsCanard, const uavcan_pnp_NodeIDAllocationData_1_0* const msg); + static void processReceivedTransfer(canardClass &clsCanard, const CanardRxTransfer* const transfer, void *param); + + State_t state; + CanParam_t param; + + // Acces static memeber parameter of class + inline static bootloader_t *boot_state; + inline static EEprom *localEeprom; + inline static cpp_freertos::Queue *localSystemMessageQueue; + inline static uint16_t last_req_rpt_time = (REPORTS_TIME_S); + inline static uint16_t last_req_obs_time = (OBSERVATIONS_TIME_S); + inline static CAN_ModePower canPower; + inline static STM32RTC& rtc = STM32RTC::getInstance(); + // Register access && Flash (Firmware and data log archive) + inline static EERegister *localRegister; + inline static cpp_freertos::BinarySemaphore *localQspiLock; + inline static cpp_freertos::BinarySemaphore *localRegisterAccessLock; + inline static Flash *localFlash; + inline static uint64_t canFlashPtr = 0; + inline static uint16_t canFlashBlock = 0; +}; + +#endif diff --git a/platformio/stima_v4/slave-leaf/include/tasks/elaborate_data_task.h b/platformio/stima_v4/slave-leaf/include/tasks/elaborate_data_task.h new file mode 100644 index 000000000..935cfad21 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/tasks/elaborate_data_task.h @@ -0,0 +1,129 @@ +/** + ****************************************************************************** + * @file elaborate_data_task.h + * @author Marco Baldinetti + * @author Moreno Gasperini + * @brief Elaborate data sensor to CAN header file + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#ifndef _ELABORATE_DATA_TASK_H +#define _ELABORATE_DATA_TASK_H + +#include "debug_config.h" +#include "local_typedef.h" +#include "stima_utility.h" +#include "str.h" + +#include +#include "thread.hpp" +#include "ticks.hpp" +#include "semaphore.hpp" +#include "queue.hpp" +#include "drivers/module_slave_hal.hpp" + +#include "debug_F.h" + +using namespace cpp_freertos; + +// Main TASK Switch Delay +#define ELABORATE_TASK_WAIT_DELAY_MS (20) +#define ELABORATE_TASK_SLEEP_DELAY_MS (850) + +/// @brief struct array for module data +typedef struct { + rmapdata_t values[SAMPLES_COUNT_MAX]; //!< samples buffer data values + uint16_t count; //!< samples counter + rmapdata_t *read_ptr; //!< reader pointer + rmapdata_t *write_ptr; //!< writer pointer +} sample_t; + +/// @brief struct array for maintenance +typedef struct { + bool values[SAMPLES_COUNT_MAX]; //!< samples buffer maintenance values + uint16_t count; //!< samples counter + bool *read_ptr; //!< reader pointer + bool *write_ptr; //!< writer pointer +} maintenance_t; + +/// @brief struct local elaborate data parameter +typedef struct { + configuration_t *configuration; //!< system configuration pointer struct + system_status_t *system_status; //!< system status pointer struct + cpp_freertos::BinarySemaphore *configurationLock; //!< semaphore access to configuration + cpp_freertos::BinarySemaphore *systemStatusLock; //!< semaphore access to system status + cpp_freertos::Queue *systemMessageQueue; //!< Queue for system message + cpp_freertos::Queue *elaborateDataQueue; //!< Queue for elaborate data + cpp_freertos::Queue *requestDataQueue; //!< Queue for request data + cpp_freertos::Queue *reportDataQueue; //!< Queue for report data +} ElaborateDataParam_t; + +/// @brief ELABORATE DATA TASK cpp_freertos class +class ElaborateDataTask : public cpp_freertos::Thread { + + /// @brief Enum for state switch of running method + typedef enum { + ELABORATE_DATA_CREATE, + ELABORATE_DATA_INIT, + ELABORATE_DATA_RUN, + ELABORATE_DATA_SUSPEND + } State_t; + +public: + ElaborateDataTask(const char *taskName, uint16_t stackSize, uint8_t priority, ElaborateDataParam_t elaborateDataParam); + +protected: + virtual void Run(); + + +private: + + #if (ENABLE_STACK_USAGE) + void TaskMonitorStack(); + #endif + void TaskWatchDog(uint32_t millis_standby); + void TaskState(uint8_t state_position, uint8_t state_subposition, task_flag state_operation); + + void make_report(bool is_init = true, uint16_t report_time_s = REPORTS_TIME_S, uint8_t observation_time_s = OBSERVATIONS_TIME_S); + uint8_t checkLeaf(rmapdata_t main_leaf); + + State_t state; + ElaborateDataParam_t param; + + sample_t leaf_samples; + + maintenance_t maintenance_samples; + report_t report; +}; + +template value_v bufferRead(buffer_g *buffer, length_v length); +template value_v bufferReadBack(buffer_g *buffer, length_v length); +template void bufferWrite(buffer_g *buffer, value_v value); +template void bufferPtrReset(buffer_g *buffer); +template void bufferPtrResetBack(buffer_g *buffer, length_v length); +template void incrementBuffer(buffer_g *buffer, length_v length); +template void bufferReset(buffer_g *buffer, length_v length); +templatevoid addValue(buffer_g *buffer, length_v length, value_v value); + +#endif diff --git a/platformio/stima_v4/slave-leaf/include/tasks/leaf_sensor_task.h b/platformio/stima_v4/slave-leaf/include/tasks/leaf_sensor_task.h new file mode 100644 index 000000000..9a7029290 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/tasks/leaf_sensor_task.h @@ -0,0 +1,147 @@ +/** + ****************************************************************************** + * @file leaf_sensor_task.cpp + * @author Marco Baldinetti + * @author Moreno Gasperini + * @brief Module sensor header file + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#ifndef _LEAF_SENSOR_TASK_H +#define _LEAF_SENSOR_TASK_H + +#include "config.h" +#include "debug_config.h" +#include "local_typedef.h" +#include "stima_utility.h" +#include "str.h" + +#if (MODULE_TYPE == STIMA_MODULE_TYPE_LEAF) + +#define LEAF_TASK_LOW_POWER_ENABLED (true) +#define LEAF_TASK_SWITCH_POWER_ENABLED (false) +#define LEAF_TASK_WAIT_DELAY_MS (50) +#define LEAF_TASK_GENERIC_RETRY_DELAY_MS (5000) +#define LEAF_TASK_GENERIC_RETRY (3) + +// POWER SENSOR SWITCHING OR FIXED MODE... +#if (!LEAF_TASK_SWITCH_POWER_ENABLED) +// Timing to wakeUP only internal comparator ADC +#define LEAF_TASK_POWER_ON_WAIT_DELAY_MS (5) +#define LEAF_FIXED_POWER_CHANEL_0 (true) +#define LEAF_FIXED_POWER_CHANEL_1 (false) +#define LEAF_FIXED_POWER_CHANEL_2 (false) +#define LEAF_FIXED_POWER_CHANEL_3 (false) +#else +// Timing to wakeUP exernal sensor if automatic powered +#define LEAF_TASK_POWER_ON_WAIT_DELAY_MS (100) +#endif + +/* Analog read resolution */ +#define LL_ADC_RESOLUTION LL_ADC_RESOLUTION_12B +#define ADC_RANGE ADC_MAX +#define SAMPLES_REPETED_ADC 16 + +#include "stm32yyxx_ll_adc.h" + +#include +#include "thread.hpp" +#include "ticks.hpp" +#include "semaphore.hpp" +#include "queue.hpp" +#include "drivers/module_slave_hal.hpp" + +#if (ENABLE_I2C1 || ENABLE_I2C2) +#include +#endif + +#include "debug_F.h" + +using namespace cpp_freertos; + +/// @brief struct local elaborate data parameter +typedef struct { + configuration_t *configuration; //!< system configuration pointer struct + system_status_t *system_status; //!< system status pointer struct + cpp_freertos::BinarySemaphore *configurationLock; //!< Semaphore to configuration access + cpp_freertos::BinarySemaphore *systemStatusLock; //!< Semaphore to system status access + cpp_freertos::Queue *systemMessageQueue; //!< Queue for system message + cpp_freertos::Queue *elaborateDataQueue; //!< Queue for elaborate data +} LeafSensorParam_t; + +/// @brief SENSOR TASK cpp_freertos class +class LeafSensorTask : public cpp_freertos::Thread { + + /// @brief Enum for state switch of running method + typedef enum + { + SENSOR_STATE_CREATE, + SENSOR_STATE_WAIT_CFG, + SENSOR_STATE_INIT, + SENSOR_STATE_SET, + SENSOR_STATE_EVALUATE, + SENSOR_STATE_READ, + SENSOR_STATE_END + } State_t; + +public: + LeafSensorTask(const char *taskName, uint16_t stackSize, uint8_t priority, LeafSensorParam_t LeafSensorParam); + +protected: + virtual void Run(); + +private: + #if (ENABLE_STACK_USAGE) + void TaskMonitorStack(); + #endif + void TaskWatchDog(uint32_t millis_standby); + void TaskState(uint8_t state_position, uint8_t state_subposition, task_flag state_operation); + + void powerOn(uint8_t chanel_out); + void powerOff(); + + void resetADCData(uint8_t chanel_out); + uint8_t addADCData(uint8_t chanel_out); + float getADCData(uint8_t chanel_out, uint8_t *quality_data); + + int32_t getVrefTemp(void); + + float getAdcCalibratedValue(float adc_value, float offset, float gain); + float getAdcAnalogValue(float adc_value, Adc_Mode adc_type); + float getLeaf(float adc_value, float adc_voltage_min, float adc_voltage_max, bool *adc_overflow); + + // Global flag powered + bool is_power_on; + + // Value of chanel ADC + uint8_t adc_in_count[MAX_ADC_CHANELS]; + uint8_t adc_err_count[MAX_ADC_CHANELS]; + uint64_t adc_in[MAX_ADC_CHANELS]; + + State_t state; + LeafSensorParam_t param; +}; + +#endif +#endif \ No newline at end of file diff --git a/platformio/stima_v4/slave-leaf/include/tasks/supervisor_task.h b/platformio/stima_v4/slave-leaf/include/tasks/supervisor_task.h new file mode 100644 index 000000000..fb9de7614 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/tasks/supervisor_task.h @@ -0,0 +1,109 @@ +/** + ****************************************************************************** + * @file supervisor_task.h + * @author Marco Baldinetti + * @author Moreno Gasperini + * @brief Supervisor module header file + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#ifndef _SUPERVISOR_TASK_H +#define _SUPERVISOR_TASK_H + +#include "debug_config.h" +#include "local_typedef.h" +#include "stima_utility.h" +#include "str.h" + +#include +#include "thread.hpp" +#include "ticks.hpp" +#include "semaphore.hpp" +#include "queue.hpp" +#include "drivers/module_slave_hal.hpp" + +#include + +// Register EEprom +#include "register_class.hpp" + +// Main TASK Switch Delay +#define SUPERVISOR_TASK_WAIT_DELAY_MS (20) +#define SUPERVISOR_TASK_SLEEP_DELAY_MS (850) + +#define SUPERVISOR_TASK_GENERIC_RETRY_DELAY_MS (5000) +#define SUPERVISOR_TASK_GENERIC_RETRY (3) + +#define SUPERVISOR_AUTO_END_MAINTENANCE_SEC (3600ul) + +#include "debug_F.h" + +/// @brief struct local elaborate data parameter +typedef struct { + configuration_t *configuration; //!< system configuration pointer struct + system_status_t *system_status; //!< system status pointer struct + cpp_freertos::BinarySemaphore *configurationLock; //!< Semaphore to configuration access + cpp_freertos::BinarySemaphore *systemStatusLock; //!< Semaphore to system status access + cpp_freertos::BinarySemaphore *registerAccessLock; //!< Semaphore to register Cyphal access + cpp_freertos::Queue *systemMessageQueue; //!< Queue for system message + EERegister *clRegister; //!< Object Register C++ access + bool is_initialization_request; //!< External require initilizaztion register E2 +} SupervisorParam_t; + +/// @brief SUPERVISOR TASK cpp_freertos class +class SupervisorTask : public cpp_freertos::Thread { + + /// @brief Enum for state switch of running method + typedef enum + { + SUPERVISOR_STATE_CREATE, + SUPERVISOR_STATE_INIT, + SUPERVISOR_STATE_CHECK_OPERATION, + SUPERVISOR_STATE_END + } SupervisorState_t; + +public: + SupervisorTask(const char *taskName, uint16_t stackSize, uint8_t priority, SupervisorParam_t SupervisorParam); + +protected: + virtual void Run(); + +private: + #if (ENABLE_STACK_USAGE) + void TaskMonitorStack(); + #endif + void TaskWatchDog(uint32_t millis_standby); + void TaskState(uint8_t state_position, uint8_t state_subposition, task_flag state_operation); + + void printConfiguration(); + void loadConfiguration(); + void saveConfiguration(bool is_default); + + STM32RTC& rtc = STM32RTC::getInstance(); + + SupervisorState_t state; + SupervisorParam_t param; +}; + +#endif diff --git a/platformio/stima_v4/slave-leaf/include/tasks/wdt_task.h b/platformio/stima_v4/slave-leaf/include/tasks/wdt_task.h new file mode 100644 index 000000000..687bb8129 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/include/tasks/wdt_task.h @@ -0,0 +1,82 @@ +/** + ****************************************************************************** + * @file wdt_task.h + * @author Moreno Gasperini + * @brief wdt_task header file (Wdt && Logging Task for Module Slave) + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#ifndef _WDT_TASK_H +#define _WDT_TASK_H + +#include "debug_config.h" +#include "local_typedef.h" +#include "str.h" + +#include +#include "thread.hpp" +#include "ticks.hpp" +#include "semaphore.hpp" +#include "queue.hpp" +#include "drivers/module_slave_hal.hpp" +#include "drivers/eeprom.h" + +#include +#include + +#include "debug_F.h" + +// Main TASK Switch Delay +#define WDT_TASK_WAIT_DELAY_MS (WDT_CONTROLLER_MS) + +using namespace cpp_freertos; + +/// @brief struct for local param access +typedef struct { + system_status_t *system_status; //!< system configuration pointer struct + bootloader_t *boot_request; //!< bootloader struct access pointer + cpp_freertos::BinarySemaphore *systemStatusLock; //!< semaphore access to system status + cpp_freertos::BinarySemaphore *wireLock; //!< semaphore access to wire I2C + cpp_freertos::BinarySemaphore *rtcLock; //!< semaphore access to RTC + EEprom *eeprom; //!< Pointer to EEprom C++ object +} WdtParam_t; + +/// @brief WATCH DOG TASK cpp_freertos class +class WdtTask : public cpp_freertos::Thread { + +public: + WdtTask(const char *taskName, uint16_t stackSize, uint8_t priority, WdtParam_t wdtParam); + +protected: + virtual void Run(); + +private: + + STM32RTC& rtc = STM32RTC::getInstance(); + + WdtParam_t param; + +}; + +#endif diff --git a/platformio/stima_v4/slave-leaf/lib/README b/platformio/stima_v4/slave-leaf/lib/README new file mode 100644 index 000000000..6debab1e8 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/platformio/stima_v4/slave-leaf/platformio.ini b/platformio/stima_v4/slave-leaf/platformio.ini new file mode 100644 index 000000000..2853f0799 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/platformio.ini @@ -0,0 +1,54 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[platformio] +boards_dir= ../../custom_boards + +[env] +platform = ststm32@17.3.0 +framework = arduino +lib_extra_dirs = + ../libraries +lib_ldf_mode = deep +lib_archive = no +monitor_speed = 115200 +board_build.variants_dir = ../../custom_variants +platform_packages = + framework-arduinoststm32 @ https://github.com/r-map/Arduino_Core_STM32_stima_v4.git#2.7.1_stima_v4 + +[env:stimav4_slave] +board = stimav4_slave +board_build.ldscript = $PROJECT_DIR/user_custom/bootloader.ld +; debug_build_flags = -O0 -ggdb3 -g3 +build_flags = + -DSTIMAV4_SLAVE_HW_VER_01_01 + -DINIT_PARAMETER=false + -DFIXED_CONFIGURATION=false + -DUSE_DEBUG=true + -fexceptions + -fno-common + -Wall + -Os + -g3 + -mcpu=cortex-m4 + -mthumb + -mfpu=fpv4-sp-d16 + -mfloat-abi=softfp + -ffunction-sections + -fdata-sections + -Wl,--gc-sections + -D__error_t_defined + -DHAL_CAN_MODULE_ENABLED + -DUSE_HAL_DRIVER=1 + -DBXCAN_MAX_IFACE_INDEX=0 + -w + -I ../cyphal/include + -I include + -std=c++11 \ No newline at end of file diff --git a/platformio/stima_v4/slave-leaf/src/canard_class_leaf.cpp b/platformio/stima_v4/slave-leaf/src/canard_class_leaf.cpp new file mode 100644 index 000000000..bf6a464cf --- /dev/null +++ b/platformio/stima_v4/slave-leaf/src/canard_class_leaf.cpp @@ -0,0 +1,1112 @@ +/** + ****************************************************************************** + * @file canard_class_leaf.cpp + * @author Moreno Gasperini + * @brief Uavcan Canard Class LibCanard, bxCan, o1Heap + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#include "drivers/module_slave_hal.hpp" +#include "canard_class_leaf.hpp" +#include "canard_config.hpp" +#include "bxcan.h" + +// Callback per ISR Routine HAL STM32 +/// @brief ISR di sistema CAN1_RX0_IRQHandler HAL_CAN_IRQHandler STM32 +canardClass::CanardRxQueue *__CAN1_RX0_IRQHandler_PTR; +extern "C" void CAN1_RX0_IRQHandler(void) { + #if (ENABLE_CAN) + HAL_CAN_IRQHandler(&hcan1); + #endif +} + +// ***************** ISR READ RX CAN_BUS, BUFFER RX SETUP ISR, CALLBACK ***************** +// Gestita come coda FIFO (In sostituzione interrupt bxCAN non funzionante correttamente) +extern "C" void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) +{ + uint8_t testElement = __CAN1_RX0_IRQHandler_PTR->wr_ptr + 1; + if(testElement >= CAN_RX_QUEUE_CAPACITY) testElement = 0; + // Leggo il messaggio già pronto per libreria CANARD (Frame) e Inserisco in Buffer RX + if (bxCANPop(IFACE_CAN_IDX, + &__CAN1_RX0_IRQHandler_PTR->msg[testElement].frame.extended_can_id, + &__CAN1_RX0_IRQHandler_PTR->msg[testElement].frame.payload_size, + __CAN1_RX0_IRQHandler_PTR->msg[testElement].buf)) { + if(testElement != __CAN1_RX0_IRQHandler_PTR->rd_ptr) { + // Non posso registrare il dato (MAX_QUEUE) se (testElement == _canard_rx_queue.rd_ptr) + // raggiunto MAX Buffer. E' più importante non perdere il primo FIFO payload + // Quindi non aggiungo il dato ma leggo per svuotare il Buffer FIFO + // altrimenti rientro sempre in Interrupt RX e mando in stallo la CPU senza RX... + // READ DATA BUFFER MSG -> + // Get payload from Buffer (possibilie inizializzazione statica fissa) + // Il Buffer non cambia indirizzo quindi basterebbe un'init statico di frame[x].payload + __CAN1_RX0_IRQHandler_PTR->msg[testElement].frame.payload = + __CAN1_RX0_IRQHandler_PTR->msg[testElement].buf; + // Push data in queue (Next_WR, Data in testElement + 1 Element from RX) + __CAN1_RX0_IRQHandler_PTR->wr_ptr = testElement; + } + } +} + +// ******************************************************************************** +// Contructor Init Class Canard, O1Heap Mem Ptr ISR +// ******************************************************************************** + +/// @brief Contruttore della classe Canard +canardClass::canardClass() { + + // CallBack RX Messaggi + _attach_rx_callback_PTR = NULL; + _attach_rx_callback = false; + _attach_param_PTR = NULL; + + // Sottoscrizioni + _rxSubscriptionIdx = 0; + + // Reset master ID all'avvio + _master_id = UINT16_MAX; + + // Init Timing + _lastMicros = micros(); + _currMicros = _lastMicros; + _syncMicros = _lastMicros; + + // Init O1Heap arena base access ram Canard + _heap = o1heapInit(_heap_arena, sizeof(_heap_arena)); + LOCAL_ASSERT(NULL != _heap); + + // Setup CanardIstance, SET Memory function Allocate, Free and set Canard Mem reference + _canard = canardInit(_memAllocate, _memFree); + _canard.user_reference = this; + _canard.node_id = CANARD_NODE_ID_UNSET; + + // Interrupt vector CB to Class PTR + memset(&_canard_rx_queue, 0, sizeof(_canard_rx_queue)); + __CAN1_RX0_IRQHandler_PTR = &_canard_rx_queue; + + // Init memory (0) per le sotto strutture dati che necessitano un INIT_MEM_RESET + memset(&master, 0, sizeof(master)); + memset(&next_transfer_id, 0, sizeof(next_transfer_id)); + memset(&flag, 0, sizeof(flag)); + + // Inizializzazioni di sicurezza + master.file.download_end(); + + // Inizializzazioni locali da module_config.h + port_id.publisher_module_leaf = UINT16_MAX; + port_id.service_module_leaf = UINT16_MAX; + + publisher_enabled.module_leaf = DEFAULT_PUBLISH_MODULE_DATA; + publisher_enabled.port_list = DEFAULT_PUBLISH_PORT_LIST; + + // Configura il trasporto dal registro standard uavcan. Capacità e CANARD_MTU_MAX + // I parametri sono fissi e configurabili dal file di configurazione + for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) { + _canard_tx_queues[ifidx] = canardTxInit(CAN_TX_QUEUE_CAPACITY, CANARD_MTU_MAX); + } +} + +// *************************************************************** +// Funzioni Timing sincronizzazione e gestione Canard +// *************************************************************** + +/// @brief Get MonotonicTime Interno realTime +/// @return Microsecondi correnti realTime di Canard +CanardMicrosecond canardClass::getMicros() { + // Start Syncro o real_time + uint32_t ts = micros(); + if(ts > _lastMicros) { + // Standard micosecond successivo + _currMicros += (ts - _lastMicros); + } else if(ts < _lastMicros) { + // Overflow registro microsecond + _currMicros += (0xFFFFFFFFul - _lastMicros + 1 + ts); + } + // Backup current register micro + _lastMicros = ts; + return _currMicros; +} + +/// @brief Get MonotonicTime Interno con richiesta del tipo di sincronizzazione Overloading di RealTime +/// @param syncro_type Tipo di funzione richiesta (sincronizzato, inizializza sincronizzazione) +/// @return Microsecondi correnti parametrizzato dalla richiesta +CanardMicrosecond canardClass::getMicros(GetMonotonicTime_Type syncro_type) { + // Time sincronizzato o da sincrtonizzare + if(syncro_type == start_syncronization) _syncMicros = getMicros(); + return _syncMicros; +} + +/// @brief Ritorna i secondi dall'avvio Canard per UpTime in (in formato 64 BIT necessario per UAVCAN) +/// Non permette il reset n ei 70 minuti circa previsti per l'overflow della funzione uS a 32 Bit +/// @return Secondi dall'avvio di Canard (per funzioni Heartbeat o altri scopi) +uint32_t canardClass::getUpTimeSecond(void) { + return (uint32_t)(_currMicros / MEGA); +} + +// *************************************************************************** +// Gestione Coda messaggi in trasmissione (ciclo di svuotamento messaggi) +// *************************************************************************** + +/// @brief Send messaggio Canard con daeadline del tempo sincronizzato +/// @param tx_deadline_usec microsecondi di validità del messaggio +/// @param metadata metadata del messaggio +/// @param payload_size dimensione del messaggio +/// @param payload messaggio da trasmettere +void canardClass::send(const CanardMicrosecond tx_deadline_usec, + const CanardTransferMetadata* const metadata, + const size_t payload_size, + const void* const payload) { + for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) { + (void)canardTxPush(&_canard_tx_queues[ifidx], + &_canard, + _syncMicros + tx_deadline_usec, + metadata, + payload_size, + payload); + } +} + +/// @brief Send risposta al messaggio Canard con daeadline del tempo sincronizzato +/// @param tx_deadline_usec microsecondi di validità del messaggio +/// @param request_metadata metadata del messaggio +/// @param payload_size dimensione del messaggio +/// @param payload messaggio da trasmettere +void canardClass::sendResponse(const CanardMicrosecond tx_deadline_usec, + const CanardTransferMetadata* const request_metadata, + const size_t payload_size, + const void* const payload) { + CanardTransferMetadata meta = *request_metadata; + meta.transfer_kind = CanardTransferKindResponse; + send(tx_deadline_usec, &meta, payload_size, payload); +} + +/// @brief Test coda presenza dati in trasmissione di Canard +/// @return true se ci sono dati da trasmettere +bool canardClass::transmitQueueDataPresent(void) { + // Transmit pending frames from the prioritized TX queues managed by libcanard. + for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) + { + CanardTxQueue* const que = &_canard_tx_queues[ifidx]; + const CanardTxQueueItem* tqi = canardTxPeek(que); // Find the highest-priority frame. + if(tqi != NULL) return true; + } + return false; +} + +/// @brief Trasmette la coda con timeStamp sincronizzato. Per inviare con real_time va aggiornata +/// la sincronizzazione prima della chiamata al relativo metodo di getMonotonicMicrosecond() +void canardClass::transmitQueue(void) { + // Transmit pending frames from the prioritized TX queues managed by libcanard. + for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) + { + CanardTxQueue* const que = &_canard_tx_queues[ifidx]; + const CanardTxQueueItem* tqi = canardTxPeek(que); // Find the highest-priority frame. + while (tqi != NULL) + { + // Delay Microsecond di sicurezza in Send (Migliora sicurezza RX Pacchetti) + // Da utilizzare con CPU poco performanti in RX o con controllo Polling gestito Canard + // N.B. Funziona perfettamente con i Nodi ma utilizzando Yakut è comunque necessario un USB/CAN + // che utilizzi un'interrupt o molto performanete, altrimenti vengono persi pacchetti in TX + // e questo comporta uina perdità dei messaggi RX in ricezione dal nodo (master/slave corrente) + #if (CAN_DELAY_US_SEND > 0) + delayMicroseconds(CAN_DELAY_US_SEND); + #endif + // Attempt transmission only if the frame is not yet timed out while waiting in the TX queue. + // Otherwise just drop it and move on to the next one. + if ((tqi->tx_deadline_usec == 0) || (tqi->tx_deadline_usec > _syncMicros)) { + // Non-blocking write attempt. + if (bxCANPush(0, + _syncMicros, + tqi->tx_deadline_usec, + tqi->frame.extended_can_id, + tqi->frame.payload_size, + tqi->frame.payload)) { + // Push CAN data + _canard.memory_free(&_canard, canardTxPop(que, tqi)); + tqi = canardTxPeek(que); + } else { + // Empty Queue + break; + } + } else { + // loop continuo per mancato aggiornamento monotonic_time su TIME_OUT + // grandi quantità di dati trasmesse e raggiunto il TIMEOUT Subscription... + // Remove frame per blocco in timeout BUG trasmission security !!! + _canard.memory_free(&_canard, canardTxPop(que, tqi)); + // Test prossimo pacchetto + tqi = canardTxPeek(que); + } + } + } +} + +// *************************************************************** +// Gestione Buffer memorizzazione RX BxCAN per Canard metodo FIFO +// *************************************************************** + +/// @brief Azzera il buffer di ricezione dati collegato a BxCAN e ISR Interrupt +void canardClass::receiveQueueEmpty(void) { + _canard_rx_queue.wr_ptr=_canard_rx_queue.rd_ptr; +} + +/// @brief Ritorna true se ci sono dati utili da gestire dal buffer di RX per BxCAN +/// @return true se il buffer non è vuoto. +bool canardClass::receiveQueueDataPresent(void) { + return _canard_rx_queue.wr_ptr!=_canard_rx_queue.rd_ptr; +} + +/// @brief Ritorna l'elemento corrente del buffer di BxCAN, pronto ad essere gestito con Canard +/// @return Indice dell'elemento ricevuto +uint8_t canardClass::receiveQueueElement(void) { + if(_canard_rx_queue.wr_ptr>=_canard_rx_queue.rd_ptr) { + return _canard_rx_queue.wr_ptr-_canard_rx_queue.rd_ptr; + } else { + return _canard_rx_queue.wr_ptr+(CAN_RX_QUEUE_CAPACITY-_canard_rx_queue.rd_ptr); + } +} + +/// @brief Ritorna il prossimo elemento del buffer +/// @param currElement elemento corrente gestito +/// @return prossimo elemento della coda FIFO +uint8_t canardClass::receiveQueueNextElement(uint8_t currElement) { + if(currElement + 1 < CAN_RX_QUEUE_CAPACITY) return currElement + 1; + return 0; +} + +/// @brief Gestione metodo ricezione coda messaggi dal buffer FIFO preparato di BxCAN +/// Il buffer gestito nella ISR CAN_Rx viene passato alla libreria Canard e in automatico +/// è gestita la richiamata di callBack per la funzione esterna di gestione su Rx Messaggi conformi +void canardClass::receiveQueue(void) { + // Leggo l'elemento disponibile in coda BUFFER RX FiFo CanardFrame + Buffer + uint8_t getElement = receiveQueueNextElement(_canard_rx_queue.rd_ptr); + _canard_rx_queue.rd_ptr = getElement; + // Passaggio CanardFrame Buffered alla RxAccept CANARD + // DeadLine a partire dal realTime assoluto + const CanardMicrosecond timestamp_usec = getMicros(); + CanardRxTransfer transfer; + const int8_t canard_result = canardRxAccept(&_canard, timestamp_usec, &_canard_rx_queue.msg[getElement].frame, IFACE_CAN_IDX, &transfer, NULL); + if (canard_result > 0) { + _attach_rx_callback_PTR(*this, &transfer, _attach_param_PTR); + _canard.memory_free(&_canard, (void*) transfer.payload); + } else if ((canard_result == 0) || (canard_result == -CANARD_ERROR_OUT_OF_MEMORY)) { + (void) 0; // The frame did not complete a transfer so there is nothing to do. + // OOM should never occur if the heap is sized correctly. You can track OOM errors via heap API. + } else { + LOCAL_ASSERT(false); // No other error can possibly occur at runtime. + } +} + +/// @brief Gestione metodo ricezione coda messaggi dal buffer FIFO preparato di BxCAN +/// Il buffer gestito nella ISR CAN_Rx viene passato alla libreria Canard e in automatico +/// è gestita la richiamata di callBack per la funzione esterna di gestione su Rx Messaggi conformi +/// Overloading per log dei messaggi +/// @param logMessage Buffer esterno per la gestione del messaggio di log +void canardClass::receiveQueue(char *logMessage) { + // Leggo l'elemento disponibile in coda BUFFER RX FiFo CanardFrame + Buffer + uint8_t getElement = receiveQueueNextElement(_canard_rx_queue.rd_ptr); + _canard_rx_queue.rd_ptr = getElement; + // ***************************************************************************** + logMessage = itoa(_canard_rx_queue.msg[getElement].frame.payload_size, logMessage, 10); + logMessage += 1; + strcpy(logMessage, ",Val:"); + logMessage += 5; + for(int iIdxPl=0; iIdxPl<_canard_rx_queue.msg[getElement].frame.payload_size; iIdxPl++) { + strcpy(logMessage, " 0x"); + logMessage += 3; + if(_canard_rx_queue.msg[getElement].buf[iIdxPl] < 16) { + strcpy(logMessage, "0"); + logMessage++; + strcpy(logMessage, itoa(_canard_rx_queue.msg[getElement].buf[iIdxPl], logMessage, HEX)); + logMessage++; + } else { + strcpy(logMessage, itoa(_canard_rx_queue.msg[getElement].buf[iIdxPl], logMessage, HEX)); + logMessage+=2; + } + } + *logMessage = 0; + // ***************************************************************************** + // Passaggio CanardFrame Buffered alla RxAccept CANARD + // DeadLine a partire dal realTime assoluto + const CanardMicrosecond timestamp_usec = getMicros(); + CanardRxTransfer transfer; + const int8_t canard_result = canardRxAccept(&_canard, timestamp_usec, &_canard_rx_queue.msg[getElement].frame, IFACE_CAN_IDX, &transfer, NULL); + if (canard_result > 0) { + _attach_rx_callback_PTR(*this, &transfer, _attach_param_PTR); + _canard.memory_free(&_canard, (void*) transfer.payload); + } else if ((canard_result == 0) || (canard_result == -CANARD_ERROR_OUT_OF_MEMORY)) { + (void) 0; // The frame did not complete a transfer so there is nothing to do. + // OOM should never occur if the heap is sized correctly. You can track OOM errors via heap API. + } else { + LOCAL_ASSERT(false); // No other error can possibly occur at runtime. + } +} + +/// @brief Setta il CallBack Function per il processo esterno di interprete su messaggio ricevuto +/// e conforme a Canard. Richiama la funzione esterna su CanardRxAccept Valido. Abilitato su SET +/// @param ptrFunction puntatore alla funzione di callBack(canardClass&, const CanardRxTransfer*) +/// @param param canard rx transfer parameters +void canardClass::setReceiveMessage_CB(void (*ptrFunction) (canardClass&, const CanardRxTransfer*, void *param), void *param) { + _attach_rx_callback_PTR = ptrFunction; + _attach_rx_callback = true; + _attach_param_PTR = param; +} + +/// @brief Abilita il CallBack Function Canard RX Message CanardRxAccept +void canardClass::enableReceiveMessage_CB(void) { + _attach_rx_callback = true; +} + +/// @brief Disabilita il CallBack Function Canard RX Message CanardRxAccept +void canardClass::disableReceiveMessage_CB(void) { + _attach_rx_callback = false; +} + +// *************************************************************************** +// Gestione sottoscrizioni messaggi Canard +// *************************************************************************** + +/// @brief Sottoscrizione di una funzione Canard +/// @param transfer_kind tipo di messaggio +/// @param port_id porta del messaggio +/// @param extent dimensione del pacchetto dati +/// @param transfer_id_timeout_usec daeadline di validità messaggio in microsecondi +/// @return true se la sottoscrizione è avvenuta +bool canardClass::rxSubscribe( + const CanardTransferKind transfer_kind, + const CanardPortID port_id, + const size_t extent, + const CanardMicrosecond transfer_id_timeout_usec) { + // Controllo limiti di sottoscrizioni + if(_rxSubscriptionIdx >= MAX_SUBSCRIPTION) return false; + // Aggiunge rxSubscription con spazio RAM interna alla classe + const int8_t res = canardRxSubscribe(&_canard, + transfer_kind, + port_id, + extent, + transfer_id_timeout_usec, + &_rxSubscription[_rxSubscriptionIdx++]); + return (res>=0); +} + +/// @brief Ritorna il numero di sottoscrizioni ancora disponibili +/// @return Numero di sottoscrizioni libere +uint8_t canardClass::rxSubscriptionAvaiable() { + return MAX_SUBSCRIPTION - _rxSubscriptionIdx; +} + +/// @brief Rimozione di una sottoscrizione di una funzione Canard +/// @param transfer_kind tipo di messaggio +/// @param port_id porta del messaggio +void canardClass::rxUnSubscribe( + const CanardTransferKind transfer_kind, + const CanardPortID port_id) { + (void)canardRxUnsubscribe(&_canard, transfer_kind, port_id); +} + +// ************************************************************************************* +// GESTIONE TIME OUT E SERVIZI RETRY ED ALTRE UTILITY FUNZIONI DI CLASSE MASTER REMOTA +// ************************************************************************************* + +/// @brief Avvia la richiesta di un blocco file contiguo al file_server tramite classe e controllo Master +/// @param timeout_us deadline di comando +/// @return true se il metodo è eseguito correttamente +bool canardClass::master_file_read_block_pending(uint32_t timeout_us) +{ + // Accedo alla sezione master con il controllo pending locale + master.file.start_pending(timeout_us); + // Invio la richiesta standard + return send_file_read_block(master.file.get_server_node(), master.file.get_name(), + master.file.get_offset_rx()); +} + +/// @brief Avvia la richiesta di un blocco file contiguo al file_server con ID Fisico +/// @param server_id remote file_server +/// @param fileName Nome del file da prelevare +/// @param remoteOffset offset di prelievo del blocco file remoto +/// @return true se il metodo è eseguito correttamente +bool canardClass::send_file_read_block(CanardNodeID server_id, char * fileName, uint64_t remoteOffset) +{ + // FileRead V1.1 Handle Message + // ***** Ricezione di file generico dalla rete UAVCAN dal nodo chiamante ***** + // Richiamo in continuazione rapida la funzione fino al riempimento del file + // Alla fine processo il firmware Upload (eventuale) vero e proprio con i relativi check + uavcan_file_Read_Request_1_1 remotefile = {0}; + remotefile.path.path.count = strlen(fileName); + memcpy(remotefile.path.path.elements, fileName, remotefile.path.path.count); + remotefile.offset = remoteOffset; + + uint8_t serialized[uavcan_file_Read_Request_1_1_SERIALIZATION_BUFFER_SIZE_BYTES_] = {0}; + size_t serialized_size = sizeof(serialized); + const int8_t err = uavcan_file_Read_Request_1_1_serialize_(&remotefile, &serialized[0], &serialized_size); + LOCAL_ASSERT(err >= 0); + if (err >= 0) + { + const CanardTransferMetadata meta = { + .priority = CanardPriorityHigh, + .transfer_kind = CanardTransferKindRequest, + .port_id = uavcan_file_Read_1_1_FIXED_PORT_ID_, + .remote_node_id = server_id, + .transfer_id = (CanardTransferID) (next_transfer_id.uavcan_file_read_data()), + }; + // Messaggio standard ad un secondo dal timeStamp Sincronizzato + send(MEGA, &meta, serialized_size, &serialized[0]); + return true; + } + return false; +} + +// ******************************************************************************* +// GESTIONE TIME OUT E SERVIZI RETRY ED ALTRE UTILITY FUNZIONI DI CLASSE MASTER +// ******************************************************************************* + +// ******************************** HEART BEAT *********************************** + +/// @brief Controlla se il modulo master è online +/// @param is_heart_syncronized (controlla se nell'ambito del tempo di haert_beat x Syncro HeartBeat Locale) +/// @return true se il mater remoto è correttamente onLine (ha comunicato) nel limite di tempo valido +bool canardClass::master::heartbeat::is_online(bool is_heart_syncronized) { + // ? Out of TIME Max (Sempre false) + if (_syncMicros < _timeout_us) { + // Richiedo Internal HeartBeat Area check ? (Possibile syncro HeartBeat Locale...) + // Altrimenti la richiesta ha già esito positivo + if(is_heart_syncronized) { + // ? Siamo all'interno del tempo di HeartBeat Max (x1.5) + if ((_syncMicros - _mastMicros) < MEGA * (TIME_PUBLISH_HEARTBEAT * 1.5)) { + // Reset (Wait Next Syncro Master...) + _mastMicros = _syncMicros; + return true; + } + else return false; + } + else return true; + } + else return false; +} + +/// @brief Imposta il nodo OnLine, richiamato in heartbeat o altre comunicazioni client +/// @param dead_line_us validità di tempo us a partire dal time_stamp sincronizzato interno +void canardClass::master::heartbeat::set_online(uint32_t dead_line_us) { + _timeout_us = _syncMicros + dead_line_us; + _mastMicros = _syncMicros; +} + +/// @brief Imposta il nodo OnLine, richiamato in heartbeat o altre comunicazioni client +CanardMicrosecond canardClass::master::heartbeat::last_online(void) { + return _mastMicros; +} + +// ******************************** TIME STAMP *********************************** + +/// @brief Controlla se messaggio valido (coerenza sequenza e validità di time_stamp) +/// @param current_transfer_id Indice di messaggio +/// @param previous_tx_timestamp_us timestamp precedente locale +/// @return true se messaggio è decodificabile con timestamp corretto +bool canardClass::master::timestamp::check_valid_syncronization(uint8_t current_transfer_id, + CanardMicrosecond previous_tx_timestamp_us) { + // Controllo coerenza messaggio per consentire l'aggiornamento timestamp + // 1) Sequenza di transfer_id con previous + // 2) differenza di tempo tra due timestamp remoti inferiore al massimo timeOut impostato di controllo + if((++_previous_transfer_id == current_transfer_id) && + ((previous_tx_timestamp_us - _previous_msg_monotonic_us) < MASTER_MAXSYNCRO_VALID_US)) + // Riferimento locale + return true; + // Risincronizzo il transferID Corretto per sicurezza + _previous_transfer_id = current_transfer_id; + return false; +} + +/// @brief Legge il tempo sincronizzato dal master, utilizzabile dopo controllo di validità del messaggio +/// @param current_rx_timestamp_us time stamp reale del messaggio entrante (al tempo di RX) +/// @param previous_tx_timestamp_us time stamp remoto attuale, riferito al precedente invio remoto di time stamp +/// @return il tempo sincronizzato, regolato ed aggiustato al microsecondo, con l'unità master +CanardMicrosecond canardClass::master::timestamp::get_timestamp_syncronized(CanardMicrosecond current_rx_timestamp_us, + CanardMicrosecond previous_tx_timestamp_us) { + // Save timestamp variabili per SET e controllo Time nella successiva chiamata time_syncronization + // Local RealTime RX timestamp monotonic locale al tempo reale di rx_message (transfer->timestamp_usec) + _syncronized_timestamp_us = previous_tx_timestamp_us + current_rx_timestamp_us - _previous_local_timestamp_us; + // Aggiusto e sommo la differenza tra il timestamp reale del messaggio ricevuto con il monotonic reale attuale (al tempo di esecuzione syncro real) + _previous_syncronized_timestamp_us = getMicros(); + _syncronized_timestamp_us += (_previous_syncronized_timestamp_us - current_rx_timestamp_us); + return _syncronized_timestamp_us; +} + +/// @brief Legge il tempo sincronizzato dal master, in Overload, regolato sui microsecondi locali +/// a partire dall'ultima sincronizzazione valida. Utilizzabile come FAKE_RTC() dopo una +/// ricezione di almeno un messagguo valido +/// @return il tempo sincronizzato, regolato ed aggiustato al microsecondo, con l'unità master +CanardMicrosecond canardClass::master::timestamp::get_timestamp_syncronized(void) { + CanardMicrosecond realtime_us = getMicros(); + CanardMicrosecond diff_synconized_us = realtime_us - _previous_syncronized_timestamp_us; + _previous_syncronized_timestamp_us = realtime_us; + _syncronized_timestamp_us += diff_synconized_us; + return _syncronized_timestamp_us; +} + +/// @brief Aggiorna i time stamp della classe sul messaggio standard UAVCAN per le successive sincronizzazioni +/// @param current_rx_timestamp_us time stamp reale del messaggio entrante (al tempo di RX) +/// @param previous_tx_timestamp_us time stamp remoto attuale, riferito al precedente invio remoto di time stamp +/// @return +CanardMicrosecond canardClass::master::timestamp::update_timestamp_message(CanardMicrosecond current_rx_timestamp_us, + CanardMicrosecond previous_tx_timestamp_us) { + // Differenza tra due messaggi in microsecondi + CanardMicrosecond difference_timestamp_us = previous_tx_timestamp_us - _previous_msg_monotonic_us; + // Save timestamp variabili per SET e controllo Time nella successiva chiamata time_syncronization + // Local RealTime RX timestamp monotonic locale al tempo reale di rx_message (transfer->timestamp_usec) + _previous_local_timestamp_us = current_rx_timestamp_us; + // RealTime callBack message RX (TX RTC uSec(Canard type) Master Remoto al tempo di send) + _previous_msg_monotonic_us = previous_tx_timestamp_us; + return difference_timestamp_us; +} + +// ******************************** FILE CLIENT *********************************** + +/// @brief Avvia una richiesta remota di download file ed inizializza le risorse nella classe +/// @param remote_node nodo di provenienza della richiesta, controllo messaggi in coerenza con il nodo +/// @param param_request_name nome del file (parametro request->element entrante) +/// @param param_request_len lunghezza del nome del file (parametro request->count) +/// @param is_firmware imposta il flag che indica se è un file firmware +void canardClass::master::file::start_request(uint8_t remote_node, uint8_t *param_request_name, + uint8_t param_request_len, bool is_firmware) { + _node_id = remote_node; + // Copio la stringa nel name file firmware disponibile su state generale (per download successivo) + memcpy(_filename, param_request_name, param_request_len); + _filename[param_request_len] = '\0'; + // Init varaiabili di download + _is_firmware = is_firmware; + _updating = true; + _updating_eof = false; + // Start Offset File + _offset = 0; + _updating_retry = 0; + // Set first pending + _timeout_us = _syncMicros + NODE_GETFILE_TIMEOUT_US; +} + +/// @brief Nome del file in download +/// @return puntatore e buffer al nome del file per il dowloading. Non necessità di inizializzazione buffer. +char* canardClass::master::file::get_name(void) { + return _filename; +} + +/// @brief Legge il nodo master file server che richiede il caricamento del file +/// @return node_id UAVCAN +CanardNodeID canardClass::master::file::get_server_node(void) { + return _node_id; +} + +/// @brief Gestione file, verifica richiesta di download da un nodo remoto +/// @return true se è in corso una procedura di ricezione file (start comando o download) +bool canardClass::master::file::download_request(void) { + return _updating; +} + +/// @brief Gestione file, fine con successo del download file da un nodo remoto. +/// Il nodo non risponde più alle richieste del comando fino a nuovo restart. +/// @return true se è finita la procedura di ricezione file con successo +bool canardClass::master::file::is_download_complete(void) { + return _updating_eof; +} + +/// @brief Gestione file, abbandona o termina richiesta di download da un nodo remoto. +/// Il nodo non risponde più alle richieste del comando fino a nuovo restart. +void canardClass::master::file::download_end(void) { + _updating = false; + _node_id = CANARD_NODE_ID_UNSET; +} + +/// @brief Controlla se il file è di tipo firmware o altra tipologia +/// @return true se il file in download è di tipo firmware +bool canardClass::master::file::is_firmware(void) +{ + // Riferimento locale + return _is_firmware; +} + +/// @brief Legge l'offset corrente +/// @return l'offste corrente del file attualmente in download +uint64_t canardClass::master::file::get_offset_rx(void) { + return _offset; +} + +/// @brief Imposta l'offset RX per la lettura di un blocco specifico del file in donload remoto +/// @param remote_file_offset indirizzo offset del byte da leggere +void canardClass::master::file::set_offset_rx(uint64_t remote_file_offset) { + _offset = remote_file_offset; +} + +/// @brief Verifica se è il primo blocco di un file. Da utilizzare per rewrite file o init E2Prom Space +/// @return true se il blocco ricevuto è il primo del file +bool canardClass::master::file::is_first_data_block(void) { + return _offset == 0; +} + +/// @brief Gestione automatica totale retry del comando file all'interno della classe +/// MAX retry è gestito nel file di configurazione module_config.h +/// @return true se ci sono ancora retry disponibili per il comando +bool canardClass::master::file::next_retry(void) { + if (++_updating_retry > NODE_GETFILE_MAX_RETRY) _updating_retry = NODE_GETFILE_MAX_RETRY; + // Reset pending state + _is_pending = false; + return _updating_retry < NODE_GETFILE_MAX_RETRY; +} + +/// @brief Gestione automatica totale retry del comando file all'interno della classe +/// MAX retry è gestito nel file di configurazione module_config.h +/// @param avaiable_retry retry ancora disponibili +/// @return true se ci sono ancora retry disponibili per il comando +bool canardClass::master::file::next_retry(uint8_t *avaiable_retry) { + if (++_updating_retry > NODE_GETFILE_MAX_RETRY) _updating_retry = NODE_GETFILE_MAX_RETRY; + *avaiable_retry = NODE_GETFILE_MAX_RETRY - _updating_retry; + // Reset pending state + _is_pending = false; + return _updating_retry < NODE_GETFILE_MAX_RETRY; +} + +/// @brief Avvia un comando per il metodo corrente con timeOut di validità +/// @param timeout_us microsecondi per entrata in timeOut di mancata risposta +void canardClass::master::file::start_pending(uint32_t timeout_us) +{ + // Riferimento locale + _is_pending = true; + _timeout_us = _syncMicros + timeout_us; +} + +/// @brief Resetta lo stato dei flag pending per il metodo corrente +void canardClass::master::file::reset_pending(void) +{ + _is_pending = false; + // Azzero contestualmente le retry di controllo x gestione MAX_RETRY -> ABORT + _updating_retry = 0; +} + +/// @brief Resetta lo stato dei flag pending per il metodo corrente +/// @param message_len lunghezza del messaggio ricevuto in risposta +void canardClass::master::file::reset_pending(size_t message_len) +{ + _is_pending = false; + // Azzero contestualmente le retry di controllo x gestione MAX_RETRY -> ABORT + _updating_retry = 0; + // Gestisco internamente l'offset di RX File + _offset += message_len; + // Overload per controllo EOF (se lunghezza messaggio != MAX_ARRAY_UAVCAN ... 256 Bytes) + _updating_eof = (message_len != uavcan_primitive_Unstructured_1_0_value_ARRAY_CAPACITY_); +} + +/// @brief Gestione timeout pending file. Controlla il raggiungimento del timeout +/// @return true se entrata in timeout del comano +bool canardClass::master::file::event_timeout(void) +{ + if(_is_pending) return _syncMicros > _timeout_us; + return false; +} + +/// @brief Verifica se un comando per il relativo modulo è in attesa. Diventerà false o verrà attivato il timeout +/// @return true se un comando è in attesa +bool canardClass::master::file::is_pending(void) +{ + return _is_pending; +} + +// *********************************************************************************** +// GESTIONE TIME OUT E SERVIZI RETRY ED ALTRE UTILITY FUNZIONI DI CLASSE LOCALE SLAVE +// *********************************************************************************** + +/// @brief Invia il messaggio di HeartBeat ai nodi remoti +/// @return true se il metodo è eseguito correttamente +bool canardClass::slave_heartbeat_send_message(void) +{ + // ***** Trasmette alla rete UAVCAN lo stato haeartbeat del modulo ***** + // Heartbeat Fisso anche per modulo Master (Visibile a yakut o altri tools/script gestionali) + uavcan_node_Heartbeat_1_0 heartbeat = {0}; + heartbeat.uptime = getUpTimeSecond(); + const O1HeapDiagnostics heap_diag = _memGetDiagnostics(); + if (heap_diag.oom_count > 0) { + heartbeat.health.value = uavcan_node_Health_1_0_CAUTION; + } else { + heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL; + } + // Stato di heartbeat gestito dalla classe + heartbeat.vendor_specific_status_code = flag.get_local_value_heartbeat_VSC(); + heartbeat.mode.value = flag.get_local_node_mode(); + // Trasmetto il pacchetto + uint8_t serialized[uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_] = {0}; + size_t serialized_size = sizeof(serialized); + const int8_t err = uavcan_node_Heartbeat_1_0_serialize_(&heartbeat, &serialized[0], &serialized_size); + LOCAL_ASSERT(err >= 0); + if (err >= 0) { + const CanardTransferMetadata meta = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = (CanardTransferID)(next_transfer_id.uavcan_node_heartbeat()), + }; + // Messaggio standard ad un secondo dal timeStamp Sincronizzato + send(MEGA, &meta, serialized_size, &serialized[0]); + return true; + } + return false; +} + +/// @brief Invia il messaggio di PNP request (richiesta di node_id valido) al nodo server PNP (master) +/// @return true se il metodo è eseguito correttamente +bool canardClass::slave_pnp_send_request(uint64_t serial_number) { + // PnP over Classic CAN, use message v1.0. + uavcan_pnp_NodeIDAllocationData_1_0 msg = {0}; + // truncated uint48 unique_id_hash + // msg.allocated_node_id.(count/element) => Solo in response non in request; + msg.unique_id_hash = serial_number >> HASH_EXCLUDING_BIT; + msg.unique_id_hash &= HASH_SERNUMB_MASK; + msg.unique_id_hash |= MODULE_TYPE; + uint8_t serialized[uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_] = {0}; + size_t serialized_size = sizeof(serialized); + const int8_t err = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, &serialized[0], &serialized_size); + LOCAL_ASSERT(err >= 0); + if (err >= 0) { + const CanardTransferMetadata meta = { + .priority = CanardPrioritySlow, + .transfer_kind = CanardTransferKindMessage, + .port_id = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = (CanardTransferID) (next_transfer_id.uavcan_pnp_allocation()), + }; + // Messaggio standard ad un secondo dal timeStamp Sincronizzato + send(MEGA, &meta, serialized_size, &serialized[0]); + return true; + } + return false; +} + + +/// @brief Prepara la lista delle sottoscrizioni Canard (privata) +/// @param tree out albero sottoscrizioni +/// @param obj oggetto sottoscrizioni +void canardClass::_fillSubscriptions(const CanardTreeNode* const tree, uavcan_node_port_SubjectIDList_0_1* const obj) +{ + if (NULL != tree) { + _fillSubscriptions(tree->lr[0], obj); + const CanardRxSubscription* crs = (const CanardRxSubscription*)tree; + if (crs->port_id <= CANARD_SUBJECT_ID_MAX) { + LOCAL_ASSERT(obj->sparse_list.count < uavcan_node_port_SubjectIDList_0_1_sparse_list_ARRAY_CAPACITY_); + obj->sparse_list.elements[obj->sparse_list.count++].value = crs->port_id; + _fillSubscriptions(tree->lr[1], obj); + } + } +} + +/// @brief Prepara la lista dei servizi Canard (privata) +/// @param tree out albero servizi +/// @param obj oggetto servizi +void canardClass::_fillServers(const CanardTreeNode* const tree, uavcan_node_port_ServiceIDList_0_1* const obj) +{ + if (NULL != tree) { + _fillServers(tree->lr[0], obj); + const CanardRxSubscription* crs = (const CanardRxSubscription*)tree; + if (crs->port_id <= CANARD_SERVICE_ID_MAX) { + (void)nunavutSetBit(&obj->mask_bitpacked_[0], sizeof(obj->mask_bitpacked_), crs->port_id, true); + _fillServers(tree->lr[1], obj); + } + } +} + +/// @brief Invia il messaggio dei servizi attivi ai nodi remoti +/// @return true se il metodo è eseguito correttamente +bool canardClass::slave_servicelist_send_message(void) +{ + // Publish the recommended (not required) port introspection message. No point publishing it if we're anonymous. + // The message is a bit heavy on the stack (about 2 KiB) but this is not a problem for a modern MCU. + // L'abilitazione del comando è facoltativa, può essere attivata/disattivata da un comando UAVCAN + if ((publisher_enabled.port_list) && + (_canard.node_id <= CANARD_NODE_ID_MAX)) + { + uavcan_node_port_List_0_1 m = {0}; + uavcan_node_port_List_0_1_initialize_(&m); + uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&m.publishers); + uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&m.subscribers); + + // Indicate which subjects we publish to. Don't forget to keep this updated if you add new publications! + { + size_t* const cnt = &m.publishers.sparse_list.count; + m.publishers.sparse_list.elements[(*cnt)++].value = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_; + m.publishers.sparse_list.elements[(*cnt)++].value = uavcan_node_port_List_0_1_FIXED_PORT_ID_; + // Aggiungo i publisher interni validi privati (quando abilitati) + if ((port_id.publisher_module_leaf <= CANARD_SUBJECT_ID_MAX)&& + (publisher_enabled.module_leaf)) + { + m.publishers.sparse_list.elements[(*cnt)++].value = port_id.publisher_module_leaf; + } + } + + // Indicate which servers and subscribers we implement. + // We could construct the list manually but it's easier and more robust to just query libcanard for that. + _fillSubscriptions(_canard.rx_subscriptions[CanardTransferKindMessage], &m.subscribers); + _fillServers(_canard.rx_subscriptions[CanardTransferKindRequest], &m.servers); + _fillServers(_canard.rx_subscriptions[CanardTransferKindResponse], &m.clients); // For regularity. + + // Serialize and publish the message. Use a small buffer because we know that our message is always small. + // Verificato massimo utilizzo a 156 bytes. Limitiamo il buffer a 256 Bytes (Come esempio UAVCAN) + uint8_t serialized[256] = {0}; + size_t serialized_size = uavcan_node_port_List_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; + if (uavcan_node_port_List_0_1_serialize_(&m, &serialized[0], &serialized_size) >= 0) + { + const CanardTransferMetadata meta = { + .priority = CanardPriorityOptional, // Mind the priority. + .transfer_kind = CanardTransferKindMessage, + .port_id = uavcan_node_port_List_0_1_FIXED_PORT_ID_, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = (CanardTransferID) (next_transfer_id.uavcan_node_port_list()), + }; + // Send a 2 secondi + send(MEGA * 2, &meta, serialized_size, &serialized[0]); + return true; + } + } + return false; +} + +// *************************************************************** +// ID di Trasferimento UAVCAN Canard +// *************************************************************** + +/// @brief Gestione transfer ID UAVCAN per la classe relativa +/// @return Prossimo transfer_id valido in standard UAVCAN +uint8_t canardClass::next_transfer_id::uavcan_node_heartbeat(void) { + return next_transfer_id::_uavcan_node_heartbeat++; +} + +/// @brief Gestione transfer ID UAVCAN per la classe relativa +/// @return Prossimo transfer_id valido in standard UAVCAN +uint8_t canardClass::next_transfer_id::uavcan_node_port_list(void) { + return next_transfer_id::_uavcan_node_port_list++; +} + +/// @brief Gestione transfer ID UAVCAN per la classe relativa +/// @return Prossimo transfer_id valido in standard UAVCAN +uint8_t canardClass::next_transfer_id::uavcan_pnp_allocation(void) { + return next_transfer_id::_uavcan_pnp_allocation++; +} + +/// @brief Gestione transfer ID UAVCAN per la classe relativa +/// @return Prossimo transfer_id valido in standard UAVCAN +uint8_t canardClass::next_transfer_id::uavcan_file_read_data(void) { + return next_transfer_id::_uavcan_file_read_data++; +} + +/// @brief Gestione transfer ID UAVCAN per la classe relativa +/// @return Prossimo transfer_id valido in standard UAVCAN +uint8_t canardClass::next_transfer_id::module_leaf(void) { + return next_transfer_id::_module_leaf++; +} + +// *************************************************************** +// Funzioni ed Utility Canard su Local Flag +// *************************************************************** + +// ******** COMMAND REMOTI E Stati gestionali ******** + +/// @brief Avvia una richiesta standard UAVCAN per il riavvio del sistema +void canardClass::flag::request_system_restart(void) { + _restart_required = true; +} + +/// @brief Verifica una richiesta di riavvio del sistema standard UAVCAN. +/// @return true se è stata avanzata una richiesta di riavvio +bool canardClass::flag::is_requested_system_restart(void) { + return _restart_required; +} + +/// @brief Avvia la richiesta di sleep del modulo. Da chiamare prima di attivare il basso consumo generale +void canardClass::flag::request_sleep(void) { + // Con inibizione, non permetto lo sleep del modulo + if(_inibith_sleep) return; + // Eseguo la procedura di messa in sleep del modulo + // Variabili, HW CAN ecc... alla fine setto la var di entrata in sleep + _enter_sleep = true; +} + +/// @brief Verifica se attiva la funzione dello sleep del modulo Canard e hardware relativo +/// @return true se il modulo è in sleep +bool canardClass::flag::is_module_sleep(void) { + return _enter_sleep; +} + +/// @brief Permetto l'attivazione sleep, funzioni ed hardware, del modulo +void canardClass::flag::disable_sleep(void) { + _inibith_sleep = true; +} + +/// @brief Inibisce l'attivazione sleep, funzioni ed hardware, del modulo +void canardClass::flag::enable_sleep(void) { + _inibith_sleep = false; +} + +// ******** HEARTBEAT Vendor Status Code ******** +// Set VendorStatusCode per Heratbeat in RX/TX al modulo Master/Slave + +/// @brief Proprietà SET per il valore VendorStatusCode di Heartbeat e per gli utilizzi locali +/// La proprietà è impostata normalmente dal master remoto e viene settata per il locale +/// @param powerMode Modalità power (CanardClass::Power_Mode) +void canardClass::flag::set_local_power_mode(Power_Mode powerMode) { + _heartLocalVSC.powerMode = powerMode; +} + +/// @brief Proprietà SET per il valore VendorStatusCode di Heartbeat e per gli utilizzi locali +/// @param fwUploading true se attivata la funzionalità di firmware uploading +void canardClass::flag::set_local_fw_uploading(bool fwUploading) { + _heartLocalVSC.fwUploading = fwUploading; +} + +/// @brief Proprietà SET per il valore VendorStatusCode di Heartbeat e per gli utilizzi locali +/// @param dataReady true se sono disponibili dati del modulo da presentare al master tramite SensorDrive +void canardClass::flag::set_local_data_ready(bool dataReady) { + _heartLocalVSC.dataReady = dataReady; +} + +/// @brief Proprietà SET per il valore VendorStatusCode di Heartbeat e per gli utilizzi locali +/// @param moduleReady true se il modulo è pronto alle funzioni standard (start complete o fine manutenzione) +void canardClass::flag::set_local_module_ready(bool moduleReady) { + _heartLocalVSC.moduleReady = moduleReady; +} + +/// @brief Proprietà SET per il valore VendorStatusCode di Heartbeat e per gli utilizzi locali +/// @param moduleError true se il modulo è in errore HW +void canardClass::flag::set_local_module_error(bool moduleError) { + _heartLocalVSC.moduleError = moduleError; +} + +/// @brief Proprietà GET per il valore VendorStatusCode di Heartbeat e per gli utilizzi locali +/// @return Modalità power (CanardClass::Power_Mode) +Power_Mode canardClass::flag::get_local_power_mode(void) { + return _heartLocalVSC.powerMode; +} + +/// @brief Proprietà GET per il valore VendorStatusCode di Heartbeat e per gli utilizzi locali +/// @return true se attivata la funzionalità di firmware uploading +bool canardClass::flag::get_local_fw_uploading(void) { + return _heartLocalVSC.fwUploading; +} + +/// @brief Proprietà GET per il valore VendorStatusCode di Heartbeat e per gli utilizzi locali +/// @return true se sono disponibili dati del modulo da presentare al master tramite SensorDrive +bool canardClass::flag::get_local_data_ready(void) { + return _heartLocalVSC.dataReady; +} + +/// @brief Proprietà GET per il valore VendorStatusCode di Heartbeat e per gli utilizzi locali +/// @return true se il modulo è pronto alle funzioni standard (start complete o fine manutenzione) +bool canardClass::flag::get_local_module_ready(void) { + return _heartLocalVSC.moduleReady; +} + +/// @brief Proprietà GET per il valore VendorStatusCode di Heartbeat e per gli utilizzi locali +/// @return true se il modulo è in errore HW +bool canardClass::flag::get_local_module_error(void) { + return _heartLocalVSC.moduleError; +} + +/// @brief Proprietà GET per il valore VendorStatusCode di Heartbeat e per gli utilizzi locali +/// @return +uint8_t canardClass::flag::get_local_value_heartbeat_VSC(void) { + return _heartLocalVSC.uint8_val; +} + +// Funzioni Locali per gestione interna NODE MODE x HeartBeat standard UAVCAN + +/// @brief Imposta la modalità nodo standard UAVCAN, gestita nelle funzioni heartbeat +/// @param heartLocalMODE Modalità local node standard di UAVCAN +void canardClass::flag::set_local_node_mode(uint8_t heartLocalMODE) { + _heartLocalMODE = heartLocalMODE; +} + +/// @brief Ritorna la modalità node mode locale standard UAVCAN per la gestione heartbeat +/// @return modalità node mode di UAVCAN +uint8_t canardClass::flag::get_local_node_mode(void) { + return _heartLocalMODE; +} + +// *************************************************************** +// Funzioni ed Utility di Classe Generali +// *************************************************************** + +/// @brief Imposta l'ID Nodo locale per l'istanza Canard privata della classe Canard +/// @param local_id id nodo locale +void canardClass::set_canard_node_id(CanardNodeID local_id) { + // Istanza del modulo canard + _canard.node_id = local_id; +} + +/// @brief Legge l'ID Nodo locale per l'istanza Canard privata della classe Canard +/// @return id nodo locale +CanardNodeID canardClass::get_canard_node_id(void) { + // Istanza del modulo canard + return _canard.node_id; +} + +/// @brief Controlla se il nodo è anonimo (senza ID Impostato) +/// @return true se il nodo è anonimo (ID Non valido o non Settato) +bool canardClass::is_canard_node_anonymous(void) { + // Istanza del modulo canard + return _canard.node_id > CANARD_NODE_ID_MAX; +} + +/// @brief Set node master ID (slave respond to RMAP command and flag with master ID) +/// @param remote_id remote id for master (normal reading in register) +void canardClass::set_canard_master_id(CanardNodeID remote_id) { + // Istanza del modulo canard + _master_id = remote_id; +} + +/// @brief Get master node id for local set and flag operation automatic (sleep, power...) +/// @return Master node ID +CanardNodeID canardClass::get_canard_master_id(void) { + // Istanza del modulo canard + return _master_id; +} + +// *************************************************************** +// Wrapper C++ O1Heap memory Access & Function CB Private +// *************************************************************** + +/// @brief Gestione O1Heap Memory Canard Allocate +/// @param ins istanza canard +/// @param amount dimensione da allocare +/// @return puntatore alla O1Heap allineata +void* canardClass::_memAllocate(CanardInstance* const ins, const size_t amount) { + O1HeapInstance* const heap = ((canardClass*)ins->user_reference)->_heap; + LOCAL_ASSERT(o1heapDoInvariantsHold(heap)); + return o1heapAllocate(heap, amount); +} + +/// @brief Gestione O1Heap Memory Canard Free +/// @param ins istanza canard +/// @param pointer puntatore alla posizione da deallocare +void canardClass::_memFree(CanardInstance* const ins, void* const pointer) { + O1HeapInstance* const heap = ((canardClass*)ins->user_reference)->_heap; + o1heapFree(heap, pointer); +} + +/// @brief Diagnostica Heap Data per Heartbeat e/o azioni interne +/// @return O1HeapDiagnostics Info +O1HeapDiagnostics canardClass::_memGetDiagnostics(void) { + return o1heapGetDiagnostics(_heap); +} diff --git a/platformio/stima_v4/slave-leaf/src/debug.cpp b/platformio/stima_v4/slave-leaf/src/debug.cpp new file mode 100644 index 000000000..31ce42819 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/src/debug.cpp @@ -0,0 +1,113 @@ +/** + ****************************************************************************** + * @file debug.cpp + * @author Moreno Gasperini + * @author Marco Baldinetti + * @brief debug for stimaV4 Slave source file + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#include "debug.h" + +/// @brief Put char to file output +/// @param c char to write +/// @param stream output mode file +/// @return success +int_t fputc(int_t c, FILE *stream) +{ + // Standard output or error output? + if (stream == stdout || stream == stderr) + { + #ifndef DISABLE_SERIAL + Serial.write(c); + #endif + return c; + } + // Unknown output? + else + { + // If a writing error occurs, EOF is returned + return EOF; + } +} + +/// @brief init serial monitor +/// @param baudrate speed monitor +void init_debug(uint32_t baudrate) { + #ifndef DISABLE_SERIAL + Serial.begin(baudrate); + while (!Serial); + #endif +} + +/// @brief Print debug from ram +/// @param fmt pointer to class FlashStringHelper +void print_debug(const char *fmt, ...) +{ + va_list args; + va_start(args, fmt); + vfprintf(stdout, fmt, args); + va_end(args); + #ifndef DISABLE_SERIAL + Serial.flush(); + #endif +} + +/// @brief Display the contents of an array +/// @param prepend String to prepend to the left of each line +/// @param data Pointer to the data array +/// @param length Number of bytes to display +void print_debug_array(const char *prepend, const void *data, size_t length) +{ + for (uint8_t i = 0; i < length; i++) + { + // Beginning of a new line? + if ((i % 16) == 0) + { + TRACE_PRINTF("%s", prepend); + } + // Display current data byte + TRACE_PRINTF("%02" PRIX8 " ", *((const uint8_t *)data + i)); + // End of current line? + if ((i % 16) == 15 || i == (length - 1)) + { + TRACE_PRINTF("\r\n"); + } + } +} + +/// @brief Print debug from rom Flash +/// @param fmt pointer to class FlashStringHelper +void print_debug_F(const __FlashStringHelper *fmt, ...) +{ + osSuspendAllTasks(); + va_list args; + va_start(args, fmt); + vfprintf(stdout, (const char *)fmt, args); + va_end(args); + #ifndef DISABLE_SERIAL + Serial.flush(); + #endif + osResumeAllTasks(); +} diff --git a/platformio/stima_v4/slave-leaf/src/drivers/accelerometer.cpp b/platformio/stima_v4/slave-leaf/src/drivers/accelerometer.cpp new file mode 100644 index 000000000..fd947b54b --- /dev/null +++ b/platformio/stima_v4/slave-leaf/src/drivers/accelerometer.cpp @@ -0,0 +1,1962 @@ +/** + ****************************************************************************** + * @file Accelerometer.cpp + * @author Moreno Gasperini + * @brief Accelerometer IIS328DQ driver file + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#include "drivers/accelerometer.h" + +/// @brief Constructor Class +Accelerometer::Accelerometer() +{ +} + +/// @brief Construct a new Accelerometer::Accelerometer object +/// @param wire I2C class +/// @param wireLock I2C semaphore +/// @param i2c_address I2C address +Accelerometer::Accelerometer(TwoWire *wire, BinarySemaphore *wireLock, uint8_t i2c_address) +{ + _wire = wire; + _wireLock = wireLock; + _i2c_address = i2c_address; + // Reset scroll filter array; + for(uint8_t scrl=0; scrl no Error) +int32_t Accelerometer::iis328dq_read_reg(uint8_t reg, uint8_t *data, uint16_t len) +{ + // Try lock Semaphore + if (_wireLock->Take(Ticks::MsToTicks(ACCELEROMETER_SEMAPHORE_MAX_WAITING_TIME_MS))) + { + /* Read multiple command */ + reg |= 0x80; + _wire->beginTransmission(_i2c_address); + _wire->write(reg); + _wire->endTransmission(); + _wire->beginTransmission(_i2c_address); + _wire->requestFrom(_i2c_address, len); + for(uint8_t i=0; iread(); + _wire->endTransmission(); + _wireLock->Give(); + return 0; + } else + return 1; +} + +/// @brief Write generic device register +/// @param reg register to write +/// @param data pointer to data to write in register reg(ptr) +/// @param len number of consecutive register to write +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_write_reg(uint8_t reg, uint8_t *data, uint16_t len) +{ + // Try lock Semaphore + if (_wireLock->Take(Ticks::MsToTicks(ACCELEROMETER_SEMAPHORE_MAX_WAITING_TIME_MS))) + { + // /* Write multiple command */ + reg|=0x80; + _wire->beginTransmission(_i2c_address); + _wire->write(reg); + for(uint8_t i=0; iwrite(*(data + i)); + _wire->endTransmission(); + _wireLock->Give(); + return 0; + } else + return 1; +} + +/** + * @defgroup Accelerometer Accelerometer driver functions + * @{ + */ + +/** + * @defgroup IIS328DQ_Sensitivity IIS328DQ_Sensitivity + * @brief These functions convert raw-data into engineering units + * @{ + */ + +/// @brief Push data row for scroll mean sensibility data +/// @param data_raw +void Accelerometer::push_raw_data(int16_t *data_raw) +{ + // Scroll Value + for(uint8_t i = (ARR_REG_FILTER-1); i>0; i--) { + _raw_scroll[0][i] = _raw_scroll[0][i-1]; + _raw_scroll[1][i] = _raw_scroll[1][i-1]; + _raw_scroll[2][i] = _raw_scroll[2][i-1]; + } + // Add New + _raw_scroll[0][0] = data_raw[0]; + _raw_scroll[1][0] = data_raw[1]; + _raw_scroll[2][0] = data_raw[2]; +} + +/// @brief Get mean value from scroll filter raw array +/// @param request coordinate to get +/// @return raw_data filtered +int16_t Accelerometer::get_raw_mean(coordinate request) { + uint32_t tmp_data = 0; + uint8_t tmp_count = 0; + for(uint8_t i=0; i=-16000) && (_raw_scroll[request][i]<=16000)) { + tmp_data += _raw_scroll[request][i]; + tmp_count++; + } + } + return (int16_t) (tmp_data / tmp_count); +} + +/// @brief Read istant value accelerometer scaled on 2G to mg +/// @param lsb int16 readed from device +/// @return float conversion to mg +float_t Accelerometer::iis328dq_from_fs2_to_mg(int16_t lsb) +{ + return ((float_t)lsb * 0.98f / 16.0f); +} + +/// @brief Read from sroll mean accelerometer scaled on 2G to mg +/// @param request coordinate to get +/// @return float conversion to mg +float_t Accelerometer::iis328dq_from_fs2_to_mg(coordinate request) +{ + return ((float_t)get_raw_mean(request) * 0.98f / 16.0f); +} + +/// @brief Read istant value accelerometer scaled on 4G to mg +/// @param lsb int16 readed from device +/// @return float conversion to mg +float_t Accelerometer::iis328dq_from_fs4_to_mg(int16_t lsb) +{ + return ((float_t)lsb * 1.95f / 16.0f); +} + +/// @brief Read from sroll mean accelerometer scaled on 4G to mg +/// @param request coordinate to get +/// @return float conversion to mg +float_t Accelerometer::iis328dq_from_fs4_to_mg(coordinate request) +{ + return ((float_t)get_raw_mean(request) * 1.95f / 16.0f); +} + +/// @brief Read istant value accelerometer scaled on 8G to mg +/// @param lsb int16 readed from device +/// @return float conversion to mg +float_t Accelerometer::iis328dq_from_fs8_to_mg(int16_t lsb) +{ + return ((float_t)lsb * 3.91f / 16.0f); +} + +/// @brief Read from sroll mean accelerometer scaled on 8G to mg +/// @param request coordinate to get +/// @return float conversion to mg +float_t Accelerometer::iis328dq_from_fs8_to_mg(coordinate request) +{ + return ((float_t)get_raw_mean(request) * 3.91f / 16.0f); +} + +/// @brief Read istant value accelerometer scaled 0-100% to inclinometer value +/// @param lsb int16 readed from device +/// @return float conversion to inclinometer value +float_t Accelerometer::iis328dq_from_fsx_to_inc(int16_t lsb) +{ + return ((float_t)lsb / 16000.0f); +} + +/// @brief Read from sroll mean accelerometer scaled 0-100% to inclinometer value +/// @param request coordinate to get +/// @return float conversion to inclinometer value +float_t Accelerometer::iis328dq_from_fsx_to_inc(coordinate request) +{ + return ((float_t)get_raw_mean(request) / 16000.0f); +} + +/** + * @} + */ + +/** + * @defgroup IIS328DQ_Data_Generation IIS328DQ_Data_Generation + * @brief This section group all the functions concerning data generation + * @{ + */ + +/// @brief X axis enable/disable.[set] +/// @param val change the values of xen in reg CTRL_REG1 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_axis_x_data_set(uint8_t val) +{ + iis328dq_ctrl_reg1_t ctrl_reg1; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG1, + (uint8_t *)&ctrl_reg1, 1); + + if (ret == 0) + { + ctrl_reg1.xen = val; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG1, + (uint8_t *)&ctrl_reg1, 1); + } + + return ret; +} + +/// @brief X axis enable/disable.[get] +/// @param val change the values of xen in reg CTRL_REG1 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_axis_x_data_get(uint8_t *val) +{ + iis328dq_ctrl_reg1_t ctrl_reg1; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG1, + (uint8_t *)&ctrl_reg1, 1); + *val = ctrl_reg1.xen; + + return ret; +} + +/// @brief Y axis enable/disable.[set] +/// @param val change the values of yen in reg CTRL_REG1 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_axis_y_data_set(uint8_t val) +{ + iis328dq_ctrl_reg1_t ctrl_reg1; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG1, + (uint8_t *)&ctrl_reg1, 1); + + if (ret == 0) + { + ctrl_reg1.yen = val; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG1, + (uint8_t *)&ctrl_reg1, 1); + } + + return ret; +} + +/// @brief Y axis enable/disable.[get] +/// @param val change the values of yen in reg CTRL_REG1 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_axis_y_data_get(uint8_t *val) +{ + iis328dq_ctrl_reg1_t ctrl_reg1; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG1, + (uint8_t *)&ctrl_reg1, 1); + *val = ctrl_reg1.yen; + + return ret; +} + +/// @brief Z axis enable/disable.[set] +/// @param val change the values of zen in reg CTRL_REG1 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_axis_z_data_set(uint8_t val) +{ + iis328dq_ctrl_reg1_t ctrl_reg1; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG1, + (uint8_t *)&ctrl_reg1, 1); + + if (ret == 0) + { + ctrl_reg1.zen = val; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG1, + (uint8_t *)&ctrl_reg1, 1); + } + + return ret; +} + +/// @brief Z axis enable/disable.[get] +/// @param val change the values of zen in reg CTRL_REG1 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_axis_z_data_get(uint8_t *val) +{ + iis328dq_ctrl_reg1_t ctrl_reg1; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG1, + (uint8_t *)&ctrl_reg1, 1); + *val = ctrl_reg1.zen; + + return ret; +} + +/// @brief Accelerometer data rate selection.[set] +/// @param val change the values of dr in reg CTRL_REG1 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_data_rate_set(iis328dq_dr_t val) +{ + iis328dq_ctrl_reg1_t ctrl_reg1; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG1, + (uint8_t *)&ctrl_reg1, 1); + + if (ret == 0) + { + ctrl_reg1.pm = (uint8_t)val & 0x07U; + ctrl_reg1.dr = ((uint8_t)val & 0x30U) >> 4; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG1, + (uint8_t *)&ctrl_reg1, 1); + } + + return ret; +} + +/// @brief Accelerometer data rate selection.[get] +/// @param val Get the values of dr in reg CTRL_REG1 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_data_rate_get(iis328dq_dr_t *val) +{ + iis328dq_ctrl_reg1_t ctrl_reg1; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG1, + (uint8_t *)&ctrl_reg1, 1); + + switch ((ctrl_reg1.dr << 4) + ctrl_reg1.pm) + { + case IIS328DQ_ODR_OFF: + *val = IIS328DQ_ODR_OFF; + break; + + case IIS328DQ_ODR_Hz5: + *val = IIS328DQ_ODR_Hz5; + break; + + case IIS328DQ_ODR_1Hz: + *val = IIS328DQ_ODR_1Hz; + break; + + case IIS328DQ_ODR_5Hz2: + *val = IIS328DQ_ODR_5Hz2; + break; + + case IIS328DQ_ODR_5Hz: + *val = IIS328DQ_ODR_5Hz; + break; + + case IIS328DQ_ODR_10Hz: + *val = IIS328DQ_ODR_10Hz; + break; + + case IIS328DQ_ODR_50Hz: + *val = IIS328DQ_ODR_50Hz; + break; + + case IIS328DQ_ODR_100Hz: + *val = IIS328DQ_ODR_100Hz; + break; + + case IIS328DQ_ODR_400Hz: + *val = IIS328DQ_ODR_400Hz; + break; + + case IIS328DQ_ODR_1kHz: + *val = IIS328DQ_ODR_1kHz; + break; + + default: + *val = IIS328DQ_ODR_OFF; + break; + } + + return ret; +} + +/// @brief High pass filter mode selection.[set] +/// @param val change the values of hpm in reg CTRL_REG2 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_reference_mode_set(iis328dq_hpm_t val) +{ + iis328dq_ctrl_reg2_t ctrl_reg2; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG2, + (uint8_t *)&ctrl_reg2, 1); + + if (ret == 0) + { + ctrl_reg2.hpm = (uint8_t)val; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG2, + (uint8_t *)&ctrl_reg2, 1); + } + + return ret; +} + +/// @brief High pass filter mode selection.[get] +/// @param val Get the values of hpm in reg CTRL_REG2 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_reference_mode_get(iis328dq_hpm_t *val) +{ + iis328dq_ctrl_reg2_t ctrl_reg2; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG2, + (uint8_t *)&ctrl_reg2, 1); + + switch (ctrl_reg2.hpm) + { + case IIS328DQ_NORMAL_MODE: + *val = IIS328DQ_NORMAL_MODE; + break; + + case IIS328DQ_REF_MODE_ENABLE: + *val = IIS328DQ_REF_MODE_ENABLE; + break; + + default: + *val = IIS328DQ_NORMAL_MODE; + break; + } + + return ret; +} + +/// @brief Accelerometer full-scale selection.[set] +/// @param val change the values of fs in reg CTRL_REG4 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_full_scale_set(iis328dq_fs_t val) +{ + iis328dq_ctrl_reg4_t ctrl_reg4; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG4, + (uint8_t *)&ctrl_reg4, 1); + + if (ret == 0) + { + ctrl_reg4.fs = (uint8_t)val; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG4, + (uint8_t *)&ctrl_reg4, 1); + } + + return ret; +} + +/// @brief Accelerometer full-scale selection.[get] +/// @param val Get the values of fs in reg CTRL_REG4 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_full_scale_get(iis328dq_fs_t *val) +{ + iis328dq_ctrl_reg4_t ctrl_reg4; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG4, + (uint8_t *)&ctrl_reg4, 1); + + switch (ctrl_reg4.fs) + { + case IIS328DQ_2g: + *val = IIS328DQ_2g; + break; + + case IIS328DQ_4g: + *val = IIS328DQ_4g; + break; + + case IIS328DQ_8g: + *val = IIS328DQ_8g; + break; + + default: + *val = IIS328DQ_2g; + break; + } + + return ret; +} + +/// @brief Block data update.[set] +/// @param val change the values of bdu in reg CTRL_REG4 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_block_data_update_set(uint8_t val) +{ + iis328dq_ctrl_reg4_t ctrl_reg4; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG4, + (uint8_t *)&ctrl_reg4, 1); + + if (ret == 0) + { + ctrl_reg4.bdu = val; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG4, + (uint8_t *)&ctrl_reg4, 1); + } + + return ret; +} + +/// @brief Block data update.[get] +/// @param val change the values of bdu in reg CTRL_REG4 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_block_data_update_get(uint8_t *val) +{ + iis328dq_ctrl_reg4_t ctrl_reg4; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG4, + (uint8_t *)&ctrl_reg4, 1); + *val = ctrl_reg4.bdu; + + return ret; +} + +/// @brief The STATUS_REG register is read by the interface.[get] +/// @param val registers STATUS_REG +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_status_reg_get(iis328dq_status_reg_t *val) +{ + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_STATUS_REG, (uint8_t *) val, 1); + + return ret; +} + +/// @brief Accelerometer new data available.[get] +/// @param val change the values of zyxda in reg STATUS_REG +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_flag_data_ready_get(uint8_t *val) +{ + iis328dq_status_reg_t status_reg; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_STATUS_REG, + (uint8_t *)&status_reg, 1); + *val = status_reg.zyxda; + + return ret; +} + +/** + * @} + */ + +/** + * @defgroup IIS328DQ_Data_Output IIS328DQ_Data_Output + * @brief This section groups all the data output functions. + * @{ + */ + +/// @brief Linear acceleration output register. The value is expressed +/// as a 16-bit word in two’s complement.[get] +/// @param val data read +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_acceleration_raw_get(int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_OUT_X_L, buff, 6); + val[0] = (int16_t)buff[1]; + val[0] = (val[0] * 256) + (int16_t)buff[0]; + val[1] = (int16_t)buff[3]; + val[1] = (val[1] * 256) + (int16_t)buff[2]; + val[2] = (int16_t)buff[5]; + val[2] = (val[2] * 256) + (int16_t)buff[4]; + + return ret; +} + +/** + * @} + */ + +/** + * @defgroup IIS328DQ_Common IIS328DQ_Common + * @brief This section groups common useful functions. + * @{ + */ + +/// @brief Device Who am I.[get] +/// @param buff buffer that stores data read +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_device_id_get(uint8_t *buff) +{ + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_WHO_AM_I, buff, 1); + + return ret; +} + +/// @brief Reboot memory content. Reload the calibration parameters.[set] +/// @param val change the values of boot in reg CTRL_REG2 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_boot_set(uint8_t val) +{ + iis328dq_ctrl_reg2_t ctrl_reg2; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG2, + (uint8_t *)&ctrl_reg2, 1); + + if (ret == 0) + { + ctrl_reg2.boot = val; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG2, + (uint8_t *)&ctrl_reg2, 1); + } + + return ret; +} + +/// @brief Reboot memory content. Reload the calibration parameters.[get] +/// @param val change the values of boot in reg CTRL_REG2 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_boot_get(uint8_t *val) +{ + iis328dq_ctrl_reg2_t ctrl_reg2; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG2, + (uint8_t *)&ctrl_reg2, 1); + *val = ctrl_reg2.boot; + + return ret; +} + +/// @brief Linear acceleration sensor self-test enable.[set] +/// @param val change the values of st in reg CTRL_REG4 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_self_test_set(iis328dq_st_t val) +{ + iis328dq_ctrl_reg4_t ctrl_reg4; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG4, + (uint8_t *)&ctrl_reg4, 1); + + if (ret == 0) + { + ctrl_reg4.st = (uint8_t)val; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG4, + (uint8_t *)&ctrl_reg4, 1); + } + + return ret; +} + +/// @brief Linear acceleration sensor self-test enable.[get] +/// @param val Get the values of st in reg CTRL_REG4 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_self_test_get(iis328dq_st_t *val) +{ + iis328dq_ctrl_reg4_t ctrl_reg4; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG4, + (uint8_t *)&ctrl_reg4, 1); + + switch (ctrl_reg4.st) + { + case IIS328DQ_ST_DISABLE: + *val = IIS328DQ_ST_DISABLE; + break; + + case IIS328DQ_ST_POSITIVE: + *val = IIS328DQ_ST_POSITIVE; + break; + + case IIS328DQ_ST_NEGATIVE: + *val = IIS328DQ_ST_NEGATIVE; + break; + + default: + *val = IIS328DQ_ST_DISABLE; + break; + } + + return ret; +} + +/// @brief Big/Little Endian Data selection.[set] +/// @param val change the values of ble in reg CTRL_REG4 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_data_format_set(iis328dq_ble_t val) +{ + iis328dq_ctrl_reg4_t ctrl_reg4; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG4, + (uint8_t *)&ctrl_reg4, 1); + + if (ret == 0) + { + ctrl_reg4.ble = (uint8_t)val; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG4, + (uint8_t *)&ctrl_reg4, 1); + } + + return ret; +} + +/// @brief Big/Little Endian Data selection.[get] +/// @param val Get the values of ble in reg CTRL_REG4 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_data_format_get(iis328dq_ble_t *val) +{ + iis328dq_ctrl_reg4_t ctrl_reg4; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG4, + (uint8_t *)&ctrl_reg4, 1); + + switch (ctrl_reg4.ble) + { + case IIS328DQ_LSB_AT_LOW_ADD: + *val = IIS328DQ_LSB_AT_LOW_ADD; + break; + + case IIS328DQ_MSB_AT_LOW_ADD: + *val = IIS328DQ_MSB_AT_LOW_ADD; + break; + + default: + *val = IIS328DQ_LSB_AT_LOW_ADD; + break; + } + + return ret; +} + +/** + * @} + */ + +/** + * @defgroup IIS328DQ_Filters IIS328DQ_Filters + * @brief This section group all the functions concerning the filters configuration. + * @{ + */ + +/// @brief High pass filter cut-off frequency configuration.[set] +/// @param val change the values of hpcf in reg CTRL_REG2 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_hp_bandwidth_set(iis328dq_hpcf_t val) +{ + iis328dq_ctrl_reg2_t ctrl_reg2; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG2, + (uint8_t *)&ctrl_reg2, 1); + + if (ret == 0) + { + ctrl_reg2.hpcf = (uint8_t)val; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG2, + (uint8_t *)&ctrl_reg2, 1); + } + + return ret; +} + +/// @brief High pass filter cut-off frequency configuration.[get] +/// @param val Get the values of hpcf in reg CTRL_REG2 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_hp_bandwidth_get(iis328dq_hpcf_t *val) +{ + iis328dq_ctrl_reg2_t ctrl_reg2; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG2, + (uint8_t *)&ctrl_reg2, 1); + + switch (ctrl_reg2.hpcf) + { + case IIS328DQ_CUT_OFF_8Hz: + *val = IIS328DQ_CUT_OFF_8Hz; + break; + + case IIS328DQ_CUT_OFF_16Hz: + *val = IIS328DQ_CUT_OFF_16Hz; + break; + + case IIS328DQ_CUT_OFF_32Hz: + *val = IIS328DQ_CUT_OFF_32Hz; + break; + + case IIS328DQ_CUT_OFF_64Hz: + *val = IIS328DQ_CUT_OFF_64Hz; + break; + + default: + *val = IIS328DQ_CUT_OFF_8Hz; + break; + } + + return ret; +} + +/// @brief Select High Pass filter path.[set] +/// @param val change the values of hpen in reg CTRL_REG2 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_hp_path_set(iis328dq_hpen_t val) +{ + iis328dq_ctrl_reg2_t ctrl_reg2; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG2, + (uint8_t *)&ctrl_reg2, 1); + + if (ret == 0) + { + ctrl_reg2.hpen = (uint8_t)val & 0x03U; + ctrl_reg2.fds = ((uint8_t)val & 0x04U) >> 2; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG2, + (uint8_t *)&ctrl_reg2, 1); + } + + return ret; +} + +/// @brief Select High Pass filter path.[get] +/// @param val Get the values of hpen in reg CTRL_REG2 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_hp_path_get(iis328dq_hpen_t *val) +{ + iis328dq_ctrl_reg2_t ctrl_reg2; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG2, + (uint8_t *)&ctrl_reg2, 1); + + switch ((ctrl_reg2.fds << 2) + ctrl_reg2.hpen) + { + case IIS328DQ_HP_DISABLE: + *val = IIS328DQ_HP_DISABLE; + break; + + case IIS328DQ_HP_ON_OUT: + *val = IIS328DQ_HP_ON_OUT; + break; + + case IIS328DQ_HP_ON_INT1: + *val = IIS328DQ_HP_ON_INT1; + break; + + case IIS328DQ_HP_ON_INT2: + *val = IIS328DQ_HP_ON_INT2; + break; + + case IIS328DQ_HP_ON_INT1_INT2: + *val = IIS328DQ_HP_ON_INT1_INT2; + break; + + case IIS328DQ_HP_ON_INT1_INT2_OUT: + *val = IIS328DQ_HP_ON_INT1_INT2_OUT; + break; + + case IIS328DQ_HP_ON_INT2_OUT: + *val = IIS328DQ_HP_ON_INT2_OUT; + break; + + case IIS328DQ_HP_ON_INT1_OUT: + *val = IIS328DQ_HP_ON_INT1_OUT; + break; + + default: + *val = IIS328DQ_HP_DISABLE; + break; + } + + return ret; +} + +/// @brief Reading at this address zeroes instantaneously +/// the content of the internal high pass-filter. +/// If the high pass filter is enabled all three axes +/// are instantaneously set to 0g. This allows to +/// overcome the settling time of the high pass +/// filter.[get] +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_hp_reset_get(void) +{ + uint8_t dummy; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_HP_FILTER_RESET, + (uint8_t *)&dummy, 1); + + return ret; +} + +/// @brief Reference value for high-pass filter.[set] +/// @param val change the values of ref in reg REFERENCE +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_hp_reference_value_set(uint8_t val) +{ + int32_t ret; + + ret = iis328dq_write_reg(IIS328DQ_REFERENCE, (uint8_t *)&val, 1); + + return ret; +} + +/// @brief Reference value for high-pass filter.[get] +/// @param val change the values of ref in reg REFERENCE +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_hp_reference_value_get(uint8_t *val) +{ + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_REFERENCE, val, 1); + + return ret; +} + +/** + * @} + */ + +/** + * @defgroup IIS328DQ_Serial_Interface IIS328DQ_Serial_Interface + * @brief This section groups all the functions concerning serial interface management. + * @{ + */ + +/// @brief SPI 3- or 4-wire interface.[set] +/// @param val change the values of sim in reg CTRL_REG4 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_spi_mode_set(iis328dq_sim_t val) +{ + iis328dq_ctrl_reg4_t ctrl_reg4; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG4, + (uint8_t *)&ctrl_reg4, 1); + + if (ret == 0) + { + ctrl_reg4.sim = (uint8_t)val; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG4, + (uint8_t *)&ctrl_reg4, 1); + } + + return ret; +} + +/// @brief SPI 3- or 4-wire interface.[get] +/// @param val Get the values of sim in reg CTRL_REG4 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_spi_mode_get(iis328dq_sim_t *val) +{ + iis328dq_ctrl_reg4_t ctrl_reg4; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG4, + (uint8_t *)&ctrl_reg4, 1); + + switch (ctrl_reg4.sim) + { + case IIS328DQ_SPI_4_WIRE: + *val = IIS328DQ_SPI_4_WIRE; + break; + + case IIS328DQ_SPI_3_WIRE: + *val = IIS328DQ_SPI_3_WIRE; + break; + + default: + *val = IIS328DQ_SPI_4_WIRE; + break; + } + + return ret; +} + +/** + * @} + */ + +/** + * @defgroup IIS328DQ_Interrupt_Pins IIS328DQ_Interrupt_Pins + * @brief This section groups all the functions that manage interrupt pins. + * @{ + */ + +/// @brief Data signal on INT 1 pad control bits.[set] +/// @param val change the values of i1_cfg in reg CTRL_REG3 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_pin_int1_route_set(iis328dq_i1_cfg_t val) +{ + iis328dq_ctrl_reg3_t ctrl_reg3; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG3, + (uint8_t *)&ctrl_reg3, 1); + + if (ret == 0) + { + ctrl_reg3.i1_cfg = (uint8_t)val; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG3, + (uint8_t *)&ctrl_reg3, 1); + } + + return ret; +} + +/// @brief Data signal on INT 1 pad control bits.[get] +/// @param val Get the values of i1_cfg in reg CTRL_REG3 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_pin_int1_route_get(iis328dq_i1_cfg_t *val) +{ + iis328dq_ctrl_reg3_t ctrl_reg3; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG3, + (uint8_t *)&ctrl_reg3, 1); + + switch (ctrl_reg3.i1_cfg) + { + case IIS328DQ_PAD1_INT1_SRC: + *val = IIS328DQ_PAD1_INT1_SRC; + break; + + case IIS328DQ_PAD1_INT1_OR_INT2_SRC: + *val = IIS328DQ_PAD1_INT1_OR_INT2_SRC; + break; + + case IIS328DQ_PAD1_DRDY: + *val = IIS328DQ_PAD1_DRDY; + break; + + case IIS328DQ_PAD1_BOOT: + *val = IIS328DQ_PAD1_BOOT; + break; + + default: + *val = IIS328DQ_PAD1_INT1_SRC; + break; + } + + return ret; +} + +/// @brief Latch interrupt request on INT1_SRC register, with INT1_SRC +/// register cleared by reading INT1_SRC register.[set] +/// @param val change the values of lir1 in reg CTRL_REG3 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int1_notification_set(iis328dq_lir1_t val) +{ + iis328dq_ctrl_reg3_t ctrl_reg3; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG3, + (uint8_t *)&ctrl_reg3, 1); + + if (ret == 0) + { + ctrl_reg3.lir1 = (uint8_t)val; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG3, + (uint8_t *)&ctrl_reg3, 1); + } + + return ret; +} + +/// @brief Latch interrupt request on INT1_SRC register, with INT1_SRC +/// register cleared by reading INT1_SRC register.[get] +/// @param val Get the values of lir1 in reg CTRL_REG3 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int1_notification_get(iis328dq_lir1_t *val) +{ + iis328dq_ctrl_reg3_t ctrl_reg3; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG3, + (uint8_t *)&ctrl_reg3, 1); + + switch (ctrl_reg3.lir1) + { + case IIS328DQ_INT1_PULSED: + *val = IIS328DQ_INT1_PULSED; + break; + + case IIS328DQ_INT1_LATCHED: + *val = IIS328DQ_INT1_LATCHED; + break; + + default: + *val = IIS328DQ_INT1_PULSED; + break; + } + + return ret; +} + +/// @brief Data signal on INT 2 pad control bits.[set] +/// @param val change the values of i2_cfg in reg CTRL_REG3 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_pin_int2_route_set(iis328dq_i2_cfg_t val) +{ + iis328dq_ctrl_reg3_t ctrl_reg3; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG3, + (uint8_t *)&ctrl_reg3, 1); + + if (ret == 0) + { + ctrl_reg3.i2_cfg = (uint8_t)val; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG3, + (uint8_t *)&ctrl_reg3, 1); + } + + return ret; +} + +/// @brief Data signal on INT 2 pad control bits.[get] +/// @param val Get the values of i2_cfg in reg CTRL_REG3 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_pin_int2_route_get(iis328dq_i2_cfg_t *val) +{ + iis328dq_ctrl_reg3_t ctrl_reg3; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG3, + (uint8_t *)&ctrl_reg3, 1); + + switch (ctrl_reg3.i2_cfg) + { + case IIS328DQ_PAD2_INT2_SRC: + *val = IIS328DQ_PAD2_INT2_SRC; + break; + + case IIS328DQ_PAD2_INT1_OR_INT2_SRC: + *val = IIS328DQ_PAD2_INT1_OR_INT2_SRC; + break; + + case IIS328DQ_PAD2_DRDY: + *val = IIS328DQ_PAD2_DRDY; + break; + + case IIS328DQ_PAD2_BOOT: + *val = IIS328DQ_PAD2_BOOT; + break; + + default: + *val = IIS328DQ_PAD2_INT2_SRC; + break; + } + + return ret; +} + +/// @brief Latch interrupt request on INT2_SRC register, with INT2_SRC +/// register cleared by reading INT2_SRC itself.[set] +/// @param val change the values of lir2 in reg CTRL_REG3 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int2_notification_set(iis328dq_lir2_t val) +{ + iis328dq_ctrl_reg3_t ctrl_reg3; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG3, + (uint8_t *)&ctrl_reg3, 1); + + if (ret == 0) + { + ctrl_reg3.lir2 = (uint8_t)val; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG3, + (uint8_t *)&ctrl_reg3, 1); + } + + return ret; +} + +/// @brief Latch interrupt request on INT2_SRC register, with INT2_SRC +/// register cleared by reading INT2_SRC itself.[get] +/// @param val Get the values of lir2 in reg CTRL_REG3 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int2_notification_get(iis328dq_lir2_t *val) +{ + iis328dq_ctrl_reg3_t ctrl_reg3; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG3, + (uint8_t *)&ctrl_reg3, 1); + + switch (ctrl_reg3.lir2) + { + case IIS328DQ_INT2_PULSED: + *val = IIS328DQ_INT2_PULSED; + break; + + case IIS328DQ_INT2_LATCHED: + *val = IIS328DQ_INT2_LATCHED; + break; + + default: + *val = IIS328DQ_INT2_PULSED; + break; + } + + return ret; +} + +/// @brief Push-pull/open drain selection on interrupt pads.[set] +/// @param val change the values of pp_od in reg CTRL_REG3 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_pin_mode_set(iis328dq_pp_od_t val) +{ + iis328dq_ctrl_reg3_t ctrl_reg3; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG3, + (uint8_t *)&ctrl_reg3, 1); + + if (ret == 0) + { + ctrl_reg3.pp_od = (uint8_t)val; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG3, + (uint8_t *)&ctrl_reg3, 1); + } + + return ret; +} + +/// @brief Push-pull/open drain selection on interrupt pads.[get] +/// @param val Get the values of pp_od in reg CTRL_REG3 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_pin_mode_get(iis328dq_pp_od_t *val) +{ + iis328dq_ctrl_reg3_t ctrl_reg3; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG3, + (uint8_t *)&ctrl_reg3, 1); + + switch (ctrl_reg3.pp_od) + { + case IIS328DQ_PUSH_PULL: + *val = IIS328DQ_PUSH_PULL; + break; + + case IIS328DQ_OPEN_DRAIN: + *val = IIS328DQ_OPEN_DRAIN; + break; + + default: + *val = IIS328DQ_PUSH_PULL; + break; + } + + return ret; +} + +/// @brief Interrupt active-high/low.[set] +/// @param val change the values of ihl in reg CTRL_REG3 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_pin_polarity_set(iis328dq_ihl_t val) +{ + iis328dq_ctrl_reg3_t ctrl_reg3; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG3, + (uint8_t *)&ctrl_reg3, 1); + + if (ret == 0) + { + ctrl_reg3.ihl = (uint8_t)val; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG3, + (uint8_t *)&ctrl_reg3, 1); + } + + return ret; +} + +/// @brief Interrupt active-high/low.[get] +/// @param val Get the values of ihl in reg CTRL_REG3 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_pin_polarity_get(iis328dq_ihl_t *val) +{ + iis328dq_ctrl_reg3_t ctrl_reg3; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG3, + (uint8_t *)&ctrl_reg3, 1); + + switch (ctrl_reg3.ihl) + { + case IIS328DQ_ACTIVE_HIGH: + *val = IIS328DQ_ACTIVE_HIGH; + break; + + case IIS328DQ_ACTIVE_LOW: + *val = IIS328DQ_ACTIVE_LOW; + break; + + default: + *val = IIS328DQ_ACTIVE_HIGH; + break; + } + + return ret; +} + +/** + * @} + */ + +/** + * @defgroup IIS328DQ_interrupt_on_threshold IIS328DQ_interrupt_on_threshold + * @brief This section groups all the functions that manage the interrupt on threshold event generation. + * @{ + */ + +/// @brief Configure the interrupt 1 threshold sign.[set] +/// @param val enable sign and axis for interrupt on threshold +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int1_on_threshold_conf_set(int1_on_th_conf_t val) +{ + iis328dq_int1_cfg_t int1_cfg; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT1_CFG, (uint8_t *)&int1_cfg, 1); + + if (ret == 0) + { + int1_cfg.xlie = val.int1_xlie; + int1_cfg.xhie = val.int1_xhie; + int1_cfg.ylie = val.int1_ylie; + int1_cfg.yhie = val.int1_yhie; + int1_cfg.zlie = val.int1_zlie; + int1_cfg.zhie = val.int1_zhie; + ret = iis328dq_write_reg(IIS328DQ_INT1_CFG, + (uint8_t *)&int1_cfg, 1); + } + + return ret; +} + +/// @brief Configure the interrupt 1 threshold sign.[get] +/// @param val enable sign and axis for interrupt on threshold +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int1_on_threshold_conf_get(int1_on_th_conf_t *val) +{ + iis328dq_int1_cfg_t int1_cfg; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT1_CFG, (uint8_t *)&int1_cfg, 1); + val->int1_xlie = int1_cfg.xlie; + val->int1_xhie = int1_cfg.xhie; + val->int1_ylie = int1_cfg.ylie; + val->int1_yhie = int1_cfg.yhie; + val->int1_zlie = int1_cfg.zlie; + val->int1_zhie = int1_cfg.zhie; + + return ret; +} + +/// @brief AND/OR combination of Interrupt 1 events.[set] +/// @param val change the values of aoi in reg INT1_CFG +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int1_on_threshold_mode_set(iis328dq_int1_aoi_t val) +{ + iis328dq_int1_cfg_t int1_cfg; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT1_CFG, (uint8_t *)&int1_cfg, 1); + + if (ret == 0) + { + int1_cfg.aoi = (uint8_t) val; + ret = iis328dq_write_reg(IIS328DQ_INT1_CFG, + (uint8_t *)&int1_cfg, 1); + } + + return ret; +} + +/// @brief AND/OR combination of Interrupt 1 events.[get] +/// @param val Get the values of aoi in reg INT1_CFG +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int1_on_threshold_mode_get(iis328dq_int1_aoi_t *val) +{ + iis328dq_int1_cfg_t int1_cfg; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT1_CFG, (uint8_t *)&int1_cfg, 1); + + switch (int1_cfg.aoi) + { + case IIS328DQ_INT1_ON_THRESHOLD_OR: + *val = IIS328DQ_INT1_ON_THRESHOLD_OR; + break; + + case IIS328DQ_INT1_ON_THRESHOLD_AND: + *val = IIS328DQ_INT1_ON_THRESHOLD_AND; + break; + + default: + *val = IIS328DQ_INT1_ON_THRESHOLD_OR; + break; + } + + return ret; +} + +/// @brief Interrupt generator 1 on threshold source register.[get] +/// @param val registers INT1_SRC +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int1_src_get(iis328dq_int1_src_t *val) +{ + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT1_SRC, (uint8_t *) val, 1); + + return ret; +} + +/// @brief Interrupt 1 threshold.[set] +/// @param val change the values of ths in reg INT1_THS +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int1_treshold_set(uint8_t val) +{ + iis328dq_int1_ths_t int1_ths; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT1_THS, (uint8_t *)&int1_ths, 1); + + if (ret == 0) + { + int1_ths.ths = val; + ret = iis328dq_write_reg(IIS328DQ_INT1_THS, + (uint8_t *)&int1_ths, 1); + } + + return ret; +} + +/// @brief Interrupt 1 threshold.[get] +/// @param val change the values of ths in reg INT1_THS +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int1_treshold_get(uint8_t *val) +{ + iis328dq_int1_ths_t int1_ths; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT1_THS, (uint8_t *)&int1_ths, 1); + *val = int1_ths.ths; + + return ret; +} + +/// @brief Duration value for interrupt 1 generator.[set] +/// @param val change the values of d in reg INT1_DURATION +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int1_dur_set(uint8_t val) +{ + iis328dq_int1_duration_t int1_duration; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT1_DURATION, + (uint8_t *)&int1_duration, 1); + + if (ret == 0) + { + int1_duration.d = val; + ret = iis328dq_write_reg(IIS328DQ_INT1_DURATION, + (uint8_t *)&int1_duration, 1); + } + + return ret; +} + +/// @brief Duration value for interrupt 1 generator.[get] +/// @param val change the values of d in reg INT1_DURATION +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int1_dur_get(uint8_t *val) +{ + iis328dq_int1_duration_t int1_duration; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT1_DURATION, + (uint8_t *)&int1_duration, 1); + *val = int1_duration.d; + + return ret; +} + +/// @brief Configure the interrupt 2 threshold sign.[set] +/// @param val enable sign and axis for interrupt on threshold +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int2_on_threshold_conf_set(int2_on_th_conf_t val) +{ + iis328dq_int2_cfg_t int2_cfg; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT2_CFG, + (uint8_t *)&int2_cfg, 1); + + if (ret == 0) + { + int2_cfg.xlie = val.int2_xlie; + int2_cfg.xhie = val.int2_xhie; + int2_cfg.ylie = val.int2_ylie; + int2_cfg.yhie = val.int2_yhie; + int2_cfg.zlie = val.int2_zlie; + int2_cfg.zhie = val.int2_zhie; + ret = iis328dq_write_reg(IIS328DQ_INT2_CFG, + (uint8_t *)&int2_cfg, 1); + } + + return ret; +} + +/// @brief Configure the interrupt 2 threshold sign.[get] +/// @param val enable sign and axis for interrupt on threshold +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int2_on_threshold_conf_get(int2_on_th_conf_t *val) +{ + iis328dq_int2_cfg_t int2_cfg; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT2_CFG, (uint8_t *)&int2_cfg, 1); + val->int2_xlie = int2_cfg.xlie; + val->int2_xhie = int2_cfg.xhie; + val->int2_ylie = int2_cfg.ylie; + val->int2_yhie = int2_cfg.yhie; + val->int2_zlie = int2_cfg.zlie; + val->int2_zhie = int2_cfg.zhie; + + return ret; +} + +/// @brief AND/OR combination of Interrupt 2 events.[set] +/// @param val change the values of aoi in reg INT2_CFG +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int2_on_threshold_mode_set(iis328dq_int2_aoi_t val) +{ + iis328dq_int2_cfg_t int2_cfg; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT2_CFG, (uint8_t *)&int2_cfg, 1); + + if (ret == 0) + { + int2_cfg.aoi = (uint8_t) val; + ret = iis328dq_write_reg(IIS328DQ_INT2_CFG, + (uint8_t *)&int2_cfg, 1); + } + + return ret; +} + +/// @brief AND/OR combination of Interrupt 2 events.[get] +/// @param val Get the values of aoi in reg INT2_CFG +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int2_on_threshold_mode_get(iis328dq_int2_aoi_t *val) +{ + iis328dq_int2_cfg_t int2_cfg; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT2_CFG, (uint8_t *)&int2_cfg, 1); + + switch (int2_cfg.aoi) + { + case IIS328DQ_INT2_ON_THRESHOLD_OR: + *val = IIS328DQ_INT2_ON_THRESHOLD_OR; + break; + + case IIS328DQ_INT2_ON_THRESHOLD_AND: + *val = IIS328DQ_INT2_ON_THRESHOLD_AND; + break; + + default: + *val = IIS328DQ_INT2_ON_THRESHOLD_OR; + break; + } + + return ret; +} + +/// @brief Interrupt generator 1 on threshold source register.[get] +/// @param val registers INT2_SRC +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int2_src_get(iis328dq_int2_src_t *val) +{ + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT2_SRC, (uint8_t *) val, 1); + + return ret; +} + +/// @brief Interrupt 2 threshold.[set] +/// @param val change the values of ths in reg INT2_THS +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int2_treshold_set(uint8_t val) +{ + iis328dq_int2_ths_t int2_ths; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT2_THS, (uint8_t *)&int2_ths, 1); + + if (ret == 0) + { + int2_ths.ths = val; + ret = iis328dq_write_reg(IIS328DQ_INT2_THS, + (uint8_t *)&int2_ths, 1); + } + + return ret; +} + +/// @brief Interrupt 2 threshold.[get] +/// @param val change the values of ths in reg INT2_THS +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int2_treshold_get(uint8_t *val) +{ + iis328dq_int2_ths_t int2_ths; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT2_THS, (uint8_t *)&int2_ths, 1); + *val = int2_ths.ths; + + return ret; +} + +/// @brief Duration value for interrupt 2 generator.[set] +/// @param val change the values of d in reg INT2_DURATION +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int2_dur_set(uint8_t val) +{ + iis328dq_int2_duration_t int2_duration; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT2_DURATION, + (uint8_t *)&int2_duration, 1); + + if (ret == 0) + { + int2_duration.d = val; + ret = iis328dq_write_reg(IIS328DQ_INT2_DURATION, + (uint8_t *)&int2_duration, 1); + } + + return ret; +} + +/// @brief Duration value for interrupt 2 generator.[get] +/// @param val change the values of d in reg INT2_DURATION +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int2_dur_get(uint8_t *val) +{ + iis328dq_int2_duration_t int2_duration; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT2_DURATION, + (uint8_t *)&int2_duration, 1); + *val = int2_duration.d; + + return ret; +} + +/** + * @} + */ + +/** + * @defgroup IIS328DQ_Wake_Up_Event IIS328DQ_Wake_Up_Event + * @brief This section groups all the functions that manage the Wake Up event generation. + * @{ + */ + +/// @brief Turn-on mode selection for sleep to wake function.[set] +/// @param val change the values of turnon in reg CTRL_REG5 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_wkup_to_sleep_set(uint8_t val) +{ + iis328dq_ctrl_reg5_t ctrl_reg5; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG5, + (uint8_t *)&ctrl_reg5, 1); + + if (ret == 0) + { + ctrl_reg5.turnon = val; + ret = iis328dq_write_reg(IIS328DQ_CTRL_REG5, + (uint8_t *)&ctrl_reg5, 1); + } + + return ret; +} + +/// @brief Turn-on mode selection for sleep to wake function.[get] +/// @param val change the values of turnon in reg CTRL_REG5 +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_wkup_to_sleep_get(uint8_t *val) +{ + iis328dq_ctrl_reg5_t ctrl_reg5; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_CTRL_REG5, + (uint8_t *)&ctrl_reg5, 1); + *val = ctrl_reg5.turnon; + + return ret; +} + +/** + * @} + */ + +/** + * @defgroup IIS328DQ_Six_Position_Detection IIS328DQ_Six_Position_Detection + * @brief This section groups all the functions concerning six position detection (6D). + * @{ + */ + +/// @brief Configure the 6d on interrupt 1 generator.[set] +/// @param val change the values of 6d in reg INT1_CFG +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int1_6d_mode_set(iis328dq_int1_6d_t val) +{ + iis328dq_int1_cfg_t int1_cfg; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT1_CFG, (uint8_t *)&int1_cfg, 1); + + if (ret == 0) + { + int1_cfg._6d = (uint8_t)val & 0x01U; + int1_cfg.aoi = ((uint8_t)val & 0x02U) >> 1; + ret = iis328dq_write_reg(IIS328DQ_INT1_CFG, (uint8_t *)&int1_cfg, 1); + } + + return ret; +} + +/// @brief Configure the 6d on interrupt 1 generator.[get] +/// @param val Get the values of 6d in reg INT1_CFG +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int1_6d_mode_get(iis328dq_int1_6d_t *val) +{ + iis328dq_int1_cfg_t int1_cfg; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT1_CFG, (uint8_t *)&int1_cfg, 1); + + switch ((int1_cfg.aoi << 1) + int1_cfg._6d) + { + case IIS328DQ_6D_INT1_DISABLE: + *val = IIS328DQ_6D_INT1_DISABLE; + break; + + case IIS328DQ_6D_INT1_MOVEMENT: + *val = IIS328DQ_6D_INT1_MOVEMENT; + break; + + case IIS328DQ_6D_INT1_POSITION: + *val = IIS328DQ_6D_INT1_POSITION; + break; + + default: + *val = IIS328DQ_6D_INT1_DISABLE; + break; + } + + return ret; +} + +/// @brief 6D on interrupt generator 1 source register.[get] +/// @param val registers INT1_SRC +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int1_6d_src_get(iis328dq_int1_src_t *val) +{ + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT1_SRC, (uint8_t *) val, 1); + + return ret; +} + +/// @brief Interrupt 1 threshold.[set] +/// @param val change the values of ths in reg INT1_THS +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int1_6d_treshold_set(uint8_t val) +{ + iis328dq_int1_ths_t int1_ths; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT1_THS, (uint8_t *)&int1_ths, 1); + + if (ret == 0) + { + int1_ths.ths = val; + ret = iis328dq_write_reg(IIS328DQ_INT1_THS, (uint8_t *)&int1_ths, 1); + } + + return ret; +} + +/// @brief Interrupt 1 threshold.[get] +/// @param val change the values of ths in reg INT1_THS +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int1_6d_treshold_get(uint8_t *val) +{ + iis328dq_int1_ths_t int1_ths; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT1_THS, (uint8_t *)&int1_ths, 1); + *val = int1_ths.ths; + + return ret; +} + +/// @brief Configure the 6d on interrupt 2 generator.[set] +/// @param val change the values of 6d in reg INT2_CFG +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int2_6d_mode_set(iis328dq_int2_6d_t val) +{ + iis328dq_int2_cfg_t int2_cfg; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT2_CFG, (uint8_t *)&int2_cfg, 1); + + if (ret == 0) + { + int2_cfg._6d = (uint8_t)val & 0x01U; + int2_cfg.aoi = ((uint8_t)val & 0x02U) >> 1; + ret = iis328dq_write_reg(IIS328DQ_INT2_CFG, + (uint8_t *)&int2_cfg, 1); + } + + return ret; +} + +/// @brief Configure the 6d on interrupt 2 generator.[get] +/// @param val Get the values of 6d in reg INT2_CFG +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int2_6d_mode_get(iis328dq_int2_6d_t *val) +{ + iis328dq_int2_cfg_t int2_cfg; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT2_CFG, (uint8_t *)&int2_cfg, 1); + + switch ((int2_cfg.aoi << 1) + int2_cfg._6d) + { + case IIS328DQ_6D_INT2_DISABLE: + *val = IIS328DQ_6D_INT2_DISABLE; + break; + + case IIS328DQ_6D_INT2_MOVEMENT: + *val = IIS328DQ_6D_INT2_MOVEMENT; + break; + + case IIS328DQ_6D_INT2_POSITION: + *val = IIS328DQ_6D_INT2_POSITION; + break; + + default: + *val = IIS328DQ_6D_INT2_DISABLE; + break; + } + + return ret; +} + +/// @brief 6D on interrupt generator 2 source register.[get] +/// @param val registers INT2_SRC +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int2_6d_src_get(iis328dq_int2_src_t *val) +{ + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT2_SRC, (uint8_t *) val, 1); + + return ret; +} + +/// @brief Interrupt 2 threshold.[set] +/// @param val change the values of ths in reg INT2_THS +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int2_6d_treshold_set(uint8_t val) +{ + iis328dq_int2_ths_t int2_ths; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT2_THS, (uint8_t *)&int2_ths, 1); + + if (ret == 0) + { + int2_ths.ths = val; + ret = iis328dq_write_reg(IIS328DQ_INT2_THS, + (uint8_t *)&int2_ths, 1); + } + + return ret; +} + +/// @brief Interrupt 2 threshold.[get] +/// @param val change the values of ths in reg INT2_THS +/// @retval Interface status (MANDATORY: return 0 -> no Error) +int32_t Accelerometer::iis328dq_int2_6d_treshold_get(uint8_t *val) +{ + iis328dq_int2_ths_t int2_ths; + int32_t ret; + + ret = iis328dq_read_reg(IIS328DQ_INT2_THS, (uint8_t *)&int2_ths, 1); + *val = int2_ths.ths; + + return ret; +} + +/** + * @} + */ + +/** + * @} + */ + diff --git a/platformio/stima_v4/slave-leaf/src/drivers/eeprom.cpp b/platformio/stima_v4/slave-leaf/src/drivers/eeprom.cpp new file mode 100644 index 000000000..b4402e002 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/src/drivers/eeprom.cpp @@ -0,0 +1,219 @@ +/** + ****************************************************************************** + * @file Eeprom.cpp + * @author Moreno Gasperini + * @brief eeprom AT24C64 cpp source file + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#include "drivers/eeprom.h" + +/// @brief Constructor Class +EEprom::EEprom() +{ +} + +/// @brief Construct a new EEprom::EEprom object +/// @param wire I2C class +/// @param wireLock I2C semaphore +/// @param i2c_address I2C address +EEprom::EEprom(TwoWire *wire, BinarySemaphore *wireLock, uint8_t i2c_address) +{ + _wire = wire; + _wireLock = wireLock; + _i2c_address = i2c_address; +} + +/// @brief Write a number of data byte into EEPROM +/// @param[in] address EEPROM data address +/// @param[in] value data value to write +/// @return true if success, otherwise false returned. +bool EEprom::Write(uint16_t address, uint8_t value) +{ + bool status = true; + if (_wireLock->Take(Ticks::MsToTicks(EEPROM_SEMAPHORE_MAX_WAITING_TIME_MS))) + { + _wire->beginTransmission(_i2c_address); + if (_wire->endTransmission() == 0) + { + _wire->beginTransmission(_i2c_address); + _wire->write(address >> 8); + _wire->write(address & 0xFF); + _wire->write(value); + _wire->endTransmission(); + // Look time for write min 1.4 mS (On Byte) + delay(WR_TIME_MS); + } + else + { + status = false; + } + _wireLock->Give(); + } + else + { + status = false; + } + return status; +} + +/// @brief Write a number of data byte into EEPROM +/// @param[in] address EEPROM data address +/// @param[in] buffer source data buffer location +/// @param[in] length buffer length +/// @return true if success, otherwise false returned. +bool EEprom::Write(uint16_t address, uint8_t *buffer, uint16_t length) +{ + bool status = true; // status ok + bool eot = false; // end of transmission + uint16_t iByte = 0; // index byte to send + if (_wireLock->Take(Ticks::MsToTicks(EEPROM_SEMAPHORE_MAX_WAITING_TIME_MS))) + { + while((eot == false)&&(status == true)) { + _wire->beginTransmission(_i2c_address); + if (_wire->endTransmission() == 0) + { + _wire->beginTransmission(_i2c_address); + _wire->write(address >> 8); + _wire->write(address & 0xFF); + while(iBytewrite(buffer[iByte++]); + // Update address && perform page pass for next byte + if((++address % EEPAGESIZE) == 0) { + // Switch next page (End of page) + // Other Write rollUp to the init Byte of page + break; + } + } + _wire->endTransmission(); + // Test other data to write or eot + eot = iByte >= length; + // Look time for write min 3.6 mS (On Page) + delay(WR_TIME_MS); + } + else + { + status = false; + } + } + _wireLock->Give(); + } + else + { + status = false; + } + return status; +} + +/// @brief Read a single byte from EEPROM +/// @param[in] address EEPROM data address +/// @param[in] value destination data buffer location +/// @return true if success, otherwise false returned. +bool EEprom::Read(uint16_t address, uint8_t *value) +{ + bool status = true; + if (_wireLock->Take(Ticks::MsToTicks(EEPROM_SEMAPHORE_MAX_WAITING_TIME_MS))) + { + _wire->beginTransmission(_i2c_address); + if (_wire->endTransmission() == 0) + { + _wire->beginTransmission(_i2c_address); + _wire->write(address >> 8); + _wire->write(address & 0xFF); + if (_wire->endTransmission() == 0) + _wire->requestFrom((uint8_t)_i2c_address, 1); + *value = _wire->read(); + _wire->endTransmission(); + } + else + { + status = false; + } + _wireLock->Give(); + } + else + { + status = false; + } + return status; +} + +/// @brief Read a number of data byte from EEPROM +/// @param[in] address EEPROM data address +/// @param[in] buffer destination data buffer location +/// @param[in] length buffer length +/// @return true if success, otherwise false returned. +bool EEprom::Read(uint16_t address, uint8_t *buffer, uint16_t length) +{ + bool status = true; + bool eor = false; + uint16_t iIdx = 0; + if (_wireLock->Take(Ticks::MsToTicks(EEPROM_SEMAPHORE_MAX_WAITING_TIME_MS))) + { + // Loop to end of receive total byte + // Block read divise for PAGESIZE maxreceive bytes + while (eor==false) + { + _wire->beginTransmission(_i2c_address); + if (_wire->endTransmission() == 0) + { + _wire->beginTransmission(_i2c_address); + _wire->write(address >> 8); + _wire->write(address & 0xFF); + if (_wire->endTransmission() == 0) + { + uint8_t requestLen; + if(length > EEPAGESIZE) { + requestLen = EEPAGESIZE; + length -= EEPAGESIZE; + } else { + requestLen = length; + eor = true; + } + _wire->requestFrom((uint8_t)_i2c_address, requestLen); + for (uint16_t i = 0; i < requestLen; i++) + { + address++; + buffer[iIdx++] = _wire->read(); + } + _wire->endTransmission(); + } + else + { + status = false; + } + } + else + { + status = false; + } + } + _wireLock->Give(); + } + else + { + status = false; + } + return status; +} \ No newline at end of file diff --git a/platformio/stima_v4/slave-leaf/src/drivers/flash.cpp b/platformio/stima_v4/slave-leaf/src/drivers/flash.cpp new file mode 100644 index 000000000..92c302d90 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/src/drivers/flash.cpp @@ -0,0 +1,1013 @@ +/** + ****************************************************************************** + * @file Flash.cpp + * @author Moreno Gasperini + * @brief flash QSPI ETH452 AT256F161 cpp file + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#include "drivers/module_slave_hal.hpp" +#include "drivers/flash.h" + +/* QSPI IRQ CallBack for QSPI Class ---------------------------------------------------*/ + +/// @brief This function handles QUADSPI global interrupt. +extern "C" void QUADSPI_IRQHandler(void) +{ + /* USER CODE BEGIN QUADSPI_IRQn 0 */ + + /* USER CODE END QUADSPI_IRQn 0 */ + HAL_QSPI_IRQHandler(&hqspi); + /* USER CODE BEGIN QUADSPI_IRQn 1 */ + + /* USER CODE END QUADSPI_IRQn 1 */ +} + +// Flash Object Pointer IRQ_CallBACK Function +Flash::QSPI_IT_EventFlag QSPI_IRQ_Flag; + +/// @brief Transfer Error callback. +/// @param hqspi : QSPI handle +extern "C" void HAL_QSPI_ErrorCallback(QSPI_HandleTypeDef *hqspi) +{ + /* Prevent unused argument(s) compilation warning */ + UNUSED(hqspi); + QSPI_IRQ_Flag.flagBit.flag_TE = 1; +} + +/// @brief Command completed callback. +/// @param hqspi : QSPI handle +extern "C" void HAL_QSPI_CmdCpltCallback(QSPI_HandleTypeDef *hqspi) +{ + UNUSED(hqspi); + QSPI_IRQ_Flag.flagBit.flag_TC = 1; +} + +/// @brief Rx Transfer completed callback. +/// @param hqspi : QSPI handle +extern "C" void HAL_QSPI_RxCpltCallback(QSPI_HandleTypeDef *hqspi) +{ + UNUSED(hqspi); + QSPI_IRQ_Flag.flagBit.flag_TC = 1; +} + +/// @brief Tx Transfer completed callback. +/// @param hqspi : QSPI handle +extern "C" void HAL_QSPI_TxCpltCallback(QSPI_HandleTypeDef *hqspi) +{ + /* Prevent unused argument(s) compilation warning */ + UNUSED(hqspi); + QSPI_IRQ_Flag.flagBit.flag_TC = 1; +} + +/// @brief Status Match callback. +/// @param hqspi : QSPI handle +extern "C" void HAL_QSPI_StatusMatchCallback(QSPI_HandleTypeDef *hqspi) +{ + /* Prevent unused argument(s) compilation warning */ + UNUSED(hqspi); + QSPI_IRQ_Flag.flagBit.flag_SM = 1; +} + +/* Constructor Class ---------------------------------------------------------*/ + +Flash::Flash() +{ +} + +Flash::Flash(QSPI_HandleTypeDef *hqspi) +{ + _hqspi = hqspi; + _evtFlag = &QSPI_IRQ_Flag; +} + +/* Implementation functions ---------------------------------------------------------*/ + +/// @brief Initializes the QSPI interface. +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::BSP_QSPI_Init(void) +{ + /* Check if QSPI is initialized */ + if (_FlashInfo.State == QSPI_READY) + return QSPI_OK; + + _hqspi->Instance = QUADSPI; + _FlashInfo.State = QSPI_ERROR; + + /* Call the DeInit function to reset the driver */ + if (HAL_QSPI_DeInit(_hqspi) != HAL_OK) + return QSPI_ERROR; + + /* QSPI initialization (done in MX_QUADSPI_Init() here rewrite only FlashSize) */ + _hqspi->Init.FlashSize = POSITION_VAL(AT25SF161_FLASH_SIZE) - 1; + + if (HAL_QSPI_Init(_hqspi) != HAL_OK) + return QSPI_ERROR; + + /* Start Flash Get State */ + _FlashInfo.State = QSPI_KO_INIT; + + /* Exit from deep power down */ + if (QSPI_ExitDeepPowerDown()) + return QSPI_ERROR; + + /* Read flash status */ + if (BSP_QSPI_ReadStatus(&_FlashInfo.StatusRegister) != QSPI_OK) + return QSPI_ERROR; + + /* Flash busy? */ + if ((_FlashInfo.StatusRegister & AT25SF161_SR_BUSY)) + return QSPI_BUSY; + + /* Device Quad mode enable */ + if ((_FlashInfo.StatusRegister & AT25SF161_SR_QE) != AT25SF161_SR_QE) { + uint32_t Flash_SR = _FlashInfo.StatusRegister | AT25SF161_SR_QE; + if (BSP_QSPI_WriteStatus(Flash_SR) != QSPI_OK) + return QSPI_ERROR; + } + + /* Read again flash status */ + if (BSP_QSPI_ReadStatus(&_FlashInfo.StatusRegister) != QSPI_OK) + return QSPI_ERROR; + + /* QSPI memory reset */ + if (QSPI_ResetMemory() != QSPI_OK) + return QSPI_NOT_SUPPORTED; + + /* Dummy cycles configuration on QSPI memory side */ + if (QSPI_DummyCyclesCfg() != QSPI_OK) + return QSPI_NOT_SUPPORTED; + + /* Get Info State and Set Ready */ + BSP_QSPI_GetInfo(&_FlashInfo); + + _FlashInfo.State = QSPI_READY; + return QSPI_OK; +} + +/// @brief De-Initializes the QSPI interface. +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::BSP_QSPI_DeInit(void) +{ + HAL_StatusTypeDef sts; + QSPI_StatusTypeDef qsts; + + if (_FlashInfo.State == 0 || _FlashInfo.State == QSPI_RESET) + return QSPI_OK; + + _hqspi->Instance = QUADSPI; + + /* Disable memory mapped mode */ + if (BSP_QSPI_DisableMemoryMappedMode() != QSPI_OK) + return QSPI_ERROR; + + /* WakeUp */ + if ((qsts = QSPI_SetDeepPowerDown()) != QSPI_OK) + return qsts; + HAL_Delay(1); + + /* Call the DeInit function to reset the driver */ + if ((sts = HAL_QSPI_DeInit(_hqspi)) != HAL_OK) + return (QSPI_StatusTypeDef) sts; + + /* Init QSPI status flag */ + _evtFlag->flag = 0; + + _FlashInfo.State = QSPI_RESET; + + return QSPI_OK; +} + +/// @brief Reads an amount of data from the QSPI memory. +/// @param pData: Pointer to data to be read +/// @param ReadAddr: Read start address +/// @param Size: Size of data to read +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::BSP_QSPI_Read(uint8_t *pData, uint32_t ReadAddr, uint32_t Size) +{ + HAL_StatusTypeDef sts; + QSPI_CommandTypeDef sCommand = {0}; + + /* Disable memory mapped mode */ + if (BSP_QSPI_DisableMemoryMappedMode() != QSPI_OK) + return QSPI_ERROR; + + /* Initialize the read command */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = QUAD_INOUT_FAST_READ_CMD; + sCommand.AddressMode = QSPI_ADDRESS_4_LINES; + sCommand.AddressSize = QSPI_ADDRESS_24_BITS; + sCommand.Address = ReadAddr; + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_4_LINES; + sCommand.AlternateBytesSize = QSPI_ALTERNATE_BYTES_8_BITS; + sCommand.AlternateBytes = 0; + sCommand.DataMode = QSPI_DATA_4_LINES; + sCommand.DummyCycles = AT25SF161_DUMMY_CYCLES_READ_QUAD; + sCommand.NbData = Size; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + /* Configure the command */ + if ((sts = HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)) != HAL_OK) + return (QSPI_StatusTypeDef) sts; + + /* Reception of the data */ + return BSP_QSPI_Receive(pData, HAL_QSPI_TIMEOUT_DEFAULT_VALUE); +} + +/// @brief Writes an amount of data to the QSPI memory. +/// @param pData: Pointer to data to be written +/// @param WriteAddr: Write start address +/// @param Size: Size of data to write +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::BSP_QSPI_Write(uint8_t *pData, uint32_t WriteAddr, uint32_t Size) +{ + HAL_StatusTypeDef sts; + QSPI_StatusTypeDef qsts; + QSPI_CommandTypeDef sCommand = {0}; + uint32_t end_addr, current_size, current_addr; + + /* Calculation of the size between the write address and the end of the page */ + current_size = AT25SF161_PAGE_SIZE - (WriteAddr % AT25SF161_PAGE_SIZE); + + /* Check if the size of the data is less than the remaining place in the page */ + if (current_size > Size) + current_size = Size; + + /* Initialize the adress variables */ + current_addr = WriteAddr; + end_addr = WriteAddr + Size; + + /* Disable memory mapped mode */ + if ((qsts = BSP_QSPI_DisableMemoryMappedMode()) != QSPI_OK) + return qsts; + + /* Initialize the program command */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = PAGE_PROG_CMD; + sCommand.AddressMode = QSPI_ADDRESS_1_LINE; + sCommand.AddressSize = QSPI_ADDRESS_24_BITS; + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + sCommand.DataMode = QSPI_DATA_1_LINE; + sCommand.DummyCycles = 0; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + /* Perform the write page by page */ + do { + sCommand.Address = current_addr; + sCommand.NbData = current_size; + + /* Enable write operations */ + if ((qsts = QSPI_WriteEnable()) != QSPI_OK) + return qsts; + + /* Configure the command */ + if ((sts = HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)) != HAL_OK) + return (QSPI_StatusTypeDef) sts; + + /* Transmission of the data */ + if ((qsts = BSP_QSPI_Transmit(pData, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)) != QSPI_OK) + return qsts; + + /* Configure automatic polling mode to wait for end of program */ + if ((qsts = QSPI_AutoPollingMemReady(HAL_QSPI_TIMEOUT_DEFAULT_VALUE)) != QSPI_OK) + return qsts; + + /* Update the address and size variables for next page programming */ + current_addr += current_size; + pData += current_size; + current_size = ((current_addr + AT25SF161_PAGE_SIZE) > end_addr) ? (end_addr - current_addr) : AT25SF161_PAGE_SIZE; + } while (current_addr < end_addr); + + return QSPI_OK; +} + +/// @brief Erases the specified block of the QSPI memory. +/// @param BlockAddress: Block address to erase +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::BSP_QSPI_Erase_Block(uint32_t BlockAddress) +{ + QSPI_CommandTypeDef sCommand = {0}; + + /* Disable memory mapped mode */ + if (BSP_QSPI_DisableMemoryMappedMode() != QSPI_OK) + return QSPI_ERROR; + + /* Initialize the erase command */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = BLOCK_ERASE_CMD; + sCommand.AddressMode = QSPI_ADDRESS_1_LINE; + sCommand.AddressSize = QSPI_ADDRESS_24_BITS; + sCommand.Address = (BlockAddress * AT25SF161_BLOCK_SIZE); + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + sCommand.DataMode = QSPI_DATA_NONE; + sCommand.DummyCycles = 0; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + /* Enable write operations */ + if (QSPI_WriteEnable() != QSPI_OK) + return QSPI_ERROR; + + /* Send the command */ + if (HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE) != HAL_OK) + return QSPI_ERROR; + + /* Configure automatic polling mode to wait for end of erase */ + return QSPI_AutoPollingMemReady(AT25SF161_BLOCK_ERASE_MAX_TIME); +} + +/// @brief Erases the specified sector of the QSPI memory. +/// @param Sector: Sector address to erase (0 to 255) +/// @retval QSPI memory status +/// @note This function is non blocking meaning that sector erase +/// operation is started but not completed when the function +/// returns. Application has to call BSP_QSPI_GetStatus() +/// to know when the device is available again (i.e. erase operation +/// completed). +Flash::QSPI_StatusTypeDef Flash::BSP_QSPI_Erase_Sector(uint32_t Sector) +{ + QSPI_CommandTypeDef sCommand = {0}; + + if (Sector >= (uint32_t)(AT25SF161_FLASH_SIZE / AT25SF161_SECTOR_SIZE)) + return QSPI_ERROR; + + /* Disable memory mapped mode */ + if (BSP_QSPI_DisableMemoryMappedMode() != QSPI_OK) + return QSPI_ERROR; + + /* Initialize the erase command */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = SECTOR_ERASE_CMD; + sCommand.AddressMode = QSPI_ADDRESS_1_LINE; + sCommand.AddressSize = QSPI_ADDRESS_24_BITS; + sCommand.Address = (Sector * AT25SF161_SECTOR_SIZE); + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + sCommand.DataMode = QSPI_DATA_NONE; + sCommand.DummyCycles = 0; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + /* Enable write operations */ + if (QSPI_WriteEnable() != QSPI_OK) + return QSPI_ERROR; + + /* Send the command */ + if (HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE) != HAL_OK) + return QSPI_ERROR; + + /* Configure automatic polling mode to wait for end of erase */ + return QSPI_AutoPollingMemReady(AT25SF161_SECTOR_ERASE_MAX_TIME); +} + +/// @brief Erases the entire QSPI memory. +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::BSP_QSPI_Erase_Chip(void) +{ + QSPI_CommandTypeDef sCommand = {0}; + + /* Disable memory mapped mode */ + if (BSP_QSPI_DisableMemoryMappedMode() != QSPI_OK) + return QSPI_ERROR; + + /* Initialize the erase command */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = BULK_ERASE_CMD; + sCommand.AddressMode = QSPI_ADDRESS_NONE; + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + sCommand.DataMode = QSPI_DATA_NONE; + sCommand.DummyCycles = 0; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + /* Enable write operations */ + if (QSPI_WriteEnable() != QSPI_OK) + return QSPI_ERROR; + + /* Send the command */ + if (HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE) != HAL_OK) + return QSPI_ERROR; + + /* Configure automatic polling mode to wait for end of erase */ + return QSPI_AutoPollingMemReady(AT25SF161_BULK_ERASE_MAX_TIME); +} + +/// @brief Reads current full status registers (byte1 and 2) of the QSPI memory. +/// @param Reg: destination variable address +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::BSP_QSPI_ReadStatus(uint32_t *Reg) +{ + HAL_StatusTypeDef sts; + QSPI_CommandTypeDef sCommand = {0}; + uint8_t reg1, reg2; + + /* Initialize the read status register byte 1 command */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = READ_STATUS_REG_CMD; + sCommand.AddressMode = QSPI_ADDRESS_NONE; + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + sCommand.DataMode = QSPI_DATA_1_LINE; + sCommand.DummyCycles = 0; + sCommand.NbData = 1; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + /* Configure the command */ + if ((sts = HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)) != HAL_OK) + return (QSPI_StatusTypeDef) sts; + + /* Reception of the data byte 1*/ + if ((sts = HAL_QSPI_Receive(_hqspi, ®1, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)) != HAL_OK) + return (QSPI_StatusTypeDef) sts; + + /* Initialize the read status register byte 2 command */ + sCommand.Instruction = READ_STATUS2_REG_CMD; + + /* Configure the command */ + if ((sts = HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)) != HAL_OK) + return (QSPI_StatusTypeDef) sts; + + /* Reception of the data byte 2*/ + if ((sts = HAL_QSPI_Receive(_hqspi, ®2, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)) != HAL_OK) + return (QSPI_StatusTypeDef) sts; + + *Reg = (reg2 << 8) + reg1; + return QSPI_OK; +} + +/// @brief Write status registers of the QSPI memory. +/// @param Reg: source variable +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::BSP_QSPI_WriteStatus(uint32_t Reg) +{ + HAL_StatusTypeDef sts; + QSPI_StatusTypeDef qsts; + QSPI_CommandTypeDef sCommand = {0}; + uint8_t reg[2]; + reg[0] = Reg & 0xFF; + reg[1] = Reg >> 8; + + /* Enable Volatile Status Register mode */ + if ((qsts = QSPI_WriteEnableVolat())!= QSPI_OK) + return qsts; +#if 0 + /* Enable write operations (NON CI VUOLE!) */ + if ((qsts = QSPI_WriteEnable()) != QSPI_OK) + return qsts; +#endif + + /* Initialize the write status register command */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = WRITE_STATUS_REG_CMD; + sCommand.AddressMode = QSPI_ADDRESS_NONE; + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + sCommand.DataMode = QSPI_DATA_1_LINE; + sCommand.DummyCycles = 0; + sCommand.NbData = 2; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + /* Configure the command */ + if ((sts = HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)) != HAL_OK) + return (QSPI_StatusTypeDef) sts; + + /* Transmission of the status data */ + return BSP_QSPI_Transmit(reg, HAL_QSPI_TIMEOUT_DEFAULT_VALUE); +} + +/// @brief Reads current status (RDY/BUSY, WEL and SUS bits) of the QSPI memory. +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::BSP_QSPI_GetStatus(void) +{ + uint32_t sts; + + if (BSP_QSPI_ReadStatus(&sts) != QSPI_OK) + return QSPI_ERROR; + + if ((sts & AT25SF161_FS_ERSUS) == AT25SF161_FS_ERSUS) + return QSPI_SUSPENDED; + else if ((sts & AT25SF161_SR_BUSY) == AT25SF161_SR_BUSY) + return QSPI_BUSY; + else + return QSPI_OK; +} + +/// @brief Return the configuration of the QSPI memory. +/// @param pInfo: pointer on the configuration structure +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::BSP_QSPI_GetInfo(QSPI_Info *pInfo) +{ + /* Configure the structure with the memory configuration */ + pInfo->FlashSize = AT25SF161_FLASH_SIZE; + pInfo->EraseBlockSize = AT25SF161_BLOCK_SIZE; + pInfo->EraseBlockNumber = (AT25SF161_FLASH_SIZE / AT25SF161_BLOCK_SIZE); + pInfo->ProgPageSize = AT25SF161_PAGE_SIZE; + pInfo->ProgPagesNumber = (AT25SF161_FLASH_SIZE / AT25SF161_PAGE_SIZE); + + return QSPI_OK; +} + +/// @brief Configure the QSPI in memory-mapped mode +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::BSP_QSPI_EnableMemoryMappedMode(void) +{ + QSPI_CommandTypeDef sCommand = {0}; + QSPI_MemoryMappedTypeDef sMemMappedCfg = {0}; + + /* Is already in memory mapped mode? */ + if (_hqspi->State == HAL_QSPI_STATE_BUSY_MEM_MAPPED) + return QSPI_OK; + + /* Configure the command for the read instruction */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = QUAD_INOUT_FAST_READ_CMD; + sCommand.AddressMode = QSPI_ADDRESS_4_LINES; + sCommand.AddressSize = QSPI_ADDRESS_24_BITS; + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_4_LINES; + sCommand.AlternateBytesSize = QSPI_ALTERNATE_BYTES_8_BITS; + sCommand.AlternateBytes = 0x20; + + sCommand.DataMode = QSPI_DATA_4_LINES; + sCommand.DummyCycles = AT25SF161_DUMMY_CYCLES_READ_QUAD; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_ONLY_FIRST_CMD; + + /* Configure the memory mapped mode */ + sMemMappedCfg.TimeOutActivation = QSPI_TIMEOUT_COUNTER_DISABLE; + + return (QSPI_StatusTypeDef) HAL_QSPI_MemoryMapped(_hqspi, &sCommand, &sMemMappedCfg); +} + +/// @brief Disable the QSPI memory-mapped mode +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::BSP_QSPI_DisableMemoryMappedMode(void) +{ + if (__HAL_QSPI_GET_FLAG(_hqspi, QSPI_FLAG_BUSY) == SET) { + if (HAL_QSPI_Abort(_hqspi) != HAL_OK) + return QSPI_ERROR; + if (QSPI_DisableContinuousMode() != QSPI_OK) + return QSPI_ERROR; + } + return QSPI_OK; +} + +/// @brief This function suspends an ongoing erase command. +/// @retval QSPI memory status +uint8_t Flash::BSP_QSPI_SuspendErase(void) +{ + QSPI_CommandTypeDef sCommand = {0}; + + /* Check whether the device is busy (erase operation is + in progress). + */ + if (BSP_QSPI_GetStatus() == QSPI_BUSY) { + /* Initialize the erase command */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = PROG_ERASE_SUSPEND_CMD; + sCommand.AddressMode = QSPI_ADDRESS_NONE; + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + sCommand.DataMode = QSPI_DATA_NONE; + sCommand.DummyCycles = 0; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + /* Enable write operations */ + if (QSPI_WriteEnable() != QSPI_OK) + return QSPI_ERROR; + + /* Send the command */ + if (HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE) != HAL_OK) + return QSPI_ERROR; + + if (BSP_QSPI_GetStatus() == QSPI_SUSPENDED) + return QSPI_OK; + + return QSPI_ERROR; + } + + return QSPI_OK; +} + +/// @brief This function resumes a paused erase command. +/// @retval QSPI memory status +uint8_t Flash::BSP_QSPI_ResumeErase(void) +{ + QSPI_CommandTypeDef sCommand = {0}; + + /* Check whether the device is in suspended state */ + if (BSP_QSPI_GetStatus() == QSPI_SUSPENDED) { + /* Initialize the erase command */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = PROG_ERASE_RESUME_CMD; + sCommand.AddressMode = QSPI_ADDRESS_NONE; + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + sCommand.DataMode = QSPI_DATA_NONE; + sCommand.DummyCycles = 0; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + /* Enable write operations */ + if (QSPI_WriteEnable() != QSPI_OK) + return QSPI_ERROR; + + /* Send the command */ + if (HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE) != HAL_OK) + return QSPI_ERROR; + + /* + When this command is executed, the status register write in progress bit is set to 1, and + the flag status register program erase controller bit is set to 0. This command is ignored + if the device is not in a suspended state. + */ + + if (BSP_QSPI_GetStatus() == QSPI_BUSY) + return QSPI_OK; + + return QSPI_ERROR; + } + + return QSPI_OK; +} + +/// @brief Receive an amount of data in interrupt mode. +/// @param pData : pointer to data buffer +/// @param Timeout : Timeout duration +/// @note This function is used only in Indirect Read Mode +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::BSP_QSPI_Receive(uint8_t *pData, uint32_t Timeout) +{ + /* Try Match QSPI status flag */ + _evtFlag->flag = 0; + _evtFlag->flagBit.match_TE = 1; + _evtFlag->flagBit.match_TC = 1; + if (HAL_QSPI_Receive_IT(_hqspi, pData) != HAL_OK) + return QSPI_ERROR; + return BSP_QSPI_WaitingForEvent(Timeout); +} + +/// @brief Transmit an amount of data in interrupt mode. +/// @param pData : pointer to data buffer +/// @param Timeout : Timeout duration +/// @note This function is used only in Indirect Write Mode +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::BSP_QSPI_Transmit(uint8_t *pData, uint32_t Timeout) +{ + /* Try Match QSPI status flag */ + _evtFlag->flag = 0; + _evtFlag->flagBit.match_TE = 1; + _evtFlag->flagBit.match_TC = 1; + if (HAL_QSPI_Transmit_IT(_hqspi, pData) != HAL_OK) + return QSPI_ERROR; + return BSP_QSPI_WaitingForEvent(Timeout); +} + +/// @brief Configure the QSPI Automatic Polling Mode in interrupt mode. +/// @param cmd : structure that contains the command configuration information. +/// @param cfg : structure that contains the polling configuration information. +/// @param Timeout : Timeout duration +/// @note This function is used only in Automatic Polling Mode +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::BSP_QSPI_AutoPolling(QSPI_CommandTypeDef *cmd, QSPI_AutoPollingTypeDef *cfg, uint32_t Timeout) +{ + /* Try Match QSPI status flag */ + _evtFlag->flag = 0; + _evtFlag->flagBit.match_TE = 1; + _evtFlag->flagBit.match_SM = 1; + if (HAL_QSPI_AutoPolling_IT(_hqspi, cmd, cfg) != HAL_OK) + return QSPI_ERROR; + return BSP_QSPI_WaitingForEvent(Timeout); +} + +/// @brief Waiting event for QSPI Module +/// @param Timeout Timeout for operation +/// @return QSPI polling status flag +Flash::QSPI_StatusTypeDef Flash::BSP_QSPI_WaitingForEvent(uint32_t Timeout) { + bool bSetOFlag = false; + bool bTimeOut = false; + bool bMatchField = false; + Timeout += millis(); + if( Timeout < millis()) bSetOFlag = true; + while((!bTimeOut)&&(!bMatchField)) { + // Switch Task + taskYIELD(); + // Test Flag + if(_evtFlag->flagBit.match_SM && _evtFlag->flagBit.flag_SM) bMatchField = true; + if(_evtFlag->flagBit.match_TC && _evtFlag->flagBit.flag_TC) bMatchField = true; + if(_evtFlag->flagBit.match_TE && _evtFlag->flagBit.flag_TE) bMatchField = true; + // Check Timeout + if(bSetOFlag) { + if(millis() < Timeout) bSetOFlag = false; + } else { + if(millis() > Timeout) bTimeOut = true; + } + } + if(bTimeOut) return QSPI_TIMEOUT; + if(_evtFlag->flagBit.match_SM && _evtFlag->flagBit.flag_SM) return QSPI_OK; + if(_evtFlag->flagBit.match_TC && _evtFlag->flagBit.flag_TC) return QSPI_OK; + return QSPI_ERROR; +} + +/// @brief This function enter in deep power down the QSPI memory. +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::QSPI_SetDeepPowerDown(void) +{ + QSPI_CommandTypeDef sCommand = {0}; + + /* Enable write operations */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = DEEP_POWER_DOWN_CMD; + sCommand.AddressMode = QSPI_ADDRESS_NONE; + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + sCommand.DataMode = QSPI_DATA_NONE; + sCommand.DummyCycles = 0; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + return (QSPI_StatusTypeDef) HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE); +} + +/// @brief This function exit from deep power down the QSPI memory. +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::QSPI_ExitDeepPowerDown(void) +{ + QSPI_CommandTypeDef sCommand = {0}; + + /* Enable write operations */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = RESUME_FROM_DEEP_PWD_CMD; + sCommand.AddressMode = QSPI_ADDRESS_NONE; + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + sCommand.DataMode = QSPI_DATA_NONE; + sCommand.DummyCycles = 0; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + return (QSPI_StatusTypeDef) HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE); +} + +/// @brief This function reset the QSPI memory. +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::QSPI_ResetMemory(void) +{ +#ifdef __AT25SF161_H + return QSPI_OK; +#else + QSPI_CommandTypeDef sCommand = {0}; + + /* Initialize the reset enable command */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = RESET_ENABLE_CMD; + sCommand.AddressMode = QSPI_ADDRESS_NONE; + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + sCommand.DataMode = QSPI_DATA_NONE; + sCommand.DummyCycles = 0; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + /* Send the command */ + if (HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE) != HAL_OK) + return QSPI_ERROR; + + /* Send the reset memory command */ + sCommand.Instruction = RESET_MEMORY_CMD; + if (HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE) != HAL_OK) + return QSPI_ERROR; + + /* Configure automatic polling mode to wait the memory is ready */ + if (QSPI_AutoPollingMemReady(HAL_QSPI_TIMEOUT_DEFAULT_VALUE) != QSPI_OK) + return QSPI_ERROR; + + return QSPI_OK; +#endif +} + +/** + * @brief This function configure the dummy cycles on memory side. + * @retval QSPI memory status + */ +Flash::QSPI_StatusTypeDef Flash::QSPI_DummyCyclesCfg(void) +{ +#ifdef __AT25SF161_H + return QSPI_OK; +#else + + QSPI_CommandTypeDef sCommand = {0}; + uint8_t reg; + + /* Initialize the read volatile configuration register command */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = READ_VOL_CFG_REG_CMD; + sCommand.AddressMode = QSPI_ADDRESS_NONE; + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + sCommand.DataMode = QSPI_DATA_1_LINE; + sCommand.DummyCycles = 0; + sCommand.NbData = 1; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + /* Configure the command */ + if (HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE) != HAL_OK) + return QSPI_ERROR; + + /* Reception of the data */ + if (HAL_QSPI_Receive(_hqspi, ®, HAL_QSPI_TIMEOUT_DEFAULT_VALUE) != HAL_OK) + return QSPI_ERROR; + + /* Enable write operations */ + if (QSPI_WriteEnable() != QSPI_OK) + return QSPI_ERROR; + + /* Update volatile configuration register (with new dummy cycles) */ + sCommand.Instruction = WRITE_VOL_CFG_REG_CMD; + MODIFY_REG(reg, AT25SF161_VCR_NB_DUMMY, (AT25SF161_DUMMY_CYCLES_READ_QUAD << POSITION_VAL(AT25SF161_VCR_NB_DUMMY))); + + /* Configure the write volatile configuration register command */ + if (HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE) != HAL_OK) + return QSPI_ERROR; + + /* Transmission of the data */ + if (HAL_QSPI_Transmit(_hqspi, ®, HAL_QSPI_TIMEOUT_DEFAULT_VALUE) != HAL_OK) + return QSPI_ERROR; + + return QSPI_OK; +#endif +} + +/// @brief This function enable the write for Volatile Status Register only. +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::QSPI_WriteEnableVolat(void) +{ + QSPI_CommandTypeDef sCommand = {0}; + + /* Enable write volatile operations */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = WRITE_EN_VOLAT_STATUS_REG_CMD; + sCommand.AddressMode = QSPI_ADDRESS_NONE; + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + sCommand.DataMode = QSPI_DATA_NONE; + sCommand.DummyCycles = 0; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + return (QSPI_StatusTypeDef) HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE); +} + +/// @brief This function set Write Enable Latch bit and wait it is effective. +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::QSPI_WriteEnable(void) +{ + HAL_StatusTypeDef sts; + QSPI_CommandTypeDef sCommand = {0}; + QSPI_AutoPollingTypeDef sConfig = {0}; + + /* Enable write operations */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = WRITE_ENABLE_CMD; + sCommand.AddressMode = QSPI_ADDRESS_NONE; + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + sCommand.DataMode = QSPI_DATA_NONE; + sCommand.DummyCycles = 0; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + if ((sts = HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)) != HAL_OK) + return (QSPI_StatusTypeDef) sts; + + /* Configure automatic polling mode to wait for write enabling */ + sConfig.Match = AT25SF161_SR_WEL; + sConfig.Mask = AT25SF161_SR_WEL; + sConfig.MatchMode = QSPI_MATCH_MODE_AND; + sConfig.StatusBytesSize = 1; + sConfig.Interval = 0x10; + sConfig.AutomaticStop = QSPI_AUTOMATIC_STOP_ENABLE; + + sCommand.Instruction = READ_STATUS_REG_CMD; + sCommand.DataMode = QSPI_DATA_1_LINE; + + return BSP_QSPI_AutoPolling(&sCommand, &sConfig, HAL_QSPI_TIMEOUT_DEFAULT_VALUE); +} + +/// @brief This function clear Write Enable Latch bit and wait it is effective. +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::QSPI_WriteDisable(void) +{ + HAL_StatusTypeDef sts; + QSPI_CommandTypeDef sCommand = {0}; + QSPI_AutoPollingTypeDef sConfig = {0}; + + /* Disable write operations */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = WRITE_DISABLE_CMD; + sCommand.AddressMode = QSPI_ADDRESS_NONE; + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + sCommand.DataMode = QSPI_DATA_NONE; + sCommand.DummyCycles = 0; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + if ((sts = HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)) != HAL_OK) + return (QSPI_StatusTypeDef) sts; + + /* Configure automatic polling mode to wait for write disabling */ + sConfig.Match = RESET; + sConfig.Mask = AT25SF161_SR_WEL; + sConfig.MatchMode = QSPI_MATCH_MODE_AND; + sConfig.StatusBytesSize = 1; + sConfig.Interval = 0x10; + sConfig.AutomaticStop = QSPI_AUTOMATIC_STOP_ENABLE; + + sCommand.Instruction = READ_STATUS_REG_CMD; + sCommand.DataMode = QSPI_DATA_1_LINE; + + return BSP_QSPI_AutoPolling(&sCommand, &sConfig, HAL_QSPI_TIMEOUT_DEFAULT_VALUE); +} + +/// @brief This function disable Continuous Read Mode Reset - Quad. +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::QSPI_DisableContinuousMode(void) +{ + QSPI_CommandTypeDef sCommand = {0}; + + /* Enable write volatile operations */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = QUAD_CONTINUOUS_READ_MODE_RESET; + sCommand.AddressMode = QSPI_ADDRESS_NONE; + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + sCommand.DataMode = QSPI_DATA_NONE; + sCommand.DummyCycles = 0; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + return (QSPI_StatusTypeDef) HAL_QSPI_Command(_hqspi, &sCommand, HAL_QSPI_TIMEOUT_DEFAULT_VALUE); +} + +/// @brief This function read the SR of the memory and wait the EOP. +/// @param Timeout: Timeout for auto-polling +/// @retval QSPI memory status +Flash::QSPI_StatusTypeDef Flash::QSPI_AutoPollingMemReady(uint32_t Timeout) +{ + QSPI_CommandTypeDef sCommand = {0}; + QSPI_AutoPollingTypeDef sConfig = {0}; + + /* Configure automatic polling mode to wait for memory ready */ + sCommand.InstructionMode = QSPI_INSTRUCTION_1_LINE; + sCommand.Instruction = READ_STATUS_REG_CMD; + sCommand.AddressMode = QSPI_ADDRESS_NONE; + sCommand.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE; + sCommand.DataMode = QSPI_DATA_1_LINE; + sCommand.DummyCycles = 0; + sCommand.DdrMode = QSPI_DDR_MODE_DISABLE; + sCommand.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY; + sCommand.SIOOMode = QSPI_SIOO_INST_EVERY_CMD; + + sConfig.Match = RESET; + sConfig.Mask = AT25SF161_SR_BUSY; + sConfig.MatchMode = QSPI_MATCH_MODE_AND; + sConfig.StatusBytesSize = 1; + sConfig.Interval = 0x10; + sConfig.AutomaticStop = QSPI_AUTOMATIC_STOP_ENABLE; + + return BSP_QSPI_AutoPolling(&sCommand, &sConfig, Timeout); +} + + diff --git a/platformio/stima_v4/slave-leaf/src/drivers/freeRTOS_callback.cpp b/platformio/stima_v4/slave-leaf/src/drivers/freeRTOS_callback.cpp new file mode 100644 index 000000000..6c902ad0c --- /dev/null +++ b/platformio/stima_v4/slave-leaf/src/drivers/freeRTOS_callback.cpp @@ -0,0 +1,231 @@ +/** + ****************************************************************************** + * @file freeRTOS_callback.cpp + * @author Moreno Gasperini + * @brief CallBack Freertos and Handler base system function + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#include "FreeRTOS.h" +#include "task.h" +#include "drivers/module_slave_hal.hpp" +#include "STM32LowPower.h" +#include + +// /******************************************************************************************* +// ******************************************************************************************** +// Freertos Param Config and CallBack __Weak Function Redefinition +// ******************************************************************************************** +// *******************************************************************************************/ + +#ifdef _USE_FREERTOS_LOW_POWER + +/// @brief Prepara il sistema allo Sleep (OFF Circuirterie ed entrata in PowerDown) +/// @param xExpectedIdleTime Ticks RTOS (ms) attesi per la funzione di Sleep +extern "C" void xTaskSleepPrivate(TickType_t *xExpectedIdleTime) { + #if (LOWPOWER_MODE==SLEEP_IDLE) + LowPower.idle(*xExpectedIdleTime); + #elif (LOWPOWER_MODE==SLEEP_LOWPOWER) + LowPower.sleep(*xExpectedIdleTime - 10); + #elif (LOWPOWER_MODE==SLEEP_STOP2) + LowPower.deepSleep(*xExpectedIdleTime - 10); + #else + *xExpectedIdleTime = 0; + #endif +} + +/// @brief Riattiva il sistema dopo lo Sleep (Riattivazione perifieriche, Clock ecc...) +/// @param xExpectedIdleTime Ticks RTOS (ms) effettivamente eseguiti dalla funzione di Sleep +extern "C" void xTaskWakeUpPrivate(TickType_t *xExpectedIdleTime) { +} + +// Remove Arduino OSSysTick for LPTIM(x) IRQ lptimTick.c Driver (AutoInc OsTick) +// Is Need to redefined weak void __attribute__((weak)) osSystickHandler(void) +// Note FROM Freertos_Config.h +/* + * IMPORTANT: + * SysTick_Handler() from stm32duino core is calling weak osSystickHandler(). + * Both CMSIS-RTOSv2 and CMSIS-RTOS override osSystickHandler() + * which is calling xPortSysTickHandler(), defined in respective CortexM-x port +*/ +#if ( configUSE_TICKLESS_IDLE == 2 ) +extern "C" void osSystickHandler() +{ + // osSystickHandler CallBack UNUSED for LPTIM1 IRQ Set Increment of OsTickHadler + // Optional User Code about osSystickHandler Private Here + // ... +} +#endif + +#endif + +#if(DEBUG_MODE) +//------------------------------------------------------------------------------ +/// @brief LocalMS Delay Handler_XXX +/// @param millis Millisecondi di Wait +static void delayMS(uint32_t millis) { + uint32_t iterations = millis * (F_CPU/7000); + uint32_t i; + for(i = 0; i < iterations; ++i) { + __asm__("nop\n\t"); + } +} + +/// @brief Buzzer Redefiniton Segnalazione LedBlink con Buzzer +/// @param n Numero di iterazioni per segnalazione +static void faultStimaV4(int n) { + #ifdef HFLT_PIN + __disable_irq(); + pinMode(HFLT_PIN, OUTPUT); + for (;;) { + int i; + #if (ENABLE_WDT) + // Error signal WDT Refresh (For debugger only) + IWatchdog.reload(); + #endif + for (i = 0; i < n; i++) { + digitalWrite(HFLT_PIN, 1); + delayMS(300); + digitalWrite(HFLT_PIN, 0); + delayMS(300); + } + delayMS(2000); + } +#else + while(1); +#endif // HFLT_PIN +} +//------------------------------------------------------------------------------ +#endif + +#if ( configCHECK_FOR_STACK_OVERFLOW >= 1 ) + /** Blink three short pulses if stack overflow is detected. + Run time stack overflow checking is performed if + configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook + function is called if a stack overflow is detected. + \param[in] pxTask Task handle + \param[in] pcTaskName Task name + */ +extern "C" void vApplicationStackOverflowHook(TaskHandle_t pxTask, char *pcTaskName) { + // Ned to use Serial.print direct for prevent Malloc from RTOS (ISR Malloc ASSERT Error) + Serial.print("Error stack overflow form task: "); + Serial.print(pcTaskName); + Serial.flush(); + #if(DEBUG_MODE) + faultStimaV4(3); + #else + NVIC_SystemReset(); + #endif +} +#endif /* configCHECK_FOR_STACK_OVERFLOW >= 1 */ + +//------------------------------------------------------------------------------ +// catch exceptions + +// Generic Error_Handler +#if(ERROR_HANDLER_CB) +extern "C" void _Error_Handler(const char *msg, int val) +{ + /* User can add his own implementation to report the HAL error return state */ + // Ned to use Serial.print direct for prevent Malloc from RTOS (ISR Malloc ASSERT Error) + Serial.print("Error handler: "); + Serial.print(msg); + Serial.print(", "); + Serial.print(val); + Serial.flush(); + #if(DEBUG_MODE) + faultStimaV4(3); + #else + NVIC_SystemReset(); + #endif +} +#endif + +/** Hard fault - blink four short flash every two seconds */ +extern "C" void hard_fault_isr() { + #if(DEBUG_MODE) + faultStimaV4(4); + #else + NVIC_SystemReset(); + #endif +} +/** Hard fault - blink four short flash every two seconds */ +extern "C" void HardFault_Handler() { + #if(DEBUG_MODE) + faultStimaV4(4); + #else + NVIC_SystemReset(); + #endif +} + +/** Bus fault - blink five short flashes every two seconds */ +extern "C" void bus_fault_isr() { + #if(DEBUG_MODE) + faultStimaV4(5); + #else + NVIC_SystemReset(); + #endif +} +/** Bus fault - blink five short flashes every two seconds */ +extern "C" void BusFault_Handler() { + #if(DEBUG_MODE) + faultStimaV4(5); + #else + NVIC_SystemReset(); + #endif +} + +/** Usage fault - blink six short flashes every two seconds */ +extern "C" void usage_fault_isr() { + #if(DEBUG_MODE) + faultStimaV4(6); + #else + NVIC_SystemReset(); + #endif +} +/** Usage fault - blink six short flashes every two seconds */ +extern "C" void UsageFault_Handler() { + #if(DEBUG_MODE) + faultStimaV4(6); + #else + NVIC_SystemReset(); + #endif +} + +/** Usage fault - blink six short flashes every two seconds */ +extern "C" void MemManage_fault_isr() { + #if(DEBUG_MODE) + faultStimaV4(7); + #else + NVIC_SystemReset(); + #endif +} +/** Usage fault - blink six short flashes every two seconds */ +extern "C" void MemManage_Handler() { + #if(DEBUG_MODE) + faultStimaV4(7); + #else + NVIC_SystemReset(); + #endif +} diff --git a/platformio/stima_v4/slave-leaf/src/drivers/freeRTOS_lptimTick.c b/platformio/stima_v4/slave-leaf/src/drivers/freeRTOS_lptimTick.c new file mode 100644 index 000000000..7fc868c83 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/src/drivers/freeRTOS_lptimTick.c @@ -0,0 +1,852 @@ +/** + ****************************************************************************** + * @file freeRTOS_lptimTick.c + * @author Moreno Gasperini + * @brief LowPower LPTIM1 TickHandler adapted from lptimTick.c -- Jeff Tenney + * Copyright 2021 Jeff Tenney + * STM32 No-Drift FreeRTOS Tick/Tickless via LPTIM + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + * Copyright 2021 Jeff Tenney + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and + * associated documentation files (the "Software"), to deal in the Software without restriction, including + * without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the + * following conditions: + * + * The above copyright notice and this permission notice shall be included in all copies or substantial + * portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT + * LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO + * EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE + * USE OR OTHER DEALINGS IN THE SOFTWARE. + * + ****************************************************************************** +*/ + +#include "FreeRTOS.h" +#include "task.h" +#include "stm32l4xx.h" + +// This FreeRTOS port "extension" for STM32 uses LPTIM to generate the OS tick instead of the systick +// timer. The benefit of the LPTIM is that it continues running in "stop" mode as long as its clock source +// does. A typical clock source for the LPTIM is LSE (or LSI), which does keep running in stop mode. +// +// The resulting FreeRTOS port: +// +// o Allows use of low-power stop modes during tickless idle, while still keeping kernel time. +// o Eliminates kernel-time drift associated with tickless idle in official FreeRTOS port +// o Eliminates kernel-time drift caused by rounding the OS tick to a whole number of timer counts. +// o Avoids drift and errors found in other LPTIM implementations available from ST or the public domain. +// o Detects/reports ticks dropped due to the application masking interrupts (the tick ISR) for too long. +// +// This software is currently adapted for STM32L4(+) but is easily adaptable to (or already compatible +// with) any STM32 that provides an LPTIM peripheral, such as STM32L, STM32F, STM32G, STM32H, STM32W, and the +// new STM32U. + + +// Terminology +// +// "Count" - What a timer does in its "count" register (CNT). +// +// "Tick" - The OS tick, made up of some number of timer counts. + + +// Perfect Tick Frequency +// +// This software optionally varies the number of timer counts per OS tick to achieve the target OS tick +// frequency. The OS tick generally occurs no more than half a timer count early or half a timer count late +// compared to the ideal tick time. This is especially useful with a 32768Hz reference on LSE and a desired +// 1000Hz system tick. In that case, this software uses 32-count and 33-count tick durations as needed to +// stay on the 1000Hz tick schedule. No matter how you set configLPTIM_REF_CLOCK_HZ and configTICK_RATE_HZ, +// this software stays precisely on schedule. +// +// You can disable this feature and instead use a simple, constant number of timer counts per OS tick by +// defining configLPTIM_ENABLE_PRECISION to 0. Your effective tick rate will be as close as possible to +// configTICK_RATE_HZ while using a constant number of counts per tick. For example, with a 32768 Hz clock +// and an ideal tick frequency of 1000 Hz, the actual tick frequency is ~993 Hz. + + +// Silicon Bug +// +// The LPTIM has a silicon bug that can cause the CPU to be temporarily stuck in the LPTIM ISR with no +// way for the CPU to clear the IRQ. The silicon bug is not documented by ST (yet), but the "stuck" condition +// appears to last one full count of the LPTIM. It seems to occur only when the application is using the MCU +// "stop" power modes. +// +// Any attempt to use stop mode in the window between a "match" and the CMPM one count later causes the +// match interrupt to be asserted (early), and it cannot be cleared until the CMPM flag is set. Additionally, +// any attempt to use stop mode during the timer count after a CMPM event that we tried to suppress too late +// also results in the entire count duration stuck in the ISR. By "suppress too late", we mean write a new +// value to CMP just before a CMPM event. +// +// As a workaround, the application should be sure that configTICK_INTERRUPT_PRIORITY is a lower priority +// than application interrupts. We considered a workaround in this file but realized that our only viable +// recourse is to avoid stop mode at strategic times, but those time windows last several timer counts. Using +// sleep mode instead of stop mode for several counts is more costly than the silicon bug itself, which uses +// run mode instead of stop mode for one timer count (stuck in interrupt handlers). + + +// Quirks of LPTIM +// +// The following "quirks" indicate that LPTIM is designed for PWM output control and not for generating +// timed interrupts. This software overcomes all of them. +// +// o Writes to CMP are delayed by a sync mechanism inside the timer. The sync takes ~3 timer counts. +// o During the synchronization process, additional writes to CMP are prohibited. +// o CMPOK (sync completion) comes *after* the CMP value is asynchronously available for match events. +// o The match condition is not simply "CNT == CMP" but is actually "CNT >= CMP && CNT != ARR". +// o The timer sets CMPM (and optional IRQ) one timer count *after* the match condition is reached. +// o With a new CMP value, the timer must first be in a "no-match" condition to generate a match event. +// o Setting CMP == ARR is prohibited. See below. +// o Changing IER (interrupt-enable register) is prohibited while LPTIM is enabled. +// o The CPU can get stuck temporarily in the LPTIM ISR when using stop mode. See "Silicon Bug" above. +// +// This software sets ARR to 0xFFFF permanently and modifies CMP to arrange each next tick interrupt. +// Due to the rule against setting CMP == ARR, we never set CMP to 0xFFFF. If the ideal CMP value for a tick +// interrupt is 0xFFFF, we use 0 instead. + + +// Side Effects +// +// o Tick Overhead. OS ticks generated by this software have more overhead than ticks generated by the +// official port code. In most applications, the overhead doesn't make any real difference. Our tick ISR +// is longer by a handful of CPU instructions, and we execute a 2nd, very short ISR in between ticks. +// +// o Tick IRQ Priority. The tick IRQ priority must be high enough that no combination of ISRs can block its +// execution for longer than 1 tick. See configTICK_INTERRUPT_PRIORITY (below) for more information. +// However, the application may safely mask interrupts for longer than 1 tick, rare as that need may be. +// (One common example is for "fast-programming" flash memory on STM32.) In that case, this software even +// reports dropped ticks afterward via traceTICKS_DROPPED(). +// +// o Tick Jitter. When precision is enabled (configLPTIM_ENABLE_PRECISION), ticks generated by this +// software have jitter, as described above in "Perfect Tick Frequency". In most applications, jitter in +// the tick periods is not a concern. + + +#if ( !defined(configUSE_TICKLESS_IDLE) || configUSE_TICKLESS_IDLE != 2 ) +#warning Please edit FreeRTOSConfig.h to define configUSE_TICKLESS_IDLE as 2 *or* exclude this file. +#else + +#ifdef xPortSysTickHandler +#warning Please edit FreeRTOSConfig.h to eliminate the preprocessor definition for xPortSysTickHandler. +#endif + + +// Symbol configTICK_INTERRUPT_PRIORITY, optionally defined in FreeRTOSConfig.h, controls the tick +// interrupt priority. Most applications should define configTICK_INTERRUPT_PRIORITY to be +// configLIBRARY_LOWEST_INTERRUPT_PRIORITY because the tick doesn't need a high priority. But if your +// application has long ISRs, you may need to increase the priority of the tick. The tick priority must be +// high enough that no combination of ISRs at or above its priority level can block the tick ISR for longer +// than 1 tick. But the priority must not be higher than configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY +// (meaning that the value of configTICK_INTERRUPT_PRIORITY must not be numerically lower). +// +// Be sure to consider the information in "Silicon Bug" above before increasing the interrupt priority of +// the system tick. +// +#ifndef configTICK_INTERRUPT_PRIORITY +#define configTICK_INTERRUPT_PRIORITY configLIBRARY_LOWEST_INTERRUPT_PRIORITY // default only; see above +#endif + +// Symbol configTICK_USES_LSI, optionally defined in FreeRTOSConfig.h, is defined only when LPTIM should +// use LSI as the clock instead of LSE. By default, however, this software configures LPTIM to use LSE +// because a key feature of this software is timing accuracy -- no drift in tickless idle. +// +#ifdef configTICK_USES_LSI + #define LPTIMSEL_Val 1 // LSI + #define IS_REF_CLOCK_READY() (RCC->CSR & RCC_CSR_LSIRDY) +#else + #define LPTIMSEL_Val 3 // LSE + #define IS_REF_CLOCK_READY() (RCC->BDCR & RCC_BDCR_LSERDY) +#endif + +// Symbol configLPTIM_REF_CLOCK_HZ, optionally defined in FreeRTOSConfig.h, is the frequency of the +// selected reference clock or source clock for LPTIM. If configTICK_USES_LSI is defined, then +// configLPTIM_REF_CLOCK_HZ equals the frequency of LSI (typically 32000 Hz or 37000 Hz depending on the MCU). +// Otherwise, configLPTIM_REF_CLOCK_HZ equals the frequency of LSE (usually 32768 Hz). +// +#ifndef configLPTIM_REF_CLOCK_HZ +#define configLPTIM_REF_CLOCK_HZ 32768UL +#endif + +// Symbol configLPTIM_ENABLE_PRECISION, optionally defined in FreeRTOSConfig.h, allows a configuration to +// eliminate the arithmetic in this software that maintains configTICK_RATE_HZ with perfect precision, as +// described above in "Perfect Tick Frequency". The arithmetic corrects for any error in rounding the desired +// tick duration to a whole number of timer counts. No matter how you set the precision option, this software +// eliminates the drift normally associated with tickless idle. The precision option is enabled by default +// because it has a very small footprint by all measures (flash, RAM, execution time). However, because the +// precision feature requires additional division operations, Cortex M0 users may consider disabling it. CM0 +// does not have a native divide instruction, so division operations are a little slow on that platform. +// +#ifndef configLPTIM_ENABLE_PRECISION +#define configLPTIM_ENABLE_PRECISION 1 +#endif + +// If the application masks interrupts (specifically the tick interrupt) long enough to drop a tick, then +// the tick ISR calls this trace macro to report the condition along with the number of ticks dropped. You +// can define this macro in FreeRTOSConfig.h. Remember that the macro executes from within the tick ISR. +// Also be aware that the ISR resets the phase of the ticks after calling this macro. +// +// If ticks are dropped and you did *not* mask the tick interrupt long enough to drop a tick, you +// probably have ISRs blocking the tick ISR for too long. This is a serious issue that requires changes to +// your design. Please see configTICK_INTERRUPT_PRIORITY above. +// +#ifndef traceTICKS_DROPPED +#define traceTICKS_DROPPED(x) +#endif + +#define LPTIM_CLOCK_HZ ( configLPTIM_REF_CLOCK_HZ ) + +static TickType_t xMaximumSuppressedTicks; // We won't try to sleep longer than this many ticks during + // tickless idle because any longer might confuse the logic in + // our implementation. + +static uint32_t ulTimerCountsForOneTick; // A "baseline" tick has this many timer counts. The + // baseline tick is as close as possible to the ideal duration + // but is a whole number of timer counts. + +#if ( configLPTIM_ENABLE_PRECISION != 0 ) + + static int lSubcountErrorPerTick; // A "baseline" tick has this much error, measured in timer + // subcounts. There are configTICK_RATE_HZ subcounts per + // count. When this field is negative, the baseline tick is a + // little too long because we rounded "up" to the nearest + // whole number of counts per tick. When this field is + // positive, the baseline tick is a little too short because + // we rounded "down" to the nearest whole number of counts per + // tick. + + static volatile int lRunningSubcountError; // This error accumulator never exceeds half a count, or + // configTICK_RATE_HZ/2. When this field is negative, the + // next tick is slightly late; when this field is positive, + // the next tick is slightly early. This field allows us to + // schedule each tick on the timer count closest to the ideal + // tick time. + +#endif // configLPTIM_ENABLE_PRECISION + +#if ( configTICK_ENABLE_UWTICK_PRECISION != 0 ) + + static uint32_t uwTickSuppressedSystem; // A "system" tick read suppressed time isTickNowSuppressed + // elapsed function sleep running, for update System sysTick + +#endif // configTICK_ENABLE_UWTICK_PRECISION + +static volatile uint16_t usIdealCmp; // This field doubles as a write cache for LPTIM->CMP and a + // way to remember that we set CMP to 0 because 0xFFFF isn't + // allowed (hardware limitation). + +static volatile uint8_t isCmpWriteInProgress; // This field helps us remember when we're waiting for the + // CMP write to finish. We must not write to CMP while a + // previous write is still in progress. + +static volatile uint8_t isTickNowSuppressed; // This field helps the tick ISR determine whether + // usIdealCmp is in the past or the future. + +// LPTIM Instance Selection +// +// If your MCU has multiple LPTIM instances, you must (1) select the instance you want this software to +// use for the OS tick by updating the the following three #defines, and (2) change the first five statements +// of vPortSetupTimerInterrupt() to match your selection. The default configuration is for LPTIM1 because it +// operates even in the lowest-power STOP level. +// +// If your MCU has only one LPTIM instance, you may or may not need to update these three #defines. But +// you must change the first five statements of vPortSetupTimerInterrupt() to match your STM32. +// +#ifdef configLPTIM_SRC_LPTIM1 + #define LPTIM LPTIM1 + #define LPTIM_IRQn LPTIM1_IRQn + #define LPTIM_IRQHandler LPTIM1_IRQHandler +#endif +#ifdef configLPTIM_SRC_LPTIM2 + #define LPTIM LPTIM2 + #define LPTIM_IRQn LPTIM2_IRQn + #define LPTIM_IRQHandler LPTIM2_IRQHandler +#endif + +// Automatic LPTIM TO LPTIM1 +#ifndef LPTIM +#define LPTIM LPTIM1 +#define LPTIM_IRQn LPTIM1_IRQn +#define LPTIM_IRQHandler LPTIM1_IRQHandler +#define configLPTIM_SRC_LPTIM1 +#endif + +/// @brief This function overrides the "standard" port function, decorated with __attribute__((weak)), in port.c. +/// Call with interrupts masked. +void vPortSetupTimerInterrupt( void ) +{ + // Enable the APB clock to the LPTIM. Then select either LSE or LSI as the kernel clock for the + // LPTIM. Then be sure the LPTIM "freezes" when the debugger stops program execution. Then reset the + // LPTIM just in case it was already in use prior to this function. + // + // Modify these statements as needed for your STM32. See LPTIM Instance Selection (above) for + // additional information. + // + #ifdef configLPTIM_SRC_LPTIM1 + RCC->APB1ENR1 |= RCC_APB1ENR1_LPTIM1EN; + MODIFY_REG(RCC->CCIPR, RCC_CCIPR_LPTIM1SEL, LPTIMSEL_Val << RCC_CCIPR_LPTIM1SEL_Pos); + DBGMCU->APB1FZR1 |= DBGMCU_APB1FZR1_DBG_LPTIM1_STOP; + RCC->APB1RSTR1 |= RCC_APB1RSTR1_LPTIM1RST; // Reset the LPTIM module per erratum 2.14.1. + RCC->APB1RSTR1 &= ~RCC_APB1RSTR1_LPTIM1RST; + #endif + #ifdef configLPTIM_SRC_LPTIM2 + RCC->APB1ENR2 |= RCC_APB1ENR2_LPTIM2EN; + MODIFY_REG(RCC->CCIPR, RCC_CCIPR_LPTIM2SEL, LPTIMSEL_Val << RCC_CCIPR_LPTIM2SEL_Pos); + DBGMCU->APB1FZR2 |= DBGMCU_APB1FZR2_DBG_LPTIM2_STOP; + RCC->APB1RSTR2 |= RCC_APB1RSTR2_LPTIM2RST; // Reset the LPTIM module per erratum 2.14.1. + RCC->APB1RSTR2 &= ~RCC_APB1RSTR2_LPTIM2RST; + #endif + #ifdef STM32WL // <-- "Family" symbol is defined in the ST device header file, e.g., "stm32wlxx.h". + { + #define EXTI_IMR1_LPTIM1 (1UL << 29) + #define EXTI_IMR1_LPTIM2 (1UL << 30) + #define EXTI_IMR1_LPTIM3 (1UL << 31) + + // Users of STM32WL must also change this next statement to match their LPTIM instance selection. + // By default these MCU's disable wake-up from deep sleep via LPTIM. An oversight by ST? + // + #ifdef configLPTIM_SRC_LPTIM1 + EXTI->IMR1 |= EXTI_IMR1_LPTIM1; + #endif + #ifdef configLPTIM_SRC_LPTIM2 + EXTI->IMR1 |= EXTI_IMR1_LPTIM2; + #endif + #ifdef configLPTIM_SRC_LPTIM3 + EXTI->IMR1 |= EXTI_IMR1_LPTIM3; + #endif + } + #endif + + // Be sure the reference clock is ready. If this assertion fails, be sure your application code + // starts the reference clock (LSE or LSI) prior to starting FreeRTOS. + // + configASSERT(IS_REF_CLOCK_READY()); + + // Calculate the constants required to configure the tick interrupt. + // + ulTimerCountsForOneTick = ( LPTIM_CLOCK_HZ + ( configTICK_RATE_HZ / 2 ) ) / configTICK_RATE_HZ; + configASSERT( ulTimerCountsForOneTick >= 4UL ); // CLOCK frequency must be at least 3.5x TICK frequency + + // Calculate the maximum number of ticks we can suppress. Give 1 OS tick of margin between clearly + // future match events and clearly past match events. Anything within the previous one tick is clearly + // past, within one tick before that is in the margin between, which we call the past for convenience. + // Everything else is in the future when isTickNowSuppressed is true and in the past otherwise. And + // set up a couple of other things for the precision feature, if enabled. + + #if ( configLPTIM_ENABLE_PRECISION == 0 ) + { + xMaximumSuppressedTicks = 65536UL / ulTimerCountsForOneTick - 1 - 1; + } + #else + { + xMaximumSuppressedTicks = 65536UL * configTICK_RATE_HZ / LPTIM_CLOCK_HZ - 1 - 1; + + // For convenience, the code above rounded *up* if the ideal number of counts per tick is exactly + // X.5. So we might calculate lSubcountErrorPerTick to be -(configTICK_RATE_HZ/2) but never + // +(configTICK_RATE_HZ/2). If you get a build error on this line, be sure configTICK_RATE_HZ is a + // simple numeric literal (eg, 1000UL) with no C typecasting (eg, (TickType_t) 1000). + // + #if ( LPTIM_CLOCK_HZ % configTICK_RATE_HZ_LITERAL < configTICK_RATE_HZ_LITERAL/2 ) + #define IS_SUBCOUNT_EPT_POSITIVE 1 + #else + #define IS_SUBCOUNT_EPT_POSITIVE 0 + #endif + + lSubcountErrorPerTick = LPTIM_CLOCK_HZ - ( ulTimerCountsForOneTick * configTICK_RATE_HZ ); + configASSERT( lSubcountErrorPerTick != configTICK_RATE_HZ / 2 ); + } + #endif // configLPTIM_ENABLE_PRECISION + + + // Configure and start LPTIM. + // + LPTIM->IER = LPTIM_IER_CMPMIE | LPTIM_IER_CMPOKIE; // Modify this register only when LPTIM is disabled. + LPTIM->CFGR = (0 << LPTIM_CFGR_PRESC_Pos); // Modify this register only when LPTIM is disabled. + LPTIM->CR = LPTIM_CR_ENABLE; + LPTIM->ARR = 0xFFFF; // timer period = ARR + 1. Modify this register only when LPTIM is enabled. + LPTIM->CMP = ulTimerCountsForOneTick; // Modify this register only when LPTIM is enabled. + isCmpWriteInProgress = pdTRUE; + usIdealCmp = ulTimerCountsForOneTick; + #if ( configLPTIM_ENABLE_PRECISION != 0 ) + { + lRunningSubcountError = lSubcountErrorPerTick; + } + #endif // configLPTIM_ENABLE_PRECISION + LPTIM->CR |= LPTIM_CR_CNTSTRT; + + // Enable the timer interrupt at the configured priority. See configTICK_INTERRUPT_PRIORITY for + // important details. + // + NVIC_SetPriority( LPTIM_IRQn, configTICK_INTERRUPT_PRIORITY ); + NVIC_EnableIRQ( LPTIM_IRQn ); +} + +/// @brief This function overrides the "official" port function, decorated with __attribute__((weak)), in port.c. +/// The idle task calls this function with the scheduler suspended, and only when xExpectedIdleTime is >= 2. +/// FreeRTOS version 10.4.0 or newer is recommended to ensure this function doesn't potentially return one +/// OS tick *after* the intended time. +/// @param xExpectedIdleTime +void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime ) +{ + // Limit the time we plan to spend in tickless idle. LPTIM has only so much range. + // + if (xExpectedIdleTime > xMaximumSuppressedTicks) + { + xExpectedIdleTime = xMaximumSuppressedTicks; + } + + // Determine the number of "extra" timer counts to add to the compare register, which is currently set + // for the next tick. Because the next tick is less than one tick away, we know we won't set the compare + // register more than xMaximumSuppressedTicks (in timer counts) from the *current* CNT value. + // + #if ( configLPTIM_ENABLE_PRECISION != 0 ) + uint32_t ulExtraCounts = (xExpectedIdleTime - 1UL) * LPTIM_CLOCK_HZ / configTICK_RATE_HZ; + int32_t lExtraError = (xExpectedIdleTime - 1UL) * LPTIM_CLOCK_HZ % configTICK_RATE_HZ; + #else + uint32_t ulExtraCounts = (xExpectedIdleTime - 1UL) * ulTimerCountsForOneTick; + #endif // configLPTIM_ENABLE_PRECISION + + // Enter a critical section so we can safely check the sleep-mode status. But don't use + // taskENTER_CRITICAL() because on many platforms that function masks interrupts that we need to exit sleep + // mode. We must stay in the critical section until we go to sleep so that any interrupt starting now + // wakes us up from sleep. + // + __disable_irq(); + // __ISB() is not needed here. The CPSID instruction used by __disable_irq() is self synchronizing. + + // If a context switch is pending or a task is waiting for the scheduler to be unsuspended, then + // abandon the low power entry and the critical section. This status cannot change while interrupts are + // masked. + // + if (eTaskConfirmSleepModeStatus() == eAbortSleep) + { + __enable_irq(); + } + else + { + #if ( configLPTIM_ENABLE_PRECISION != 0 ) + { + // Adjust ulExtraCounts if needed to maintain proper alignment. We left lExtraError positive + // above instead of minimizing its absolute value, so we don't need to check the final value of + // lRunningSubcountError for being too negative. + // + lRunningSubcountError += lExtraError; + if (lRunningSubcountError > (int)(configTICK_RATE_HZ/2)) + { + ulExtraCounts++; + lRunningSubcountError -= configTICK_RATE_HZ; + } + } + #endif // configLPTIM_ENABLE_PRECISION + + // Before we suppress the tick by modifying usIdealCmp (and eventually CMP), make a note that the + // tick is now suppressed. The order isn't actually important because we're in a critical section. The + // tick ISR uses this field to help it determine whether usIdealCmp is in the past or in the future. + // + // Our design relies on the tick interrupt having high enough priority that other ISRs can't delay + // the tick ISR too much while isTickNowSuppressed is true. Too much delay would cause the ISR to + // reject the tick at the end of the delay because the tick would appear to be still in the future, but + // we need that tick, either to get us out of the loop below or to help us decide if we reached the tick + // after the loop. + // + isTickNowSuppressed = pdTRUE; + + #if ( configTICK_ENABLE_UWTICK_PRECISION != 0 ) + // Clone uwTick value to calculate systemTick Real value in LowPower Mode from xCompleteTickPeriods + uwTickSuppressedSystem = uwTick; + #endif + + // Add the extra counts to the upcoming timer interrupt. If we can't write to the CMP register + // right now, the ISR for CMPOK will do it for us. If the timer happens to match on the old value + // before the new value takes effect (any time after we mask interrupts above), the tick ISR rejects it + // as a tick when we unmask interrupts below. + // + usIdealCmp += ulExtraCounts; // (usIdealCmp is a uint16_t) + if (!isCmpWriteInProgress) + { + isCmpWriteInProgress = pdTRUE; + LPTIM->CMP = usIdealCmp == 0xFFFF ? 0 : usIdealCmp; // never write 0xFFFF to CMP (HW rule) + } + uint32_t ulExpectedEndCmp = usIdealCmp; + + // Because our implementation uses an interrupt handler to process a successful write to CMP, we + // use a loop here so we won't return to our caller merely for that interrupt. Nor will we return for a + // tick ISR that rejects the tick as described above, nor for any other ISR that doesn't make a task + // ready to execute. And don't worry about ISRs changing how long the OS expects to be idle; FreeRTOS + // doesn't let ISRs do that -- not even the xTimerXyzFromISR() functions. Those functions merely queue + // jobs for the timer task, which from our perspective is a task now ready to execute. + // + // Stay in the loop until an ISR makes a task ready to execute or until the timer reaches the end + // of the sleep period. We identify the end of the sleep period by recognizing that the tick ISR has + // modified usIdealCmp for the next tick after the sleep period ends. + // + do + { + // Give the application a chance to arrange for the deepest sleep it can tolerate right now. + // Also give it an opportunity to provide its own WFI or WFE instruction. (In that case, it sets + // xModifiableIdleTime = 0.) After the sleep, give the application a chance to restore clocks or + // take other recovery measures related to deep sleep. + // + // Note that we don't recalculate xModifiableIdleTime in the case of multiple passes through + // this loop, even though the amount of expected idle time does shrink with each pass. We don't want + // to burden this loop with those calculations because the typical ISR requests a context switch, + // inducing an exit from this loop, not another pass. The CMPOK interrupt is the exception, and is + // the primary reason for this loop, but it comes within one tick anyway, so there is no need to + // recalculate xModifiableIdleTime for that case. + // + TickType_t xModifiableIdleTime = xExpectedIdleTime; + configPRE_SLEEP_PROCESSING( xModifiableIdleTime ); + if (xModifiableIdleTime > 0) + { + // // Wait for an interrupt. + // // + // __DSB(); + // __WFI(); + // __ISB(); // required when debugging in low-power modes (ie, DBGMCU->CR != 0) on some MCUs. + } + configPOST_SLEEP_PROCESSING( (const TickType_t)xExpectedIdleTime ); + + // Re-enable interrupts, and then execute the ISR tied to the interrupt that brought the MCU out + // of sleep mode. + // + __enable_irq(); + __ISB(); // ISB is recommended by ARM; not strictly needed in Cortex-M when __disable_irq() is next. + + // Disable interrupts for our call to eTaskConfirmSleepModeStatus() and in case we iterate again + // in the loop. + // + __disable_irq(); + // __ISB() is not needed here. The CPSID instruction used by __disable_irq() is self synchronizing. + + } while (usIdealCmp == ulExpectedEndCmp && eTaskConfirmSleepModeStatus() != eAbortSleep); + + // Re-enable interrupts. We try our best to support short ISR latency, especially for interrupt + // priorities higher than configMAX_SYSCALL_INTERRUPT_PRIORITY. + // + __enable_irq(); + + // Determine how many tick periods elapsed during our sleep. And if something other than the tick + // timer woke us up, reschedule the tick that would normally come after the ones we've just skipped (if + // any). + // + // Begin by assuming we managed to stay asleep the entire time. In that case, the tick ISR already + // added one tick (well, actually the ISR "pended" the increment because the scheduler is currently + // suspended, but it's all the same to us), so we use "- 1" here. + // + TickType_t xCompleteTickPeriods = xExpectedIdleTime - (TickType_t) 1; + + // We identify that we reached the end of the expected idle time by noting that the tick ISR has + // modified usIdealCmp. So if it hasn't, then we probably have to reschedule the next tick as described + // above. We temporarily mask the tick interrupt while we make the assessment and manipulate usIdealCmp + // (and CMP) if necessary. We also mask any interrupts at or below its interrupt priority since those + // interrupts are allowed to use consecutive execution time enough to cause us to miss ticks. + // + portDISABLE_INTERRUPTS(); + if (usIdealCmp == ulExpectedEndCmp) + { + // Something else woke us up. See how many timer counts we still had left, and then use that + // number to determine how many OS ticks actually elapsed. Then reschedule the next tick exactly + // where it would have been. + + // Get a coherent copy of the current count value in the timer. The CNT register is clocked + // asynchronously, so we keep reading it until we get the same value during a verification read. + // + uint32_t ulCurrCount; + do ulCurrCount = LPTIM->CNT; while (ulCurrCount != LPTIM->CNT); + + // See how many timer counts we still had left, but don't include the timer count currently + // underway. If a tick happens to align with the beginning of the timer count currently underway, we + // consider it already suppressed. Or, in the case we didn't suppress any ticks, if the timer count + // currently underway is the first one of a tick period, then that tick must have happened before the + // idle task disabled the scheduler and called this function. We don't want to reschedule *that* + // interrupt. + // + // Several conditions can cause ulFullCountsLeft to be "negative" here, meaning that we actually + // have reached the end of the expected idle time and now merely need to allow the ISR to execute. + // First, CNT may have incremented after we masked interrupts but before we captured ulCurrCount. + // Second, LPTIM generates the CMPM IRQ one count *after* the match event. Third, when usIdealCmp is + // 0xFFFF, we set CMP to 0x0000, which when combined with the first two cases, could easily leave + // countsLeft set to -3 (65533). And finally, interrupts with priority above + // configMAX_SYSCALL_INTERRUPT_PRIORITY can delay our capture of ulCurrCount. That delay is limited + // to less than one tick duration by stated requirement. No need to do anything if ulFullCountsLeft + // is "negative". In fact there's no need to do anything if ulFullCountsLeft is less than a whole + // tick, but xFullTicksLeft (below) determines that. + // + uint32_t ulFullCountsLeft = (uint16_t)(usIdealCmp - ulCurrCount - 1UL); + #if ( configLPTIM_ENABLE_PRECISION == 0 ) + if (ulFullCountsLeft < xMaximumSuppressedTicks * ulTimerCountsForOneTick) + #else + if (ulFullCountsLeft <= xMaximumSuppressedTicks * LPTIM_CLOCK_HZ / configTICK_RATE_HZ) // See below + #endif // configLPTIM_ENABLE_PRECISION + { + // Now calculate how many "full" or whole ticks we had left in our expected idle time. If + // there are zero full ticks left, then the next scheduled tick interrupt is the one we want + // anyway. But if the time left amounts to at least one full tick, then reschedule the first tick + // interrupt that we haven't yet skipped. And update xCompleteTickPeriods not to count the ones + // we haven't skipped. + // + TickType_t xFullTicksLeft; + #if ( configLPTIM_ENABLE_PRECISION != 0 ) + { + xFullTicksLeft = ulFullCountsLeft * configTICK_RATE_HZ / LPTIM_CLOCK_HZ; + if (xFullTicksLeft == xExpectedIdleTime) + { + // For efficiency, the rescheduling code below might count a tick that occurs at the + // very next timer count, even before that count occurs, instead scheduling the subsequent + // tick. With a slow timer, the timer might *still* not have made that count as we arrive + // here again. Correct xFullTicksLeft for that case here. This same case can cause + // ulFullCountsLeft to be equal to the max above (and not necessarily less) -- see above. + // + xFullTicksLeft = xExpectedIdleTime - (TickType_t)1; + } + } + #else + { + xFullTicksLeft = ulFullCountsLeft / ulTimerCountsForOneTick; + } + #endif // configLPTIM_ENABLE_PRECISION + + // Verify that we calculated a sensible number of full ticks remaining in the expected idle + // time. If this assertion fails, then the tick ISR was delayed too long by other ISR(s). You + // must either reduce the execution times of your ISRs, decrease their priorities, or increase the + // priority of the tick ISR. See the description of configTICK_INTERRUPT_PRIORITY for details. + // + configASSERT( xFullTicksLeft < xExpectedIdleTime ); + + if (xFullTicksLeft != 0) + { + xCompleteTickPeriods -= xFullTicksLeft; + + // Reschedule the next timer interrupt; it's one we had expected to suppress. Also, + // correct lRunningSubcountError because it still reflects the error we'll have at the end of + // the expected idle time. Modify it to reflect the error at the time of the tick interrupt + // we're rescheduling now. + // + uint32_t ulFullTicksLeftAsCounts; + #if ( configLPTIM_ENABLE_PRECISION != 0 ) + { + ulFullTicksLeftAsCounts = xFullTicksLeft * LPTIM_CLOCK_HZ / configTICK_RATE_HZ; + lExtraError = xFullTicksLeft * LPTIM_CLOCK_HZ % configTICK_RATE_HZ; + lRunningSubcountError -= lExtraError; + if (lRunningSubcountError < -(int)(configTICK_RATE_HZ/2)) + { + ulFullTicksLeftAsCounts++; + lRunningSubcountError += configTICK_RATE_HZ; + } + } + #else + { + ulFullTicksLeftAsCounts = xFullTicksLeft * ulTimerCountsForOneTick; + } + #endif // configLPTIM_ENABLE_PRECISION + + usIdealCmp -= ulFullTicksLeftAsCounts; // usIdealCmp is a uint16_t + if (!isCmpWriteInProgress) + { + isCmpWriteInProgress = pdTRUE; + LPTIM->CMP = usIdealCmp == 0xFFFF ? 0 : usIdealCmp; // never write 0xFFFF to CMP (HW rule) + } + } + } + } + + // Set isTickNowSuppressed back to false before we unmask the tick interrupt so the ISR has a + // chance to identify any ticks missed while we had the tick interrupt masked. Any missed ticks here + // indicate a configuration error. No combination of ISRs above the tick priority may execute for + // longer than a tick. See configTICK_INTERRUPT_PRIORITY. + // + isTickNowSuppressed = pdFALSE; + portENABLE_INTERRUPTS(); + + // Increment the tick count by the number of (full) "extra" ticks we waited. Function + // vTaskStepTick() asserts that this function didn't oversleep. Note that we don't have to worry about + // modifying xTickCount count here while the tick count ISR is enabled because the scheduler is + // currently suspended. That causes the tick ISR to accumulate ticks into a pended-ticks field. + // + vTaskStepTick( xCompleteTickPeriods ); + + #if ( configTICK_ENABLE_UWTICK_PRECISION != 0 ) + // Adding uwTick Offset to systemTick in LowPower Mode from xCompleteTickPeriods + // Check expected period to real tick executed in lowPowerMode and update SysTick + if(xCompleteTickPeriods > (uwTick - uwTickSuppressedSystem)) { + uwTick = uwTick + ((uint32_t)(xCompleteTickPeriods - (uwTick - uwTickSuppressedSystem))); + } + #endif + + } +} + +/// @brief LPTIM_IRQHandler +void LPTIM_IRQHandler( void ) +{ + // Whether we are running this ISR for a CMPOK interrupt or a CMPM interrupt (or both), we need to see + // if it's time for a tick. Think of it as interrupt-induced polling. The synchronization mechanism that + // controls writes to CMP can cause us to miss CMPM events or to delay them while we wait to write the new + // CMP value while a previous write is still ongoing. + // + // There are two ways we can lose a CMPM interrupt. First, with very high CMP values (near 0xFFFF), + // if the counter reaches 0xFFFF before the sync mechanism finishes, the timer won't identify the match. + // And second, with *any* CMP value that becomes available to the timer a little too late, the timer won't + // identify a *new* match if the previous CMP value already matched and was lower than the new CMP value. + // Either one of these conditions might occur in a single execution of vPortSuppressTicksAndSleep(). When + // that function stops a sleep operation early due to an application interrupt, it tries to revert to + // wherever the next scheduled tick should be. In so doing, it may write a CMP value that is imminent. + // That CMP value may be "very high", or it may be numerically larger than the previous compare value, as + // it would be for a reversion that "unwraps" from a CMP value in the next counter epoch back through + // 0xFFFF to a CMP value in the current epoch. + // + // So we proceed now to identify a tick, whether we have a CMPM interrupt or not. + + // Acknowledge and clear the CMPM event. Based on the errata, we dare not clear this flag unless it + // is already set. Call it an over-abundance of caution. + // + if (LPTIM->ISR & LPTIM_ISR_CMPM) + { + LPTIM->ICR = LPTIM_ICR_CMPMCF; + } + + // Get a coherent copy of the current count value in the timer. The CNT register is clocked + // asynchronously, so we keep reading it until we get the same value during a verification read. Then + // determine how "late" we are in responding to the timer interrupt request. Normally, we're one count + // late because LPTIM raises the CMPM interrupt one count *after* the match event. Use usIdealCmp in this + // determination because it reliably reflects the match time we want right now, regardless of the sync + // mechanism for CMP. + // + uint32_t ulCountValue; + do ulCountValue = LPTIM->CNT; while (ulCountValue != LPTIM->CNT); + uint32_t ulCountsLate = (uint16_t)(ulCountValue - usIdealCmp); + + // If we're more than one full tick late, then the application masked interrupts for too long. That + // condition is typically an indication of a design flaw, so we don't take heroic measures here to avoid + // dropping the tick(s) we missed or to keep upcoming ticks in proper phase. Instead, we just resume the + // tick starting right away. Without this correction, a "missed" tick could drop all ticks for a long time + // -- until the timer came back around to the CMP value again. + // + // Be sure not to misinterpret other conditions though. When the tick is suppressed and scheduled for + // more than one tick from now, it looks like we're late. And when the tick is not suppressed, and we're + // still waiting for an upcoming tick, it looks like we're very, very late. In those cases, we're not + // actually late, and there is no tick right now. + // + if (ulCountsLate >= ulTimerCountsForOneTick && + ulCountsLate < 65536UL - 1 - ulTimerCountsForOneTick && + !isTickNowSuppressed) + { + // Optionally report the number of ticks dropped. (No need for precision here.) Then arrange to + // count the tick (below) and to schedule the next tick based on the current timer value. + // + traceTICKS_DROPPED( ulCountsLate / ulTimerCountsForOneTick ); + usIdealCmp = ulCountValue; + ulCountsLate = 0; + } + + // If the ideal CMP value is in the recent past -- within one OS tick time -- then count the tick. + // + // The condition of this one "if" statement handles several important cases. First, it temporarily + // ignores the unwanted match condition that occurs when vPortSuppressTicksAndSleep() wraps CMP into the + // next timer epoch. Second, it helps us ignore an imminent tick that vPortSuppressTicksAndSleep() is + // trying to suppress but may occur anyway due to the CMP sync mechanism. Third, it helps us honor an + // interrupt that vPortSuppressTicksAndSleep() has restored even if it happens a little bit late due to the + // sync mechanism, and even if that interrupt occurs before its corresponding CMPOK event occurs (happens + // occasionally). And finally, it helps us honor a tick that vPortSuppressTicksAndSleep() has restored, + // but when the timer appears to have missed the match completely as explained above. + // + if (ulCountsLate < ulTimerCountsForOneTick) + { + // We officially have an OS tick. Count it, and set up the next one. + + uint32_t ulNumCounts = ulTimerCountsForOneTick; + #if ( configLPTIM_ENABLE_PRECISION != 0 ) + { + lRunningSubcountError += lSubcountErrorPerTick; + #if ( IS_SUBCOUNT_EPT_POSITIVE ) + { + if (lRunningSubcountError >= (int)(configTICK_RATE_HZ/2)) + { + ulNumCounts++; + lRunningSubcountError -= configTICK_RATE_HZ; + } + } + #else + { + if (lRunningSubcountError <= -(int)(configTICK_RATE_HZ/2)) + { + ulNumCounts--; + lRunningSubcountError += configTICK_RATE_HZ; + } + } + #endif // IS_SUBCOUNT_EPT_POSITIVE + } + #endif // configLPTIM_ENABLE_PRECISION + + // Set up the next tick interrupt. We must check isCmpWriteInProgress here as usual, in case CMPM + // can come before CMPOK. + // + usIdealCmp += ulNumCounts; // usIdealCmp is a uint16_t + if (!isCmpWriteInProgress) + { + LPTIM->CMP = usIdealCmp == 0xFFFF ? 0 : usIdealCmp; // never write 0xFFFF to CMP (HW rule) + isCmpWriteInProgress = pdTRUE; + } + + // Tell the OS about the tick. We don't need to bother saving and restoring the current BASEPRI + // setting because (1) we cannot possibly already be in a critical section and (2) the NVIC won't take + // interrupts of lower priority than LPTIM even when we set BASEPRI to zero until our ISR ends. + // + portDISABLE_INTERRUPTS(); + BaseType_t xWasHigherPriorityTaskWoken = xTaskIncrementTick(); + portENABLE_INTERRUPTS(); + + portYIELD_FROM_ISR(xWasHigherPriorityTaskWoken); + } + + + // Now that we've given as much time as possible for any CMP write to be finished, see if it has + // finished. We may have a new value to write after handling CMPM above. Handling CMPOK last in this ISR + // isn't very important, but it is a slight optimization over other ordering. + // + if (LPTIM->ISR & LPTIM_ISR_CMPOK) + { + // Acknowledge and clear the CMPOK event. + // + LPTIM->ICR = LPTIM_ICR_CMPOKCF; + + // If there is a "pending" write operation to CMP, do it now. Otherwise, make note that the write + // is now complete. Remember to watch for CMP set to 0 when usIdealCmp is 0xFFFF. There's no pending + // write in that case. + // + if ((uint16_t)(LPTIM->CMP - usIdealCmp) > 1UL) + { + LPTIM->CMP = usIdealCmp == 0xFFFF ? 0 : usIdealCmp; // never write 0xFFFF to CMP (HW rule) + // isCmpWriteInProgress = pdTRUE; // already true here in the handler for write completed + } + else + { + isCmpWriteInProgress = pdFALSE; + } + } +} + +#endif // configUSE_TICKLESS_IDLE == 2 \ No newline at end of file diff --git a/platformio/stima_v4/slave-leaf/src/drivers/module_slave_hal.cpp b/platformio/stima_v4/slave-leaf/src/drivers/module_slave_hal.cpp new file mode 100644 index 000000000..765e11faf --- /dev/null +++ b/platformio/stima_v4/slave-leaf/src/drivers/module_slave_hal.cpp @@ -0,0 +1,658 @@ +/** + ****************************************************************************** + * @file module_slave_hal.cpp + * @author Moreno Gasperini + * @brief Interface STM32 hardware_hal STIMAV4 + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#include "drivers/module_slave_hal.hpp" +#include "stm32l4xx_ll_system.h" +#include "stm32l4xx_ll_cortex.h" + +#if (ENABLE_I2C2) +TwoWire Wire2 = TwoWire(PIN_I2C2_SDA, PIN_I2C2_SCL); +#endif + +/* Private Hardware_Handler istance initialization ---------------------------------------*/ +#if (ENABLE_CAN) +CAN_HandleTypeDef hcan1; +#endif +#if (ENABLE_QSPI) +QSPI_HandleTypeDef hqspi; +#endif + +// ******************************************************************************** +// LOCAL REDEFINITION Weak PinMap ARDUINO for SETUP PIN Base or Alternate Function +// ******************************************************************************** + +#ifdef HAL_ADC_MODULE_ENABLED +const PinMap PinMap_ADC[] = { + {PA_4, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 9, 0)}, // ADC1_IN9 + {PC_0, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 1, 0)}, // ADC1_IN1 + {PC_1, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 2, 0)}, // ADC1_IN2 + {PC_2, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 3, 0)}, // ADC1_IN3 + {PC_3, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 4, 0)}, // ADC1_IN4 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_DAC_MODULE_ENABLED +const PinMap PinMap_DAC[] = { + {NC, NP, 0} +}; +#endif + +#ifdef HAL_I2C_MODULE_ENABLED +const PinMap PinMap_I2C_SDA[] = { + {PB_7, I2C1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, + {PB_14, I2C2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, + {NC, NP, 0} +}; +const PinMap PinMap_I2C_SCL[] = { + {PB_6, I2C1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, + {PB_10, I2C2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_TIM_MODULE_ENABLED +const PinMap PinMap_TIM[] = { + {NC, NP, 0} +}; +#endif + +#ifdef HAL_UART_MODULE_ENABLED +const PinMap PinMap_UART_TX[] = { + {PA_2_ALT1, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PA_9, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {NC, NP, 0} +}; +const PinMap PinMap_UART_RX[] = { + {PA_10, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PA_15, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_USART2)}, + {NC, NP, 0} +}; +const PinMap PinMap_UART_RTS[] = { + {PA_1, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {NC, NP, 0} +}; +const PinMap PinMap_UART_CTS[] = { + {PA_0, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SPI_MODULE_ENABLED +const PinMap PinMap_SPI_MOSI[] = { + {PA_12, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {NC, NP, 0} +}; +const PinMap PinMap_SPI_MISO[] = { + {PA_11, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {NC, NP, 0} +}; +const PinMap PinMap_SPI_SCLK[] = { + {PA_5, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {NC, NP, 0} +}; +const PinMap PinMap_SPI_SSEL[] = { + {NC, NP, 0} +}; +#endif + +#ifdef HAL_CAN_MODULE_ENABLED +const PinMap PinMap_CAN_RD[] = { + {PB_12, CAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF10_CAN1)}, + {NC, NP, 0} +}; +const PinMap PinMap_CAN_TD[] = { + {PB_13, CAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF10_CAN1)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_QSPI_MODULE_ENABLED +const PinMap PinMap_QUADSPI_DATA0[] = { + {PB_1, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_IO0 + {NC, NP, 0} +}; +const PinMap PinMap_QUADSPI_DATA1[] = { + {PB_0, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_IO1 + {NC, NP, 0} +}; +const PinMap PinMap_QUADSPI_DATA2[] = { + {PA_7, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_IO2 + {NC, NP, 0} +}; +const PinMap PinMap_QUADSPI_DATA3[] = { + {PA_6, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_IO3 + {NC, NP, 0} +}; +const PinMap PinMap_QUADSPI_SCLK[] = { + {PA_3, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_CLK + {NC, NP, 0} +}; +const PinMap PinMap_QUADSPI_SSEL[] = { + {PB_11, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_NCS + {NC, NP, 0} +}; +#endif + +#if defined(HAL_PCD_MODULE_ENABLED) || defined(HAL_HCD_MODULE_ENABLED) +const PinMap PinMap_USB[] = { + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +const PinMap PinMap_SD[] = { + {NC, NP, 0} +}; +#endif + +/* Private Hardware_Handler istance initialization ---------------------------------------*/ + +/******************************************************************************************* +******************************************************************************************** + System clock and private setup PIN (VBat/Chg, PLLSynch) +******************************************************************************************** +*******************************************************************************************/ +/// @brief System Clock Configuration +extern "C" void SystemClock_Config(void) +{ + RCC_OscInitTypeDef RCC_OscInitStruct = {0}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; + RCC_CRSInitTypeDef RCC_CRSInitStruct = {0}; + + /** Configure the main internal regulator output voltage + */ + if (HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1) != HAL_OK) + { + Error_Handler(); + } + + /** Configure LSE Drive Capability + */ + HAL_PWR_EnableBkUpAccess(); + __HAL_RCC_LSEDRIVE_CONFIG(RCC_LSEDRIVE_LOW); + + /** Initializes the RCC Oscillators according to the specified parameters + * in the RCC_OscInitTypeDef structure. + */ + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_LSE|RCC_OSCILLATORTYPE_MSI; + RCC_OscInitStruct.LSEState = RCC_LSE_ON; + RCC_OscInitStruct.MSIState = RCC_MSI_ON; + RCC_OscInitStruct.MSICalibrationValue = 0; + RCC_OscInitStruct.MSIClockRange = RCC_MSIRANGE_6; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_MSI; + RCC_OscInitStruct.PLL.PLLM = 1; + RCC_OscInitStruct.PLL.PLLN = 40; + RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV7; + RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2; + RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2; + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) + { + Error_Handler(); + } + + /** Initializes the CPU, AHB and APB buses clocks + */ + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK + |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1; + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK) + { + Error_Handler(); + } + + /** Enable MSI Auto calibration + */ + HAL_RCCEx_EnableMSIPLLMode(); + + /** Enable the SYSCFG APB clock + */ + __HAL_RCC_CRS_CLK_ENABLE(); + + /** Configures CRS + */ + RCC_CRSInitStruct.Prescaler = RCC_CRS_SYNC_DIV1; + RCC_CRSInitStruct.Source = RCC_CRS_SYNC_SOURCE_LSE; + RCC_CRSInitStruct.Polarity = RCC_CRS_SYNC_POLARITY_RISING; + RCC_CRSInitStruct.ReloadValue = __HAL_RCC_CRS_RELOADVALUE_CALCULATE(48000000,32768); + RCC_CRSInitStruct.ErrorLimitValue = 34; + RCC_CRSInitStruct.HSI48CalibrationValue = 32; + + HAL_RCCEx_CRSConfig(&RCC_CRSInitStruct); +} + +/// @brief Startup PeripheralConfig Local Board +void SetupSystemPeripheral(void) { + + FLASH_OBProgramInitTypeDef OBInit; // flash option bytes copy + + /* get flash option bytes */ + OBInit.WRPArea = OB_WRPAREA_BANK1_AREAA; + OBInit.PCROPConfig = FLASH_BANK_1; + HAL_FLASHEx_OBGetConfig(&OBInit); + + /* config flash option bytes */ + if ((OBInit.USERConfig & OB_IWDG_STOP_RUN) == OB_IWDG_STOP_RUN) { + OBInit.OptionType = OPTIONBYTE_USER; + OBInit.USERType = OB_USER_IWDG_STOP | OB_USER_IWDG_STDBY; + OBInit.USERConfig = OB_IWDG_STOP_FREEZE | OB_IWDG_STDBY_FREEZE; + + HAL_FLASH_Unlock(); + HAL_FLASH_OB_Unlock(); + HAL_FLASHEx_OBProgram(&OBInit); + HAL_FLASH_OB_Launch(); + HAL_FLASH_OB_Lock(); + HAL_FLASH_Lock(); + } + + /** Enable cycle counter (for debug) **/ + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + DWT->CYCCNT = 0; + DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; + + // Enable Handler Fault Error + LL_HANDLER_EnableFault(LL_HANDLER_FAULT_USG); + LL_HANDLER_EnableFault(LL_HANDLER_FAULT_BUS); + LL_HANDLER_EnableFault(LL_HANDLER_FAULT_MEM); + + MX_GPIO_Init(); + + #if (ENABLE_CAN) + MX_CAN1_Init(); + #endif + #if (ENABLE_QSPI) + MX_QUADSPI_Init(); + #endif + + /* Abilito la carica del supercap */ + HAL_PWREx_EnableBatteryCharging(PWR_BATTERY_CHARGING_RESISTOR_5); +} + +/// @brief Get Unique ID HW of CPU (SerialNumber Unique ID) +/// @param ptrCpuId pointer to external 12 byte buffer required (CPU_ID) +void STM32L4GetCPUID(uint8_t *ptrCpuId) { + for(uint8_t uid=0; uid<12; uid++) { + ptrCpuId[uid++] = (uint8_t)(READ_REG(*((uint32_t *)(UID_BASE_ADDRESS + uid)))); + } +} + +/// @brief Get StimaV4 Serial Number from UID Cpu and Module TYPE +/// @return Serial Number 64 BIT +uint64_t StimaV4GetSerialNumber(void) { + volatile uint64_t serNumb = 0; + uint8_t *ptrData = (uint8_t*)&serNumb; + ptrData[0] = MODULE_TYPE; + ptrData[1] = (uint8_t)(READ_REG(*((uint32_t *)(UID_BASE_ADDRESS)))); + ptrData[2] = (uint8_t)(READ_REG(*((uint32_t *)(UID_BASE_ADDRESS + 10)))); + ptrData[3] = (uint8_t)(READ_REG(*((uint32_t *)(UID_BASE_ADDRESS + 9)))); + ptrData[4] = (uint8_t)(READ_REG(*((uint32_t *)(UID_BASE_ADDRESS + 7)))); + ptrData[5] = (uint8_t)(READ_REG(*((uint32_t *)(UID_BASE_ADDRESS + 6)))); + ptrData[6] = (uint8_t)(READ_REG(*((uint32_t *)(UID_BASE_ADDRESS + 5)))); + ptrData[7] = (uint8_t)(READ_REG(*((uint32_t *)(UID_BASE_ADDRESS + 4)))); + return serNumb; +} + +/******************************************************************************************* +******************************************************************************************** + System base Hardware Istance and private Initialization +******************************************************************************************** +*******************************************************************************************/ +#if (ENABLE_CAN) +/// @brief CAN1 Initialization Function +extern "C" void MX_CAN1_Init(void) +{ + CAN_FilterTypeDef CAN_FilterInitStruct; + + // Default Speed Base CAN Setup module 1Mhz + hcan1.Instance = CAN1; + hcan1.Init.Prescaler = 8; + hcan1.Init.Mode = CAN_MODE_NORMAL; + hcan1.Init.SyncJumpWidth = CAN_SJW_1TQ; + hcan1.Init.TimeSeg1 = CAN_BS1_8TQ; + hcan1.Init.TimeSeg2 = CAN_BS2_2TQ; + hcan1.Init.TimeTriggeredMode = DISABLE; + hcan1.Init.AutoBusOff = DISABLE; + hcan1.Init.AutoWakeUp = DISABLE; + hcan1.Init.AutoRetransmission = DISABLE; + hcan1.Init.ReceiveFifoLocked = DISABLE; + hcan1.Init.TransmitFifoPriority = DISABLE; + if (HAL_CAN_Init(&hcan1) != HAL_OK) { + Error_Handler(); + } + + // CAN filter basic initialization + CAN_FilterInitStruct.FilterIdHigh = 0x0000; + CAN_FilterInitStruct.FilterIdLow = 0x0000; + CAN_FilterInitStruct.FilterMaskIdHigh = 0x0000; + CAN_FilterInitStruct.FilterMaskIdLow = 0x0000; + CAN_FilterInitStruct.FilterFIFOAssignment = CAN_RX_FIFO0; + CAN_FilterInitStruct.FilterBank = 0; + CAN_FilterInitStruct.FilterMode = CAN_FILTERMODE_IDMASK; + CAN_FilterInitStruct.FilterScale = CAN_FILTERSCALE_32BIT; + CAN_FilterInitStruct.FilterActivation = ENABLE; + + // Check error initalization CAN filter + if (HAL_CAN_ConfigFilter(&hcan1, &CAN_FilterInitStruct) != HAL_OK) { + Error_Handler(); + } +} +#endif + +#if (ENABLE_QSPI) +/// @brief QUADSPI Initialization Function +extern "C" void MX_QUADSPI_Init(void) +{ + /* USER CODE BEGIN QUADSPI_Init 0 */ + + /* USER CODE END QUADSPI_Init 0 */ + + /* USER CODE BEGIN QUADSPI_Init 1 */ + + /* USER CODE END QUADSPI_Init 1 */ + /* QUADSPI parameter configuration*/ + hqspi.Instance = QUADSPI; + hqspi.Init.ClockPrescaler = 1; + hqspi.Init.FifoThreshold = 4; + hqspi.Init.SampleShifting = QSPI_SAMPLE_SHIFTING_NONE; + hqspi.Init.FlashSize = 20; + hqspi.Init.ChipSelectHighTime = QSPI_CS_HIGH_TIME_1_CYCLE; + hqspi.Init.ClockMode = QSPI_CLOCK_MODE_0; + hqspi.Init.FlashID = QSPI_FLASH_ID_1; + hqspi.Init.DualFlash = QSPI_DUALFLASH_DISABLE; + if (HAL_QSPI_Init(&hqspi) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN QUADSPI_Init 2 */ + + /* USER CODE END QUADSPI_Init 2 */ +} +#endif + +/// @brief GPIO Initialization Function +extern "C" void MX_GPIO_Init(void) +{ + GPIO_InitTypeDef GPIO_InitStruct = {0}; + + /* GPIO Ports Clock Enable */ + __HAL_RCC_GPIOC_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + __HAL_RCC_GPIOD_CLK_ENABLE(); +#ifdef STIMAV4_SLAVE_HW_VER_01_01 + __HAL_RCC_GPIOH_CLK_ENABLE(); +#endif + + /*Configure GPIO pins : DEN_Pin DSEL0_Pin DSEL1_Pin PW0_Pin + PW1_Pin PW2_Pin PW3_Pin I2C2_EN_Pin + EN_CAN_Pin */ + GPIO_InitStruct.Pin = DEN_Pin|DSEL0_Pin|DSEL1_Pin|PW0_Pin + |PW1_Pin|PW2_Pin|PW3_Pin|I2C2_EN_Pin + |EN_CAN_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + + /*Configure GPIO pins : IN0_Pin FAULT_Pin IN2_Pin */ + GPIO_InitStruct.Pin = IN0_Pin|FAULT_Pin|IN2_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + /*Configure GPIO pins : STB_CAN_Pin EN_5VS_Pin EN_5VA_Pin */ + GPIO_InitStruct.Pin = STB_CAN_Pin|EN_5VS_Pin|EN_5VA_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + /*Configure GPIO pin : IN1_Pin */ + GPIO_InitStruct.Pin = IN1_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(IN1_GPIO_Port, &GPIO_InitStruct); + + /*Configure GPIO pin : EN_SPLY_Pin */ + GPIO_InitStruct.Pin = EN_SPLY_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + HAL_GPIO_Init(EN_SPLY_GPIO_Port, &GPIO_InitStruct); + +#ifdef STIMAV4_SLAVE_HW_VER_01_01 + /*Configure GPIO pin : IN_BTN_Pin */ + GPIO_InitStruct.Pin = IN_BTN_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(IN_BTN_GPIO_Port, &GPIO_InitStruct); +#endif + + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(GPIOC, DEN_Pin|DSEL0_Pin|DSEL1_Pin|PW0_Pin + |PW1_Pin|PW2_Pin|PW3_Pin|I2C2_EN_Pin + |EN_CAN_Pin, GPIO_PIN_RESET); + + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(GPIOB, STB_CAN_Pin|EN_5VS_Pin|EN_5VA_Pin, GPIO_PIN_RESET); + + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(EN_SPLY_GPIO_Port, EN_SPLY_Pin, GPIO_PIN_RESET); +} + +/******************************************************************************************* +******************************************************************************************** + PRIVATE HAL_MspInit_XXModule +******************************************************************************************** +*******************************************************************************************/ + +/// @brief Initializes the Global MSP +extern "C" void HAL_MspInit(void) +{ + /* USER CODE BEGIN MspInit 0 */ + + /* USER CODE END MspInit 0 */ + PWR_PVDTypeDef sConfigPVD = {0}; + + // Control debugger StopMode + if (CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk) { + LL_DBGMCU_EnableDBGStopMode(); + } else { + LL_DBGMCU_DisableDBGStopMode(); + } + + __HAL_RCC_SYSCFG_CLK_ENABLE(); + __HAL_RCC_PWR_CLK_ENABLE(); + + /** PVD Configuration + */ + sConfigPVD.PVDLevel = PWR_PVDLEVEL_0; + sConfigPVD.Mode = PWR_PVD_MODE_NORMAL; + HAL_PWR_ConfigPVD(&sConfigPVD); + + /** Enable the PVD Output + */ + HAL_PWR_EnablePVD(); + + /* USER CODE BEGIN MspInit 1 */ + + /* USER CODE END MspInit 1 */ +} + +#if (ENABLE_CAN) +/// @brief CAN MSP Initialization. This function configures the hardware resources used in this example +/// @param hcan: CAN handle pointer +extern "C" void HAL_CAN_MspInit(CAN_HandleTypeDef* hcan) +{ + GPIO_InitTypeDef GPIO_InitStruct = {0}; + if(hcan->Instance==CAN1) + { + /* USER CODE BEGIN CAN1_MspInit 0 */ + + /* USER CODE END CAN1_MspInit 0 */ + + // GPIO Ports clock enable + __HAL_RCC_GPIOB_CLK_ENABLE(); + + // CAN1 clock enable + __HAL_RCC_CAN1_CLK_ENABLE(); + + // Mapping GPIO for CAN + /* Configure CAN pin: RX */ + GPIO_InitStruct.Pin = GPIO_PIN_12; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF10_CAN1; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + /* Configure CAN pin: TX */ + GPIO_InitStruct.Pin = GPIO_PIN_13; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_PULLUP; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF10_CAN1; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + /* USER CODE BEGIN CAN1_MspInit 1 */ + + /* USER CODE END CAN1_MspInit 1 */ + } +} + +/// @brief CAN MSP De-Initialization. This function freeze the hardware resources used in this example +/// @param hcan: CAN handle pointer +extern "C" void HAL_CAN_MspDeInit(CAN_HandleTypeDef* hcan) +{ + if(hcan->Instance==CAN1) + { + /* USER CODE BEGIN CAN1_MspDeInit 0 */ + + /* USER CODE END CAN1_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_CAN1_CLK_DISABLE(); + + /**CAN1 GPIO Configuration + PB12 ------> CAN1_RX + PB13 ------> CAN1_TX + */ + HAL_GPIO_DeInit(GPIOB, GPIO_PIN_12|GPIO_PIN_13); + + /* USER CODE BEGIN CAN1_MspDeInit 1 */ + + /* USER CODE END CAN1_MspDeInit 1 */ + } + +} +#endif + +#if (ENABLE_QSPI) +/// @brief QSPI MSP Initialization. This function configures the hardware resources used in this example +/// @param hqspi: QSPI handle pointer +extern "C" void HAL_QSPI_MspInit(QSPI_HandleTypeDef* hqspi) +{ + GPIO_InitTypeDef GPIO_InitStruct = {0}; + if(hqspi->Instance==QUADSPI) + { + /* USER CODE BEGIN QUADSPI_MspInit 0 */ + + /* USER CODE END QUADSPI_MspInit 0 */ + /* Peripheral clock enable */ + __HAL_RCC_QSPI_CLK_ENABLE(); + + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + /**QUADSPI GPIO Configuration + PA3 ------> QUADSPI_CLK + PA6 ------> QUADSPI_BK1_IO3 + PA7 ------> QUADSPI_BK1_IO2 + PB0 ------> QUADSPI_BK1_IO1 + PB1 ------> QUADSPI_BK1_IO0 + PB11 ------> QUADSPI_BK1_NCS + */ + GPIO_InitStruct.Pin = GPIO_PIN_3|GPIO_PIN_6|GPIO_PIN_7; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF10_QUADSPI; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_11; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF10_QUADSPI; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + /* QUADSPI interrupt Init */ + HAL_NVIC_SetPriority(QUADSPI_IRQn, QSPI_NVIC_INT_PREMPT_PRIORITY, 0); + HAL_NVIC_EnableIRQ(QUADSPI_IRQn); + /* USER CODE BEGIN QUADSPI_MspInit 1 */ + + /* USER CODE END QUADSPI_MspInit 1 */ + } + +} + +/// @brief QSPI MSP De-Initialization. This function freeze the hardware resources used in this example +/// @param hqspi: QSPI handle pointer +void HAL_QSPI_MspDeInit(QSPI_HandleTypeDef* hqspi) +{ + if(hqspi->Instance==QUADSPI) + { + /* USER CODE BEGIN QUADSPI_MspDeInit 0 */ + + /* USER CODE END QUADSPI_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_QSPI_CLK_DISABLE(); + + /**QUADSPI GPIO Configuration + PA3 ------> QUADSPI_CLK + PA6 ------> QUADSPI_BK1_IO3 + PA7 ------> QUADSPI_BK1_IO2 + PB0 ------> QUADSPI_BK1_IO1 + PB1 ------> QUADSPI_BK1_IO0 + PB11 ------> QUADSPI_BK1_NCS + */ + HAL_GPIO_DeInit(GPIOA, GPIO_PIN_3|GPIO_PIN_6|GPIO_PIN_7); + + HAL_GPIO_DeInit(GPIOB, GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_11); + + /* QUADSPI interrupt DeInit */ + HAL_NVIC_DisableIRQ(QUADSPI_IRQn); + /* USER CODE BEGIN QUADSPI_MspDeInit 1 */ + + /* USER CODE END QUADSPI_MspDeInit 1 */ + } + +} +#endif \ No newline at end of file diff --git a/platformio/stima_v4/slave-leaf/src/drivers/syscalls.c b/platformio/stima_v4/slave-leaf/src/drivers/syscalls.c new file mode 100644 index 000000000..f5a44257a --- /dev/null +++ b/platformio/stima_v4/slave-leaf/src/drivers/syscalls.c @@ -0,0 +1,89 @@ +#include +#include +#include +#include +#include + +#undef errno +extern int errno; + +extern caddr_t _end; +extern caddr_t _max_heap; + +extern int link(char *old, char *new); +extern int _close(int file); +extern int _fstat(int file, struct stat *st); +extern int _isatty(int file); +extern int _lseek(int file, int ptr, int dir); +extern void _exit(int status); +extern void _kill(int pid, int sig); +extern int _getpid(void); + +int _write(int file, char *ptr, int len) +{ + int i; + + if(file == 1) + { + for(i = 0; i < len; i++) + fputc(ptr[i], stdout); + } + else if(file == 2) + { + for(i = 0; i < len; i++) + fputc(ptr[i], stderr); + } + + return len; +} + +int _read(int file, char *ptr, int len) +{ + return 0; +} + +extern int link(char *old, char *new) +{ + return -1; +} + +extern int _close(int file) +{ + return -1; +} + +extern int _fstat(int file, struct stat *st) +{ + st->st_mode = S_IFCHR; + return 0; +} + +extern int _isatty(int file) +{ + return 1; +} + +extern int _lseek(int file, int ptr, int dir) +{ + return 0; +} + +extern void _exit(int status) +{ + while(1); +} + +extern void _kill(int pid, int sig) +{ + return; +} + +extern int _getpid(void) +{ + return -1; +} + +int AllowPLLInitByStartup(void) +{ + return 1; +} diff --git a/platformio/stima_v4/slave-leaf/src/main.cpp b/platformio/stima_v4/slave-leaf/src/main.cpp new file mode 100644 index 000000000..ddc2487dc --- /dev/null +++ b/platformio/stima_v4/slave-leaf/src/main.cpp @@ -0,0 +1,316 @@ +#define TRACE_LEVEL STIMA_TRACE_LEVEL + +#include "main.h" + +void setup() { + + // Reset param (Flag Fixed or on Button reset operation) + bool init_parameter = INIT_PARAMETER; + + // Semaphore, Queue && Param Config for TASK +#if (ENABLE_I2C1) + static BinarySemaphore *wireLock; // Access I2C internal EEprom, Accelerometer +#endif + +#if (ENABLE_I2C2) + static BinarySemaphore *wire2Lock; // Access I2C External (Sensor) +#endif + +#if (ENABLE_CAN) + static BinarySemaphore *canLock; // Can BUS +#endif + +#if (ENABLE_QSPI) + static BinarySemaphore *qspiLock; // Qspi (Flash Memory) +#endif + + static BinarySemaphore *rtcLock; // RTC (Access lock) + + // System Queue (Generic Message from/to Task) + static Queue *systemMessageQueue; + // Data queue (Request / exchange data from Can to Sensor and Elaborate Task) + static Queue *elaborateDataQueue; + static Queue *requestDataQueue; + static Queue *reportDataQueue; + + // System semaphore + static BinarySemaphore *configurationLock; // Access Configuration + static BinarySemaphore *systemStatusLock; // Access System status + static BinarySemaphore *registerAccessLock; // Access Register Cyphal Specifications + + // System and status configuration struct + static configuration_t configuration = {0}; + static system_status_t system_status = {0}; + + // Initializing basic hardware's configuration + SetupSystemPeripheral(); + init_debug(SERIAL_DEBUG_BAUD_RATE); + init_wire(); + init_pins(); + // Check Reset default PIN pression button if enabled +#ifdef STIMAV4_SLAVE_HW_VER_01_01 + if(!digitalRead(PIN_BTN)) init_parameter = true; +#endif + + // Init RTC + init_rtc(init_parameter); + + // Init SystemStatus Parameter !=0 ... For Check control Value + // Task check init data (Wdt = True, TaskStack Max, TaskReady = False) + // TOTAL_INFO_TASK Number of Task checked +#if (ENABLE_STACK_USAGE) + for(uint8_t id = 0; id < TOTAL_INFO_TASK; id++) + { + system_status.tasks[id].stack = 0xFFFFu; + } +#endif + // Disable all Task before INIT + for(uint8_t id = 0; id < TOTAL_INFO_TASK; id++) + { + system_status.tasks[id].state = task_flag::suspended; + } + +#if (ENABLE_WDT) + // Init the watchdog timer WDT_TIMEOUT_BASE_US mseconds timeout (only control system) + // Wdt Task Reset the value after All Task reset property single Flag + if(IWatchdog.isReset()) { + delay(50); + TRACE_INFO_F(F("\r\n\r\nALERT: Verified an WDT Reset !!!\r\n\r\n")); + IWatchdog.clearReset(); + } + IWatchdog.begin(WDT_TIMEOUT_BASE_US); +#endif + + // Hardware Semaphore +#if (ENABLE_I2C1) + wireLock = new BinarySemaphore(true); +#endif +#if (ENABLE_I2C2) + wire2Lock = new BinarySemaphore(true); +#endif +#if (ENABLE_CAN) + canLock = new BinarySemaphore(true); +#endif +#if (ENABLE_QSPI) + qspiLock = new BinarySemaphore(true); +#endif + rtcLock = new BinarySemaphore(true); + + // Software Semaphore + configurationLock = new BinarySemaphore(true); + systemStatusLock = new BinarySemaphore(true); + registerAccessLock = new BinarySemaphore(true); + + // Creating queue + systemMessageQueue = new Queue(SYSTEM_MESSAGE_QUEUE_LENGTH, sizeof(system_message_t)); + elaborateDataQueue = new Queue(ELABORATE_DATA_QUEUE_LENGTH, sizeof(elaborate_data_t)); + requestDataQueue = new Queue(REQUEST_DATA_QUEUE_LENGTH, sizeof(request_data_t)); + reportDataQueue = new Queue(REPORT_DATA_QUEUE_LENGTH, sizeof(report_t)); + + TRACE_INFO_F(F("Initialization HW Base done\r\n")); + + // Get Serial Number + configuration.serial_number = StimaV4GetSerialNumber(); + + // Serial Print Fixed for Serial Number + Serial.println(); + Serial.println(F("*****************************")); + Serial.println(F("* Stima V4 SLAVE - SER.NUM. *")); + Serial.println(F("*****************************")); + Serial.print(F("COD: ")); + for(int8_t id=7; id>=0; id--) { + if((uint8_t)((configuration.serial_number >> (8*id)) & 0xFF) < 16) Serial.print(F("0")); + Serial.print((uint8_t)((configuration.serial_number >> (8*id)) & 0xFF), 16); + if(id) Serial.print(F("-")); + } + Serial.println("\r\n"); + + // *************************************************************** + // Setup parameter for Task + // *************************************************************** + +#if (ENABLE_I2C1) + // Load Info from E2 boot_check flag and send to Config + static EEprom memEprom(&Wire, wireLock); + static bootloader_t boot_check = {0}; + // Reset to defaults? + if(init_parameter) { + boot_check.app_executed_ok = true; + memEprom.Write(BOOT_LOADER_STRUCT_ADDR, (uint8_t*) &boot_check, sizeof(boot_check)); + } else { + memEprom.Read(BOOT_LOADER_STRUCT_ADDR, (uint8_t*) &boot_check, sizeof(boot_check)); + } + // Optional send other InfoParm Boot (Uploaded, rollback, error fail ecc.. to config) +#endif + // Reset Factory register value + static EERegister clRegister(&Wire, wireLock); + // Reset to defaults? + if(init_parameter) clRegister.doFactoryReset(); + // Init access Flash istance object + static Flash memFlash(&hqspi); + + // ***************** SET PARAMETER TO TASK ********************* + + // TASK WDT, INFO STACK PARAM CONFIG AND CHECK BOOTLOADER STATUS + static WdtParam_t wdtParam = {0}; + wdtParam.system_status = &system_status; + wdtParam.boot_request = &boot_check; + wdtParam.systemStatusLock = systemStatusLock; + wdtParam.rtcLock = rtcLock; + wdtParam.eeprom = &memEprom; + +#if (ENABLE_CAN) + // TASK CAN PARAM CONFIG + static CanParam_t canParam = {0}; + canParam.configuration = &configuration; + canParam.system_status = &system_status; + canParam.boot_request = &boot_check; + canParam.configurationLock = configurationLock; + canParam.systemStatusLock = systemStatusLock; + canParam.registerAccessLock = registerAccessLock; + canParam.systemMessageQueue = systemMessageQueue; + canParam.requestDataQueue = requestDataQueue; + canParam.reportDataQueue = reportDataQueue; + canParam.eeprom = &memEprom; + canParam.clRegister = &clRegister; + canParam.flash = &memFlash; + canParam.canLock = canLock; + canParam.qspiLock = qspiLock; + canParam.rtcLock = rtcLock; +#endif + +#if (ENABLE_ACCELEROMETER) + // TASK ACCELEROMETER PARAM CONFIG + static AccelerometerParam_t accelerometerParam = {0}; + accelerometerParam.configuration = &configuration; + accelerometerParam.system_status = &system_status; + accelerometerParam.clRegister = &clRegister; +#if (ENABLE_I2C1) + accelerometerParam.wire = &Wire; + accelerometerParam.wireLock = wireLock; +#endif + accelerometerParam.systemStatusLock = systemStatusLock; + accelerometerParam.registerAccessLock = registerAccessLock; + accelerometerParam.systemMessageQueue = systemMessageQueue; +#endif + +#if (MODULE_TYPE == STIMA_MODULE_TYPE_LEAF) + static LeafSensorParam_t leafSensorParam = {0}; + leafSensorParam.configuration = &configuration; + leafSensorParam.system_status = &system_status; + leafSensorParam.configurationLock = configurationLock; + leafSensorParam.systemStatusLock = systemStatusLock; + leafSensorParam.systemMessageQueue = systemMessageQueue; + leafSensorParam.elaborateDataQueue = elaborateDataQueue; +#endif + + // TASK ELABORATE DATA PARAM CONFIG + static ElaborateDataParam_t elaborateDataParam = {0}; + elaborateDataParam.configuration = &configuration; + elaborateDataParam.system_status = &system_status; + elaborateDataParam.configurationLock = configurationLock; + elaborateDataParam.systemStatusLock = systemStatusLock; + elaborateDataParam.systemMessageQueue = systemMessageQueue; + elaborateDataParam.elaborateDataQueue = elaborateDataQueue; + elaborateDataParam.requestDataQueue = requestDataQueue; + elaborateDataParam.reportDataQueue = reportDataQueue; + + // TASK SUPERVISOR PARAM CONFIG + static SupervisorParam_t supervisorParam = {0}; + supervisorParam.configuration = &configuration; + supervisorParam.system_status = &system_status; + supervisorParam.clRegister = &clRegister; + supervisorParam.configurationLock = configurationLock; + supervisorParam.systemStatusLock = systemStatusLock; + supervisorParam.registerAccessLock = registerAccessLock; + supervisorParam.systemMessageQueue = systemMessageQueue; + supervisorParam.is_initialization_request = init_parameter; + + // ***************************************************************************** + // Startup Task, Supervisor as first for Loading parameter generic configuration + // ***************************************************************************** + static SupervisorTask supervisor_task("SupervisorTask", 300, OS_TASK_PRIORITY_04, supervisorParam); + +#if (MODULE_TYPE == STIMA_MODULE_TYPE_LEAF) + static LeafSensorTask leaf_sensor_task("LeafTask", 350, OS_TASK_PRIORITY_03, leafSensorParam); +#endif + static ElaborateDataTask elaborate_data_task("ElaborateDataTask", 400, OS_TASK_PRIORITY_02, elaborateDataParam); + +#if (ENABLE_ACCELEROMETER) + static AccelerometerTask accelerometer_task("AccelerometerTask", 350, OS_TASK_PRIORITY_01, accelerometerParam); +#endif + +#if (ENABLE_CAN) + static CanTask can_task("CanTask", 7300, OS_TASK_PRIORITY_02, canParam); +#endif + +#if (ENABLE_WDT) + static WdtTask wdt_task("WdtTask", 350, OS_TASK_PRIORITY_01, wdtParam); +#endif + + // Run Schedulher + Thread::StartScheduler(); + +} + +// FreeRTOS idleHook callBack to loop +void loop() { + // Enable LowPower idleHock reduce power consumption without disable sysTick + LowPower.idleHook(); +} + +/// @brief Init Pin (Diag and configuration) +void init_pins() { + // ***************************************************** + // STARTUP LED E PIN DIAGNOSTICI (SE UTILIZZATI) + // ***************************************************** + #if (ENABLE_DIAG_PIN) + // Output mode for LED(1/2) BLINK SW LOOP (High per Setup) + // Input mode for test button + #ifdef LED1_PIN + pinMode(LED1_PIN, OUTPUT); + digitalWrite(LED1_PIN, HIGH); + #endif + #ifdef LED2_PIN + pinMode(LED2_PIN, OUTPUT); + digitalWrite(LED1_PIN, LOW); + #endif + #ifdef USER_INP + pinMode(USER_INP, INPUT); + #endif + #endif +} + +// Setup Wire I2C Interface +void init_wire() +{ +#if (ENABLE_I2C1) + Wire.begin(); + Wire.setClock(I2C1_BUS_CLOCK_HZ); +#endif + +#if (ENABLE_I2C2) + Wire2.begin(); + Wire2.setClock(I2C2_BUS_CLOCK_HZ); +#endif +} + +// Setup RTC HW && LowPower Class STM32 +void init_rtc(bool init) +{ + // Init istance to STM RTC object + STM32RTC& rtc = STM32RTC::getInstance(); + // Select RTC clock source: LSE_CLOCK (First Istance) + rtc.setClockSource(STM32RTC::LSE_CLOCK); + rtc.begin(); // initialize RTC 24H format + // Set the time if requireq to Reset value + if(init) { + // Set the date && Time Init Value + rtc.setHours(0);rtc.setMinutes(0);rtc.setSeconds(0); + rtc.setWeekDay(0);rtc.setDay(1);rtc.setMonth(1);rtc.setYear(24); + } + // Start LowPower configuration + LowPower.begin(); + // Activate standard mode IdleHock Loop Power mode + LowPower.idleHookEnable(); +} diff --git a/platformio/stima_v4/slave-leaf/src/register_class.cpp b/platformio/stima_v4/slave-leaf/src/register_class.cpp new file mode 100644 index 000000000..0e1a739b3 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/src/register_class.cpp @@ -0,0 +1,486 @@ +/** + ****************************************************************************** + * @file register_class.cpp + * @author Moreno Gasperini + * @brief Register class (Uavcan/Other) cpp source + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#include "register_class.hpp" +#include "canard_config.hpp" + +#include "config.h" +#include "stima_utility.h" + +#include +#include +#include + +#define PASTE3_IMPL(x, y, z) x##y##z +#define PASTE3(x, y, z) PASTE3_IMPL(x, y, z) + +// *************************************************************************************** +// E2PROM STIMAV4 STM32 ARDUINO REGISTER CLASS ACCESS +// *************************************************************************************** + +/// @brief Constructor Class +EERegister::EERegister() +{ +} +/// @brief Construct a new EERegister::EERegister object +/// @param wire I2C class +/// @param wireLock I2C semaphore +/// @param i2c_address I2C address +EERegister::EERegister(TwoWire *wire, BinarySemaphore *wireLock, uint8_t i2c_address) +{ + // Memory controller for ClassRegister + _Eprom = EEprom(wire, wireLock); +} + +/// @brief Wrapper memory_write_block +/// @param address Address to write +/// @param data data to write +/// @param len packet len +void EERegister::_memory_write_block(uint16_t address, uint8_t *data, uint8_t len) { + _Eprom.Write(address, data, len); +} + +/// @brief Wrapper memory_read_block +/// @param address Address to read +/// @param data data readed +/// @param len packet len request +void EERegister::_memory_read_block(uint16_t address, uint8_t *data, uint8_t len) { + _Eprom.Read(address, data, len); +} + +/// @brief Wrapper memory_write_byte +/// @param address Address to write +/// @param data single byte data to write +void EERegister::_memory_write_byte(uint16_t address, uint8_t data) { + _Eprom.Write(address, data); +} + +/// @brief Wrapper memory_read_byte +/// @param address Address to read +/// @param data single byte readed +void EERegister::_memory_read_byte(uint16_t address, uint8_t *data) { + _Eprom.Read(address, data); +} + +/// @brief Inizializza l'area memory (indice) dedicata a REGISTER +void EERegister::_eeprom_register_factory(void) { + uint8_t register_index[MEM_UAVCAN_MAX_REG]; + // Scrivo in un unica tornata + memset(register_index, MEM_UAVCAN_REG_UNDEF, MEM_UAVCAN_MAX_REG); + _memory_write_block(MEM_UAVCAN_GET_ADDR_FLAG(), register_index, MEM_UAVCAN_MAX_REG); +} + +/// @brief Inizializza/Elimina un registro CYPAL/STIMAV4 dalla memoria +/// @param reg_numb numero di registro da eliminare +void EERegister::_eeprom_register_clear(uint8_t reg_numb) { + // Controllo area register + if(reg_numb=MEM_UAVCAN_MAX_REG) return false; + // Leggo l'indice se impostato (registro valido) + _memory_read_byte(MEM_UAVCAN_GET_ADDR_FLAG_REG(reg_numb), ®_valid); + if(reg_valid == reg_numb) { + // Registro eeprom valido, ritorno i campi name e value + _memory_read_byte(MEM_UAVCAN_GET_ADDR_NAME_LEN(reg_numb), &len_name); + _memory_read_block(MEM_UAVCAN_GET_ADDR_NAME(reg_numb), reg_name, len_name); + return true; + } + // Registro non impostato correttamente + return false; +} + +/// @brief Legge un registro CYPAL/STIMAV4 dalla memoria (per Nome) +/// @param reg_name (IN) Nome del regsitro da leggere +/// @param reg_numb (OUT) Numero del registro +/// @param data (OUT) Valore del registro +/// @return lunghezza del registro se valido, altrimenti 0 +size_t EERegister::_eeprom_register_get_from_name(uint8_t const *reg_name, uint8_t *reg_numb, uint8_t *data) { + uint8_t _reg_name[MEM_UAVCAN_LEN_NAME_REG]; + uint8_t _reg_data[MEM_UAVCAN_LEN_VALUE_REG]; + uint8_t _len_name = strlen((char*)reg_name); + size_t _len_data = 0; + uint8_t register_index[MEM_UAVCAN_MAX_REG]; + // Leggo l'intero status register per velocizzare l'indice di ricerca + // Controllo preventivo dell'array di validità dei registri indicizzata + _memory_read_block(MEM_UAVCAN_GET_ADDR_FLAG(), register_index, MEM_UAVCAN_MAX_REG); + // Controllo area register (Search for name) + for(uint8_t reg_index = 0; reg_index Rapido senza controllo validità REG + // La sua chiamata prevede un controllo preventivo della validità del REG + if (_eeprom_register_get_len_intest_fast(reg_index) == _len_name) { + // Retrieve all info register value + _len_data = _eeprom_register_get_fast(reg_index, _reg_name, _reg_data); + // Compare value name + if(memcmp(reg_name, _reg_name, _len_name) == 0) { + // Data is found + memcpy(data, _reg_data, _len_data); + *reg_numb = reg_index; + return _len_data; + } + } + } + } + // End Of Size ROM (Register Not Found) + return 0; +} + +/// @brief Legge un indiced di registro CYPAL/STIMAV4 dalla memoria (per Nome) +/// @param reg_name (IN) Nome del regsitro da leggere +/// @return indice del registro se esiste altrimenti MEM_UAVCAN_REG_UNDEF +uint8_t EERegister::_eeprom_register_get_index_from_name(uint8_t *reg_name) { + uint8_t _reg_name[MEM_UAVCAN_LEN_NAME_REG]; + uint8_t _reg_data[MEM_UAVCAN_LEN_VALUE_REG]; + uint8_t _len_name = strlen((char*)reg_name); + uint8_t register_index[MEM_UAVCAN_MAX_REG]; + // Leggo l'intero status register per velocizzare l'indice di ricerca + // Controllo preventivo dell'array di validità dei registri indicizzata + _memory_read_block(MEM_UAVCAN_GET_ADDR_FLAG(), register_index, MEM_UAVCAN_MAX_REG); + // Controllo area register (Search for name) + for(uint8_t reg_index = 0; reg_index Rapido senza controllo validità REG + // La sua chiamata prevede un controllo preventivo della validità del REG + if (_eeprom_register_get_len_intest_fast(reg_index) == _len_name) { + // Test Data (Register Valid) + _eeprom_register_get_intest_fast(reg_index, _reg_name, _len_name); + // Compare value name + if(memcmp(reg_name, _reg_name, _len_name) == 0) { + // Data is found + return reg_index; + } + } + } + } + // End Of Size ROM (Register Not Found) + return MEM_UAVCAN_REG_UNDEF; +} + +/// @brief Scrive/edita un registro CYPAL/STIMAV4 sulla memoria +/// @param reg_numb Numero di regsitro da impostare +/// @param reg_name Nome del resistro UAVCAN/CYPAL +/// @param data Valore del registro +/// @param len_data Dimensione valore del registro +void EERegister::_eeprom_register_set(uint8_t reg_numb, uint8_t *reg_name, uint8_t *data, size_t len_data) { + uint8_t reg_valid; + uint8_t name_len = strlen((char*)reg_name); + uint8_t write_block[MEM_UAVCAN_LEN_REG] = {0}; + // Controllo area register + if(reg_numb>=MEM_UAVCAN_MAX_REG) return; + // Leggo l'indice se impostato (registro valido) + _memory_read_byte(MEM_UAVCAN_GET_ADDR_FLAG_REG(reg_numb), ®_valid); + if(reg_valid != reg_numb) { + // Imposto il Numero sul BYTE relativo (Registro inizializzato) + _memory_write_byte(MEM_UAVCAN_GET_ADDR_FLAG_REG(reg_numb), reg_numb); + } + // Registro eeprom valido, imposto i campi name e value + write_block[MEM_UAVCAN_POS_LEN_NAME] = name_len; + write_block[MEM_UAVCAN_POS_LEN_DATA] = len_data; + memcpy(write_block + MEM_UAVCAN_POS_STR_NAME, reg_name, name_len); + memcpy(write_block + MEM_UAVCAN_POS_VALUE_DATA, data, len_data); + // Perform in unica scrittura + _memory_write_block(MEM_UAVCAN_GET_ADDR_BASE_REG(reg_numb), write_block, MEM_UAVCAN_LEN_REG); +} + +/// @brief Ritorna il prossimo indice (se esiste) valido nella sezione memoria Cypal dedicata +/// @param current_register indirizzo di partenza nel campo di validità [MEM_UAVCAN_MAX_REG] [IN/OUT] +void EERegister::_eeprom_register_get_next_id(uint8_t *current_register) { + uint8_t register_index[MEM_UAVCAN_MAX_REG]; + // Controllo se richiesto avvio dall'inizio della coda... get_next(MAX)... + bool is_first = (*current_register==MEM_UAVCAN_REG_UNDEF); + // Continuo alla ricerca del prossimo register se esiste + _memory_read_block(MEM_UAVCAN_GET_ADDR_FLAG(), register_index, MEM_UAVCAN_MAX_REG); + while((*current_register Set Valore default +void EERegister::read(const char* const register_name, uavcan_register_Value_1_0* const inout_value) { + LOCAL_ASSERT(inout_value != NULL); + + uint8_t register_number; + bool init_required = !uavcan_register_Value_1_0_is_empty_(inout_value); + uint8_t serialized[uavcan_register_Value_1_0_EXTENT_BYTES_] = {0}; + size_t sr_size = _eeprom_register_get_from_name((const uint8_t*)register_name, ®ister_number, serialized); + + static uavcan_register_Value_1_0 out = {0}; + const int8_t err = uavcan_register_Value_1_0_deserialize_(&out, serialized, &sr_size); + if (err >= 0) init_required = !assign(inout_value, &out); + if (init_required) write(register_name, inout_value); +} + +/// @brief Scrive un registro Cypal/Uavcan wrapper UAVCAN +/// @param register_name nome del registro +/// @param value valore del registro (formato uavcan) +void EERegister::write(const char* const register_name, const uavcan_register_Value_1_0* const value) { + uint8_t serialized[uavcan_register_Value_1_0_EXTENT_BYTES_] = {0}; + size_t sr_size = uavcan_register_Value_1_0_EXTENT_BYTES_; + const int8_t err = uavcan_register_Value_1_0_serialize_(value, serialized, &sr_size); + uint8_t register_index = _eeprom_register_get_index_from_name((uint8_t*) register_name); + if(register_index == MEM_UAVCAN_REG_UNDEF) { + printf("Init register: %s\n", register_name); + _eeprom_register_add((uint8_t*) register_name, serialized, sr_size); + } else { + _eeprom_register_set(register_index, (uint8_t*) register_name, serialized, sr_size); + } +} + +/// @brief Scroll degli indici dal primo all'ultimo e return ID UavCAN +/// Nel passaggio di un eventuale INDICE vuoto, non viene incrementato l'ID +/// ID Uavcan è solo l'elenco sequenziale degli ID registarti validi +/// La procedura scorre tutta l'area registri e ritorda IDX+1 alla lettura di un ID Valido +/// @param index indice di controllo +/// @return UavCan Name Register Formato UAVCAN +uavcan_register_Name_1_0 EERegister::getNameByIndex(const uint16_t index) { + uavcan_register_Name_1_0 out = {0}; + uavcan_register_Name_1_0_initialize_(&out); + // N.B. Get starting list from first data Next of MEM_UAVCAN_REG_UNDEF -> = 0 + // Get First... + if(index > MEM_UAVCAN_LEN_NAME_REG) + return out; + uint8_t reg_current = MEM_UAVCAN_REG_UNDEF; + _eeprom_register_get_next_id(®_current); + uint8_t reg_index = 0; + while(reg_current<=MEM_UAVCAN_MAX_REG) { + // Index Exact + if(reg_index==index) { + uint8_t _reg_name[MEM_UAVCAN_LEN_NAME_REG] = {0}; + _eeprom_register_get_name_from_index(reg_current, _reg_name); + out.name.count = nunavutChooseMin(strlen((char*)_reg_name), uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); + memcpy(out.name.elements, _reg_name, out.name.count); + break; + } + // Get Next register E2prom + _eeprom_register_get_next_id(®_current); + // Set next Id register list contiguos UAVCAN + reg_index++; + } + return out; +} + +/// @brief Set factoryReset Register UAVCAN +void EERegister::doFactoryReset(void) { + setup(); +} + +/// @brief Register type Assign UAVCAN +/// @param dst destination register +/// @param src source register +/// @return Register assign completed OK +bool EERegister::assign(uavcan_register_Value_1_0* const dst, const uavcan_register_Value_1_0* const src) { + if (uavcan_register_Value_1_0_is_empty_(dst)) { + *dst = *src; + return true; + } + if ((uavcan_register_Value_1_0_is_string_(dst) && uavcan_register_Value_1_0_is_string_(src)) || + (uavcan_register_Value_1_0_is_unstructured_(dst) && uavcan_register_Value_1_0_is_unstructured_(src))) { + *dst = *src; + return true; + } + if (uavcan_register_Value_1_0_is_bit_(dst) && uavcan_register_Value_1_0_is_bit_(src)) { + nunavutCopyBits(dst->bit.value.bitpacked, + 0, + nunavutChooseMin(dst->bit.value.count, src->bit.value.count), + src->bit.value.bitpacked, + 0); + return true; + } + // This is a violation of MISRA/AUTOSAR but it is believed to be less error-prone than manually copy-pasted code. + #define REGISTER_CASE_SAME_TYPE(TYPE) \ + if (PASTE3(uavcan_register_Value_1_0_is_, TYPE, _)(dst) && PASTE3(uavcan_register_Value_1_0_is_, TYPE, _)(src)) { \ + for (size_t i = 0; i < nunavutChooseMin(dst->TYPE.value.count, src->TYPE.value.count); ++i) { \ + dst->TYPE.value.elements[i] = src->TYPE.value.elements[i]; \ + } \ + return true; \ + } + REGISTER_CASE_SAME_TYPE(integer64) + REGISTER_CASE_SAME_TYPE(integer32) + REGISTER_CASE_SAME_TYPE(integer16) + REGISTER_CASE_SAME_TYPE(integer8) + REGISTER_CASE_SAME_TYPE(natural64) + REGISTER_CASE_SAME_TYPE(natural32) + REGISTER_CASE_SAME_TYPE(natural16) + REGISTER_CASE_SAME_TYPE(natural8) + REGISTER_CASE_SAME_TYPE(real64) + REGISTER_CASE_SAME_TYPE(real32) + REGISTER_CASE_SAME_TYPE(real16) + return false; +} \ No newline at end of file diff --git a/platformio/stima_v4/slave-leaf/src/tasks/accelerometer_task.cpp b/platformio/stima_v4/slave-leaf/src/tasks/accelerometer_task.cpp new file mode 100644 index 000000000..abe51abac --- /dev/null +++ b/platformio/stima_v4/slave-leaf/src/tasks/accelerometer_task.cpp @@ -0,0 +1,501 @@ +/** + ****************************************************************************** + * @file accelerometer_task.cpp + * @author Moreno Gasperini + * @brief accelerometer cpp_Freertos source file + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#define TRACE_LEVEL ACCELEROMETER_TASK_TRACE_LEVEL +#define LOCAL_TASK_ID ACCELEROMETER_TASK_ID + +#include "tasks/accelerometer_task.h" + +#if (ENABLE_ACCELEROMETER) +/// @brief Construct the Accelerometer Task::AccelerometerTask object +/// @param taskName name of the task +/// @param stackSize size of the stack +/// @param priority priority of the task +/// @param accelerometerParam parameters for the task +AccelerometerTask::AccelerometerTask(const char *taskName, uint16_t stackSize, uint8_t priority, AccelerometerParam_t accelerometerParam) : Thread(taskName, stackSize, priority), param(accelerometerParam) +{ + // Start WDT controller and TaskState Flags + TaskWatchDog(WDT_STARTING_TASK_MS); + TaskState(ACCELEROMETER_STATE_CREATE, UNUSED_SUB_POSITION, task_flag::normal); + + // Setup register mode && Load or Init configuration + loadConfiguration(); + + // Starting Class + accelerometer = Accelerometer(param.wire, param.wireLock); + + state = ACCELEROMETER_STATE_INIT; + Start(); +}; + +#if (ENABLE_STACK_USAGE) +/// @brief local stack Monitor (optional) +void AccelerometerTask::TaskMonitorStack() +{ + uint16_t stackUsage = (uint16_t)uxTaskGetStackHighWaterMark( NULL ); + if((stackUsage) && (stackUsage < param.system_status->tasks[LOCAL_TASK_ID].stack)) { + param.systemStatusLock->Take(); + param.system_status->tasks[LOCAL_TASK_ID].stack = stackUsage; + param.systemStatusLock->Give(); + } +} +#endif + +/// @brief local watchDog and Sleep flag Task (optional) +/// @param millis_standby time in ms to perfor check of WDT. If longer than WDT Reset, WDT is temporanly suspend +void AccelerometerTask::TaskWatchDog(uint32_t millis_standby) +{ + // Local TaskWatchDog update + param.systemStatusLock->Take(); + // Update WDT Signal (Direct or Long function Timered) + if(millis_standby) + { + // Check 1/2 Freq. controller ready to WDT only SET flag + if((millis_standby) < WDT_CONTROLLER_MS / 2) { + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::set; + } else { + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::timer; + // Add security milimal Freq to check + param.system_status->tasks[LOCAL_TASK_ID].watch_dog_ms = millis_standby + WDT_CONTROLLER_MS; + } + } + else + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::set; + param.systemStatusLock->Give(); +} + +/// @brief local suspend flag and positor running state Task (optional) +/// @param state_position Sw_Position (Local STATE) +/// @param state_subposition Sw_SubPosition (Optional Local SUB_STATE Position Monitor) +/// @param state_operation operative mode flag status for this task +void AccelerometerTask::TaskState(uint8_t state_position, uint8_t state_subposition, task_flag state_operation) +{ + // Local TaskWatchDog update + param.systemStatusLock->Take(); + // Signal Task sleep/disabled mode from request (Auto SET WDT on Resume) + if((param.system_status->tasks[LOCAL_TASK_ID].state == task_flag::suspended)&& + (state_operation==task_flag::normal)) + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::set; + param.system_status->tasks[LOCAL_TASK_ID].state = state_operation; + param.system_status->tasks[LOCAL_TASK_ID].running_pos = state_position; + param.system_status->tasks[LOCAL_TASK_ID].running_sub = state_subposition; + param.systemStatusLock->Give(); +} + +/// @brief RUN Task +void AccelerometerTask::Run() +{ + // Local parameter for command and state accelerometer + bool is_module_ready; + uint8_t hardware_check_attempt; + bool start_calibration = false; + + // System message data queue structured + system_message_t system_message; + + // Start Running Monitor and First WDT normal state + #if (ENABLE_STACK_USAGE) + TaskMonitorStack(); + #endif + TaskState(state, UNUSED_SUB_POSITION, task_flag::normal); + + while (true) + { + + // ********* SYSTEM QUEUE MESSAGE *********** + // enqueud system message from caller task + if (!param.systemMessageQueue->IsEmpty()) { + // Read queue in test mode + if (param.systemMessageQueue->Peek(&system_message, 0)) + { + // Its request addressed into this TASK... -> pull + if(system_message.task_dest == ACCELEROMETER_TASK_ID) + { + // Pull && elaborate command, after response if... + param.systemMessageQueue->Dequeue(&system_message); + if(system_message.command.do_calib) // == Calibrate && Save { + { + start_calibration = true; + } + } + // Its request addressed into ALL TASK... -> no pull (only SUPERVISOR or exernal gestor) + else if(system_message.task_dest == ALL_TASK_ID) + { + // Pull && elaborate command, + if(system_message.command.do_sleep) + { + // Start module sleep procedure + powerDownModule(); + // Enter sleep module OK and update WDT + TaskWatchDog(ACCELEROMETER_TASK_SLEEP_DELAY_MS); + TaskState(state, UNUSED_SUB_POSITION, task_flag::sleepy); + Delay(Ticks::MsToTicks(ACCELEROMETER_TASK_SLEEP_DELAY_MS)); + TaskState(state, UNUSED_SUB_POSITION, task_flag::normal); + // Restore module from Sleep + state = ACCELEROMETER_STATE_SETUP_MODULE; + } + } + } + } + + // Standard TASK switch main + switch (state) + { + case ACCELEROMETER_STATE_INIT: + TRACE_VERBOSE_F(F("ACCELEROMETER_STATE_INIT -> ACCELEROMETER_STATE_CHECK_HARDWARE\r\n")); + state = ACCELEROMETER_STATE_CHECK_HARDWARE; + // Signal reset error to system state + param.systemStatusLock->Take(); + param.system_status->events.is_accelerometer_error = true; + param.system_status->events.is_bubble_level_error = false; + param.systemStatusLock->Give(); + Delay(Ticks::MsToTicks(ACCELEROMETER_WAIT_CHECK_HARDWARE)); + hardware_check_attempt = 0; + is_module_ready = false; + break; + + case ACCELEROMETER_STATE_CHECK_HARDWARE: + if (!is_module_ready) + { + if (checkModule()) { + TRACE_VERBOSE_F(F("ACCELEROMETER_STATE_CHECK_HARDWARE -> ACCELEROMETER_STATE_SETUP_MODULE\r\n")); + state = ACCELEROMETER_STATE_SETUP_MODULE; + break; + } + // Wait for HW Check + hardware_check_attempt++; + Delay(Ticks::MsToTicks(ACCELEROMETER_WAIT_CHECK_HARDWARE)); + if(hardware_check_attempt >= ACCELEROMETER_MAX_CHECK_ATTEMPT) + state = ACCELEROMETER_STATE_HARDWARE_FAIL; + } else { + state = ACCELEROMETER_STATE_WAIT_RESUME; + } + break; + + case ACCELEROMETER_STATE_SETUP_MODULE: + setupModule(); + is_module_ready = true; + // System inform Accelerometer ready hardware OK + param.systemStatusLock->Take(); + param.system_status->events.is_accelerometer_error = false; + param.systemStatusLock->Give(); + TRACE_VERBOSE_F(F("ACCELEROMETER_STATE_SETUP_MODULE -> ACCELEROMETER_STATE_CHECK_OPERATION\r\n")); + state = ACCELEROMETER_STATE_CHECK_OPERATION; + break; + + case ACCELEROMETER_STATE_CHECK_OPERATION: + if(readModule()) { + // Checking bubble error limit (with Mirror Error "Z" become 0.001 -> 0.999 X,Y 0.001 -> -0.001) + if(((abs(value_x) > BUBBLE_ANGLE_ERROR) && (abs(value_x) < BUBBLE_ANGLE_MIRROR)) || + ((abs(value_y) > BUBBLE_ANGLE_ERROR) && (abs(value_y) < BUBBLE_ANGLE_MIRROR)) || + ((abs(value_z) > BUBBLE_ANGLE_ERROR) && (abs(value_z) < BUBBLE_ANGLE_MIRROR))) + { + if(!param.system_status->events.is_bubble_level_error) { + param.systemStatusLock->Take(); + param.system_status->events.is_bubble_level_error = true; + param.systemStatusLock->Give(); + } + } else { + if(param.system_status->events.is_bubble_level_error) { + param.systemStatusLock->Take(); + param.system_status->events.is_bubble_level_error = false; + param.systemStatusLock->Give(); + } + } + TRACE_INFO_F(F("X[ 0.%03d ] | Y[ 0.%03d ] | Z[ 0.%03d ]\r\n"), (int)(abs(value_x)*1000), (int)(abs(value_y)*1000), (int)(abs(value_z)*1000), OK_STRING); + if(start_calibration) { + TRACE_INFO_F(F("ACCELEROMETER Start calibration\r\n")); + calibrate(false, true); + printConfiguration(); + start_calibration = false; + } + } + break; + + case ACCELEROMETER_STATE_WAIT_RESUME: + TRACE_VERBOSE_F(F("ACCELEROMETER_STATE_END -> SUSPEND()\r\n")); + // Disable control TASK + TaskState(state, UNUSED_SUB_POSITION, task_flag::suspended); + Suspend(); + TaskState(state, UNUSED_SUB_POSITION, task_flag::normal); + // On Restore Next INIT + state = ACCELEROMETER_STATE_SETUP_MODULE; + break; + + case ACCELEROMETER_STATE_HARDWARE_FAIL: + TRACE_VERBOSE_F(F("ACCELEROMETER_STATE_FAIL -> SUSPEND()\r\n")); + // Disable control TASK + TaskState(state, UNUSED_SUB_POSITION, task_flag::suspended); + Suspend(); + TaskState(state, UNUSED_SUB_POSITION, task_flag::normal); + // On Restore Next INIT + state = ACCELEROMETER_STATE_SETUP_MODULE; + break; + } + + #if (ENABLE_STACK_USAGE) + TaskMonitorStack(); + #endif + + // Local WatchDog update; + TaskWatchDog(ACCELEROMETER_TASK_WAIT_DELAY_MS); + + // MAX One switch step for Task Waiting Next Step + DelayUntil(Ticks::MsToTicks(ACCELEROMETER_TASK_WAIT_DELAY_MS)); + } +} + +/// @brief Load configuration accelleration module +void AccelerometerTask::loadConfiguration(void) +{ + // Verify config valid param + bool register_config_valid = true; + // Param Reading + static uavcan_register_Value_1_0 val = {0}; + + TRACE_INFO_F(F("ACCELEROMETER: Load configuration...\r\n")); + + //! read configuration from register + // Reading RMAP Module identify Param -> (READ/WRITE) + // uint8_t config_valid; TYPE ELEMENT ID + // uint8_t module_power; Frequency get data + if(register_config_valid) { + // Select type register (uint_8) + uavcan_register_Value_1_0_select_natural8_(&val); + val.natural8.value.count = 2; + // Loading Default + val.natural8.value.elements[0] = IIS328DQ_ID; + val.natural8.value.elements[1] = Accelerometer::IIS328DQ_ODR_5Hz2; + param.registerAccessLock->Take(); + param.clRegister->read("rmap.accelerometer.config", &val); + param.registerAccessLock->Give(); + if(uavcan_register_Value_1_0_is_natural8_(&val) && (val.natural8.value.count != 2)) { + register_config_valid = false; + } else { + accelerometer_configuration.config_valid = val.natural8.value.elements[0]; + accelerometer_configuration.module_power = (Accelerometer::iis328dq_dr_t) val.natural8.value.elements[1]; + } + } + + // Reading RMAP Module Offset Config -> (READ/WRITE) + // float_t x; Offset (X) + // float_t y; Offset (Y) + // float_t z; Offset (Z) + if(register_config_valid) { + // Select type register (float_) + uavcan_register_Value_1_0_select_real32_(&val); + val.real32.value.count = 3; + // Loading Default + val.real32.value.elements[0] = 0.0; + val.real32.value.elements[1] = 0.0; + val.real32.value.elements[2] = 0.0; + param.registerAccessLock->Take(); + param.clRegister->read("rmap.accelerometer.offset", &val); + param.registerAccessLock->Give(); + if(uavcan_register_Value_1_0_is_real32_(&val) && (val.real32.value.count != 3)) { + register_config_valid = false; + } else { + accelerometer_configuration.offset_x = val.real32.value.elements[0]; + accelerometer_configuration.offset_y = val.real32.value.elements[1]; + accelerometer_configuration.offset_z = val.real32.value.elements[2]; + } + } + + // Validation Byte Config (Init Defualt Value) + if (!register_config_valid || (accelerometer_configuration.config_valid != IIS328DQ_ID)) + { + // Reset configuration... + saveConfiguration(true); + } + printConfiguration(); +} + +/// @brief Print configuration Accelerometer +void AccelerometerTask::printConfiguration(void) +{ + TRACE_INFO_F(F("--> accelerometer config: \r\n")); + TRACE_INFO_F(F("--> config flag: %d\r\n"), accelerometer_configuration.config_valid); + TRACE_INFO_F(F("--> power mode: %d\r\n"), (int)accelerometer_configuration.module_power); + TRACE_INFO_F(F("--> offset X value: %d\r\n"), (int)(accelerometer_configuration.offset_x * 1000)); + TRACE_INFO_F(F("--> offset Y value: %d\r\n"), (int)(accelerometer_configuration.offset_y * 1000)); + TRACE_INFO_F(F("--> offset Z value: %d\r\n"), (int)(accelerometer_configuration.offset_z * 1000)); +} + +/// @brief Init/Save configuration param Accelerometer +/// @param is_default true if need to reset register configuration value default +void AccelerometerTask::saveConfiguration(bool is_default) +{ + // Param Writing + static uavcan_register_Value_1_0 val = {0}; + + TRACE_INFO_F(F("Attempt to write accelerometer configuration... [ %s ]\r\n"), OK_STRING); + + // Loading defualt request + if (is_default) + { + accelerometer_configuration.config_valid = IIS328DQ_ID; + accelerometer_configuration.module_power = Accelerometer::IIS328DQ_ODR_5Hz2; + accelerometer_configuration.offset_x = 0.0f; + accelerometer_configuration.offset_y = 0.0f; + accelerometer_configuration.offset_z = 0.0f; + } + + // Writing RMAP Module identify Param -> (READ/WRITE) + // uint8_t config_valid; TYPE ELEMENT ID + // uint8_t module_power; Frequency get data + // Select type register (uint_8) + uavcan_register_Value_1_0_select_natural8_(&val); + val.natural8.value.count = 2; + val.natural8.value.elements[0] = IIS328DQ_ID; + val.natural8.value.elements[1] = accelerometer_configuration.module_power; + param.registerAccessLock->Take(); + param.clRegister->write("rmap.accelerometer.config", &val); + param.registerAccessLock->Give(); + + // Reading RMAP Module Offset Config -> (READ/WRITE) + // float_t x; Offset (X) + // float_t y; Offset (Y) + // float_t z; Offset (Z) + // Select type register (float_) + uavcan_register_Value_1_0_select_real32_(&val); + val.real32.value.count = 3; + // Loading Default + val.real32.value.elements[0] = accelerometer_configuration.offset_x; + val.real32.value.elements[1] = accelerometer_configuration.offset_y; + val.real32.value.elements[2] = accelerometer_configuration.offset_z; + param.registerAccessLock->Take(); + param.clRegister->write("rmap.accelerometer.offset", &val); + param.registerAccessLock->Give(); +} + +/// @brief Calibrate accelereometer position X-Y-Z to actual value (set offset from 0) +/// @param is_default require default value data +/// @param save_register request to save calibration in register +void AccelerometerTask::calibrate(bool is_default, bool save_register) +{ + if (is_default) + { + // Init offset to 0 + accelerometer_configuration.offset_x = 0.0f; + accelerometer_configuration.offset_y = 0.0f; + accelerometer_configuration.offset_z = 0.0f; + } else { + // Set offset to direct realtime Read Value + accelerometer_configuration.offset_x = accelerometer.iis328dq_from_fsx_to_inc(Accelerometer::coordinate::X); + accelerometer_configuration.offset_y = accelerometer.iis328dq_from_fsx_to_inc(Accelerometer::coordinate::Y); + accelerometer_configuration.offset_z = accelerometer.iis328dq_from_fsx_to_inc(Accelerometer::coordinate::Z); + } + + // Save Register Eeprom + if(save_register) { + // Param Writing + static uavcan_register_Value_1_0 val = {0}; + // Reading RMAP Module Offset Config -> (READ/WRITE) + // float_t x; Offset (X) + // float_t y; Offset (Y) + // float_t z; Offset (Z) + // Select type register (float_) + uavcan_register_Value_1_0_select_real32_(&val); + val.real32.value.count = 3; + // Loading Default + val.real32.value.elements[0] = accelerometer_configuration.offset_x; + val.real32.value.elements[1] = accelerometer_configuration.offset_y; + val.real32.value.elements[2] = accelerometer_configuration.offset_z; + param.registerAccessLock->Take(); + param.clRegister->write("rmap.accelerometer.offset", &val); + param.registerAccessLock->Give(); + } +} + +/// @brief Check hardware module +/// @return True is module ready and OK +bool AccelerometerTask::checkModule(void) +{ + bool hw_check = false; + + TRACE_INFO_F(F("Check hardware module... [ %s ]\r\n"), OK_STRING); + /* Try whoamI response from device */ + uint8_t whoamI = 0; + // Read ID Device + accelerometer.iis328dq_device_id_get(&whoamI); + if (whoamI == IIS328DQ_ID) hw_check = true; + + return hw_check; +} + +/// @brief Setup hardware configuration +void AccelerometerTask::setupModule(void) +{ + TRACE_INFO_F(F("Setup hardware module... [ %s ]\r\n"), OK_STRING); + /* Enable Block Data Update */ + accelerometer.iis328dq_block_data_update_set(PROPERTY_ENABLE); + /* Set full scale */ + accelerometer.iis328dq_full_scale_set(Accelerometer::IIS328DQ_2g); + /* Configure filtering chain */ + /* Accelerometer - High Pass / Slope path */ + accelerometer.iis328dq_hp_path_set(Accelerometer::IIS328DQ_HP_DISABLE); + /* Set Output Data Rate */ + accelerometer.iis328dq_data_rate_set(accelerometer_configuration.module_power); +} + +/// @brief Read data from module accelerometer +/// @return true if data is ready from module +bool AccelerometerTask::readModule(void) +{ + bool status = false; + /* Read output only if new value is available */ + Accelerometer::iis328dq_reg_t reg; + accelerometer.iis328dq_status_reg_get(®.status_reg); + // Is New Data avaiable + if (reg.status_reg.zyxda) { + status = true; + /* Read acceleration data */ + int16_t data_raw_acceleration[3]; + memset(data_raw_acceleration, 0x00, 3 * sizeof(int16_t)); + accelerometer.iis328dq_acceleration_raw_get(data_raw_acceleration); + accelerometer.push_raw_data(data_raw_acceleration); + value_x = accelerometer.iis328dq_from_fsx_to_inc(Accelerometer::coordinate::X) - accelerometer_configuration.offset_x; + value_y = accelerometer.iis328dq_from_fsx_to_inc(Accelerometer::coordinate::Y) - accelerometer_configuration.offset_y; + value_z = accelerometer.iis328dq_from_fsx_to_inc(Accelerometer::coordinate::Z) - accelerometer_configuration.offset_z; + } + // True if data ready + return status; +} + +/// @brief Activate power saving hardware module +void AccelerometerTask::powerDownModule(void) +{ + TRACE_INFO_F(F("Power down accelerometer... [ %s ]\r\n"), OK_STRING); + /* Set Output Data Rate to OFF */ + accelerometer.iis328dq_data_rate_set(Accelerometer::IIS328DQ_ODR_OFF); + /* Disable Block Data Update */ + accelerometer.iis328dq_block_data_update_set(PROPERTY_DISABLE); +} + +#endif diff --git a/platformio/stima_v4/slave-leaf/src/tasks/can_task.cpp b/platformio/stima_v4/slave-leaf/src/tasks/can_task.cpp new file mode 100644 index 000000000..b6a00971b --- /dev/null +++ b/platformio/stima_v4/slave-leaf/src/tasks/can_task.cpp @@ -0,0 +1,1804 @@ +/** + ****************************************************************************** + * @file can_task.cpp + * @author Moreno Gasperini + * @brief Uavcan over CanBus cpp_Freertos source file + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#define TRACE_LEVEL CAN_TASK_TRACE_LEVEL +#define LOCAL_TASK_ID CAN_TASK_ID + +#include "tasks/can_task.h" + +using namespace cpp_freertos; + +// *************************************************************************************************** +// ********** Funzioni ed utility generiche per gestione UAVCAN ********** +// *************************************************************************************************** + +/// @brief Enable Power CAN_Circuit TJA1443 +/// @param ModeCan (Mode TYPE CAN_BUS) +void CanTask::HW_CAN_Power(CAN_ModePower ModeCan) { + // Normal Mode (TX/RX Full functionally) + if(ModeCan == CAN_ModePower::CAN_INIT) { + canPower = ModeCan; + digitalWrite(PIN_CAN_STB, LOW); + digitalWrite(PIN_CAN_STB, LOW); + // Waiting min of 5 uS for Full Operational Setup bxCan && Hal_Can_Init + delayMicroseconds(10); + digitalWrite(PIN_CAN_STB, HIGH); + digitalWrite(PIN_CAN_EN, HIGH); + } + // Exit if state is the same + if (canPower == ModeCan) return; + // Normal Mode (TX/RX Full functionally) + if(ModeCan == CAN_ModePower::CAN_NORMAL) { + digitalWrite(PIN_CAN_STB, HIGH); + delayMicroseconds(10); + digitalWrite(PIN_CAN_EN, HIGH); + // Waiting min of 65 uS for Full Operational External CAN Power Circuit + // Perform secure WakeUp Timer with 100 uS + delayMicroseconds(90); + } + // Listen Mode (Only RX circuit enabled for first paket data) + if(ModeCan == CAN_ModePower::CAN_LISTEN_ONLY) { + digitalWrite(PIN_CAN_EN, LOW); + delayMicroseconds(10); + digitalWrite(PIN_CAN_STB, HIGH); + } + // Sleep (Turn OFF HW and enter sleep mode TJA1443) + if(ModeCan == CAN_ModePower::CAN_SLEEP) { + digitalWrite(PIN_CAN_STB, LOW); + delayMicroseconds(10); + digitalWrite(PIN_CAN_EN, HIGH); + } + // Saving state + canPower = ModeCan; +} + +/// @brief Ritorna unique-ID 128-bit del nodo locale. E' utilizzato in uavcan.node.GetInfo.Response e durante +/// plug-and-play node-ID allocation da uavcan.pnp.NodeIDAllocationData. SerialNumber, Produttore.. +/// Dovrebbe essere verificato in uavcan.node.GetInfo.Response per la verifica non sia cambiato Nodo. +/// Al momento vengono inseriti 2 BYTE fissi, altri eventuali, che Identificano il Tipo Modulo +/// @param out data out UniqueID +/// @param serNumb local Hardware Serial Number (64Bit) Already Send in PNP 1.0 Request (Hash 48Bit) +void CanTask::getUniqueID(uint8_t out[uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_], uint64_t serNumb) +{ + // A real hardware node would read its unique-ID from some hardware-specific source (typically stored in ROM). + // This example is a software-only node so we store the unique-ID in a (read-only) register instead. + static uavcan_register_Value_1_0 val = {0}; + uavcan_register_Value_1_0_select_unstructured_(&val); + + // Crea default unique_id con 8 BYTES From local_serial Number (N.B. serNumb[0] rappresenta Tipo Nodo ) + uint8_t *ptrData = (uint8_t*)&serNumb; + for (uint8_t i = 0; i < 8; i++) + { + val.unstructured.value.elements[val.unstructured.value.count++] = ptrData[i]; + } + // Il resto dei 128 vengono impostati RANDOM + for (uint8_t i = val.unstructured.value.count; i < uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_; i++) + { + val.unstructured.value.elements[val.unstructured.value.count++] = (uint8_t) rand(); // NOLINT + } + localRegisterAccessLock->Take(); + localRegister->read(REGISTER_UAVCAN_UNIQUE_ID, &val); + localRegisterAccessLock->Give(); + LOCAL_ASSERT(uavcan_register_Value_1_0_is_unstructured_(&val) && + val.unstructured.value.count == uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_); + memcpy(&out[0], &val.unstructured.value, uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_); +} + +/// @brief Legge il subjectID per il modulo corrente per la risposta al servizio di gestione dati. +/// @param modeAccessID tipo di accesso +/// @param port_name nome porta uavcan +/// @param type_name tipo nome registro +/// @return Canard Port ID associato +CanardPortID CanTask::getModeAccessID(uint8_t modeAccessID, const char* const port_name, const char* const type_name) { + // Deduce the register name from port name e modeAccess + char register_name[uavcan_register_Name_1_0_name_ARRAY_CAPACITY_] = {0}; + // In funzione del modo imposta il registro corretto + switch (modeAccessID) { + case canardClass::Introspection_Port::PublisherSubjectID: + snprintf(®ister_name[0], sizeof(register_name), "uavcan.pub.%s.id", port_name); + break; + case canardClass::Introspection_Port::SubscriptionSubjectID: + snprintf(®ister_name[0], sizeof(register_name), "uavcan.sub.%s.id", port_name); + break; + case canardClass::Introspection_Port::ClientPortID: + snprintf(®ister_name[0], sizeof(register_name), "uavcan.cln.%s.id", port_name); + break; + case canardClass::Introspection_Port::ServicePortID: + snprintf(®ister_name[0], sizeof(register_name), "uavcan.srv.%s.id", port_name); + break; + } + + // Set up the default value. It will be used to populate the register if it doesn't exist. + static uavcan_register_Value_1_0 val = {0}; + uavcan_register_Value_1_0_select_natural16_(&val); + val.natural16.value.count = 1; + val.natural16.value.elements[0] = UINT16_MAX; // This means "undefined", per Specification, which is the default. + + // Read the register with defensive self-checks. + localRegisterAccessLock->Take(); + localRegister->read(®ister_name[0], &val); + localRegisterAccessLock->Give(); + LOCAL_ASSERT(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == 1)); + const uint16_t result = val.natural16.value.elements[0]; + + // This part is NOT required but recommended by the Specification for enhanced introspection capabilities. It is + // very cheap to implement so all implementations should do so. This register simply contains the name of the + // type exposed at this port. It should be immutable but it is not strictly required so in this implementation + // we take shortcuts by making it mutable since it's behaviorally simpler in this specific case. + // In funzione del modo imposta il registro corretto + switch (modeAccessID) { + case canardClass::Introspection_Port::PublisherSubjectID: + snprintf(®ister_name[0], sizeof(register_name), "uavcan.pub.%s.type", port_name); + break; + case canardClass::Introspection_Port::SubscriptionSubjectID: + snprintf(®ister_name[0], sizeof(register_name), "uavcan.sub.%s.type", port_name); + break; + case canardClass::Introspection_Port::ClientPortID: + snprintf(®ister_name[0], sizeof(register_name), "uavcan.cln.%s.type", port_name); + break; + case canardClass::Introspection_Port::ServicePortID: + snprintf(®ister_name[0], sizeof(register_name), "uavcan.srv.%s.type", port_name); + break; + } + + uavcan_register_Value_1_0_select_string_(&val); + val._string.value.count = nunavutChooseMin(strlen(type_name), uavcan_primitive_String_1_0_value_ARRAY_CAPACITY_); + memcpy(&val._string.value.elements[0], type_name, val._string.value.count); + localRegisterAccessLock->Take(); + localRegister->read(®ister_name[0], &val); + localRegisterAccessLock->Give(); + + return result; +} + +/// @brief Scrive dati in append su Flash per scrittura sequenziale file data remoto +/// @param file_name nome del file UAVCAN +/// @param is_firmware true se il file +-è di tipo firmware +/// @param rewrite true se necessaria la riscrittura del file +/// @param buf blocco dati da scrivere in formato UAVCAN [256 Bytes] +/// @param count numero del blocco da scrivere in formato UAVCAN [Blocco x Buffer] +/// @return true if block saved OK, false on any error +bool CanTask::putFlashFile(const char* const file_name, const bool is_firmware, const bool rewrite, void* buf, size_t count) +{ + #ifdef CHECK_FLASH_WRITE + // check data (W->R) Verify Flash integrity OK + uint8_t check_data[FLASH_BUFFER_SIZE]; + #endif + // Request New File Init Upload + if(rewrite) { + // Qspi Security Semaphore + if(localQspiLock->Take(Ticks::MsToTicks(FLASH_SEMAPHORE_MAX_WAITING_TIME_MS))) { + // Init if required (DeInit after if required PowerDown Module) + if(localFlash->BSP_QSPI_Init() != Flash::QSPI_OK) { + localQspiLock->Give(); + return false; + } + // Check Status Flash OK + Flash::QSPI_StatusTypeDef sts = localFlash->BSP_QSPI_GetStatus(); + if (sts) { + localQspiLock->Give(); + return false; + } + // Start From PtrFlash 0x100 (Reserve 256 Bytes For InfoFile) + if (is_firmware) { + // Firmware Flash + canFlashPtr = FLASH_FW_POSITION; + } else { + // Standard File Data Upload + canFlashPtr = FLASH_FILE_POSITION; + } + // Get Block Current into Flash + canFlashBlock = canFlashPtr / AT25SF161_BLOCK_SIZE; + // Erase First Block Block (Block OF 4KBytes) + TRACE_INFO_F(F("FLASH: Erase block: %d\r\n"), canFlashBlock); + if (localFlash->BSP_QSPI_Erase_Block(canFlashBlock)) { + localQspiLock->Give(); + return false; + } + // Write Name File (Size at Eof...) + uint8_t file_flash_name[FLASH_FILE_SIZE_LEN] = {0}; + memcpy(file_flash_name, file_name, strlen(file_name)); + localFlash->BSP_QSPI_Write(file_flash_name, canFlashPtr, FLASH_FILE_SIZE_LEN); + // Write into Flash + TRACE_INFO_F(F("FLASH: Write [ %d ] bytes at addr: %d\r\n"), FLASH_FILE_SIZE_LEN, canFlashPtr); + #ifdef CHECK_FLASH_WRITE + localFlash->BSP_QSPI_Read(check_data, canFlashPtr, FLASH_FILE_SIZE_LEN); + if(memcmp(file_flash_name, check_data, FLASH_FILE_SIZE_LEN)==0) { + TRACE_INFO_F(F("FLASH: Reading check OK\r\n")); + } else { + TRACE_ERROR_F(F("FLASH: Reading check ERROR\r\n")); + localQspiLock->Give(); + return false; + } + #endif + // Start Page... + canFlashPtr += FLASH_INFO_SIZE_LEN; + localQspiLock->Give(); + } + } + // Write Data Block + // Qspi Security Semaphore + if(localQspiLock->Take(Ticks::MsToTicks(FLASH_SEMAPHORE_MAX_WAITING_TIME_MS))) { + // 0 = Is UavCan Signal EOF for Last Block Exact Len 256 Bytes... + // If Value Count is 0 no need to Write Flash Data (Only close Fule Info) + if(count!=0) { + // Write into Flash + TRACE_INFO_F(F("FLASH: Write [ %d ] bytes at addr: %d\r\n"), count, canFlashPtr); + // Starting Write at OFFSET Required... Erase here is Done + localFlash->BSP_QSPI_Write((uint8_t*)buf, canFlashPtr, count); + #ifdef CHECK_FLASH_WRITE + localFlash->BSP_QSPI_Read(check_data, canFlashPtr, count); + if(memcmp(buf, check_data, count)==0) { + TRACE_INFO_F(F("FLASH: Reading check OK\r\n")); + } else { + TRACE_ERROR_F(F("FLASH: Reading check ERROR\r\n")); + localQspiLock->Give(); + return false; + } + #endif + canFlashPtr += count; + // Check if Next Page Addressed (For Erase Next Block) + if((canFlashPtr / AT25SF161_BLOCK_SIZE) != canFlashBlock) { + canFlashBlock = canFlashPtr / AT25SF161_BLOCK_SIZE; + // Erase First Block Block (Block OF 4KBytes) + TRACE_INFO_F(F("FLASH: Erase block: %d\r\n"), canFlashBlock); + if (localFlash->BSP_QSPI_Erase_Block(canFlashBlock)) { + localQspiLock->Give(); + return false; + } + } + } + // Eof if != 256 Bytes Write + if(count!=0x100) { + // Write Info File for Closing... + // Size at + uint64_t lenghtFile = canFlashPtr - FLASH_INFO_SIZE_LEN; + if (is_firmware) { + // Firmware Flash + canFlashPtr = FLASH_FW_POSITION; + } else { + // Standard File Data Upload + canFlashPtr = FLASH_FILE_POSITION; + } + localFlash->BSP_QSPI_Write((uint8_t*)&lenghtFile, FLASH_SIZE_ADDR(canFlashPtr), FLASH_INFO_SIZE_U64); + // Write into Flash + TRACE_INFO_F(F("FLASH: Write [ %d ] bytes at addr: %d\r\n"), FLASH_INFO_SIZE_U64, canFlashPtr); + #ifdef CHECK_FLASH_WRITE + localFlash->BSP_QSPI_Read(check_data, FLASH_SIZE_ADDR(canFlashPtr), FLASH_INFO_SIZE_U64); + if(memcmp(&lenghtFile, check_data, FLASH_INFO_SIZE_U64)==0) { + TRACE_INFO_F(F("FLASH: Reading check OK\r\n")); + } else { + TRACE_INFO_F(F("FLASH: Reading check ERROR\r\n")); + } + #endif + } + localQspiLock->Give(); + } + return true; +} + +/// @brief GetInfo for Firmware File on Flash +/// @param module_type type module of firmware +/// @param version version firmware +/// @param revision revision firmware +/// @param len length of file in bytes +/// @return true if exixst +bool CanTask::getFlashFwInfoFile(uint8_t *module_type, uint8_t *version, uint8_t *revision, uint64_t *len) +{ + uint8_t block[FLASH_FILE_SIZE_LEN]; + bool fileReady = false; + + // Qspi Security Semaphore + if(localQspiLock->Take(Ticks::MsToTicks(FLASH_SEMAPHORE_MAX_WAITING_TIME_MS))) { + // Init if required (DeInit after if required PowerDown Module) + if(localFlash->BSP_QSPI_Init() != Flash::QSPI_OK) { + localQspiLock->Give(); + return false; + } + // Check Status Flash OK + if (localFlash->BSP_QSPI_GetStatus()) { + localQspiLock->Give(); + return false; + } + + // Read Name file, Version and Info + localFlash->BSP_QSPI_Read(block, 0, FLASH_FILE_SIZE_LEN); + char stima_name[STIMA_MODULE_NAME_LENGTH] = {0}; + getStimaNameByType(stima_name, MODULE_TYPE); + if(checkStimaFirmwareType((char*)block, module_type, version, revision)) { + localFlash->BSP_QSPI_Read((uint8_t*)len, FLASH_SIZE_ADDR(0), FLASH_INFO_SIZE_U64); + fileReady = true; + } + localQspiLock->Give(); + } + return fileReady; +} + +// *********************************************************************************************** +// *********************************************************************************************** +// FUNZIONI CHIAMATE DA MAIN_LOOP DI PUBBLICAZIONE E RICHIESTE DATI E SERVIZI +// *********************************************************************************************** +// *********************************************************************************************** + +// ******* FUNZIONI RMAP PUBLISH LOCAL DATA ********* + +/// @brief Prepara il blocco messaggio dati per il modulo corrente istantaneo +/// NB: Aggiorno solo i dati fisici in questa funzione i metadati sono esterni +/// @param sensore tipo di sensore richiesto rmap class_canard di modulo +/// @param report report data +/// @param rmap_data report data module output value per modulo sensore specifico publish +/// oppure in overload metodo tramite metodo Response applucapile al servizio request +void CanTask::prepareSensorsDataValue(uint8_t const sensore, const report_t *report, rmap_module_Leaf_1_0 *rmap_data) { + // Inserisco i dati reali + switch (sensore) { + case canardClass::Sensor_Type::bft: + // Prepara i dati SMP (Sample) + rmap_data->BFT.leaf.val.value = report->avg; + rmap_data->BFT.leaf.confidence.value = report->quality; + break; + } +} +void CanTask::prepareSensorsDataValue(uint8_t const sensore, const report_t *report, rmap_service_module_Leaf_Response_1_0 *rmap_data) { + // Inserisco i dati reali + switch (sensore) { + case canardClass::Sensor_Type::bft: + // Prepara i dati SMP (Sample) + rmap_data->BFT.leaf.val.value = report->avg; + rmap_data->BFT.leaf.confidence.value = report->quality; + break; + } +} + +/// @brief Pubblica i dati RMAP con il metodo publisher se abilitato e configurato +/// @param clCanard classe +/// @param param parametri del modulo UAVCAN +void CanTask::publish_rmap_data(canardClass &clCanard, CanParam_t *param) { + // Pubblica i dati del nodo corrente se abilitata la funzione e con il corretto subjectId + // Ovviamente il nodo non può essere anonimo per la pubblicazione... + if ((!clCanard.is_canard_node_anonymous()) && + (clCanard.publisher_enabled.module_leaf) && + (clCanard.port_id.publisher_module_leaf <= CANARD_SUBJECT_ID_MAX)) { + rmap_module_Leaf_1_0 module_leaf_msg = {0}; + + request_data_t request_data = {0}; + report_t report_pub = {0}; + + // preparo la struttura dati per richiedere i dati al task che li elabora + // in publish non inizializzo coda, pibblico in funzione del'ultima riichiesta di CFG + // Il dato report_time_s non risiede sullo slave ma è in chimata da master + request_data.is_init = false; // utilizza i dati esistenti (continua le elaborazioni precedentemente inizializzate) + request_data.report_time_s = last_req_rpt_time; // richiedo i dati in conformità a standard request (report) + request_data.observation_time_s = last_req_obs_time; // richiedo i dati in conformità a standard request (observation) + + // SET Dynamic metadata (Request data from master Only Data != Sample) + clCanard.module_leaf.BFT.metadata.timerange.P2 = request_data.report_time_s; + + // coda di richiesta dati + param->requestDataQueue->Enqueue(&request_data); + + // coda di attesa dati (attesa rmap_calc_data) + param->reportDataQueue->Dequeue(&report_pub); + TRACE_INFO_F(F("--> CAN leaf report\t%d\t%d\r\n"), (rmapdata_t) report_pub.avg, (rmapdata_t) report_pub.quality); + + // Preparo i dati + prepareSensorsDataValue(canardClass::Sensor_Type::bft, &report_pub, &module_leaf_msg); + // Metadata + module_leaf_msg.BFT.metadata = clCanard.module_leaf.BFT.metadata; + + // Serialize and publish the message: + uint8_t serialized[rmap_module_Leaf_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_] = {0}; + size_t serialized_size = sizeof(serialized); + const int8_t err = rmap_module_Leaf_1_0_serialize_(&module_leaf_msg, &serialized[0], &serialized_size); + LOCAL_ASSERT(err >= 0); + if (err >= 0) { + const CanardTransferMetadata meta = { + .priority = CanardPrioritySlow, + .transfer_kind = CanardTransferKindMessage, + .port_id = clCanard.port_id.publisher_module_leaf, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = (CanardTransferID)(clCanard.next_transfer_id.module_leaf()), // Increment! + }; + // Messaggio rapido 1/4 di secondo dal timeStamp Sincronizzato + clCanard.send(MEGA / 4, &meta, serialized_size, &serialized[0]); + } + } +} + +// *************************************************************************************************** +// Funzioni ed utility di ricezione dati dalla rete UAVCAN, richiamati da processReceivedTransfer() +// *************************************************************************************************** + +// Plug and Play Slave, Versione 1.0 compatibile con CAN_CLASSIC MTU 8 +// Messaggi anonimi CAN non sono consentiti se messaggi > LUNGHEZZA MTU disponibile + +/// @brief Plug and Play Slave, Versione 1.0 compatibile con CAN_CLASSIC MTU 8 +/// Messaggi anonimi CAN non sono consentiti se messaggi > LUNGHEZZA MTU disponibile +/// @param clCanard classe +/// @param msg messaggio di pubblicazione +void CanTask::processMessagePlugAndPlayNodeIDAllocation(canardClass &clCanard, + const uavcan_pnp_NodeIDAllocationData_1_0* const msg) { + // msg->unique_id_hash RX viene verificato per l'assegnazione in sicurezza del port_id corretto + uint64_t local_hash = StimaV4GetSerialNumber(); + local_hash >>= HASH_EXCLUDING_BIT; + local_hash &= HASH_SERNUMB_MASK; + local_hash |= MODULE_TYPE; + // Verifico hash messaggio ed assegno l'ID al registro locale se corretto + if (msg->unique_id_hash == local_hash) { + if (msg->allocated_node_id.elements[0].value <= CANARD_NODE_ID_MAX) { + printf("Got PnP node-ID allocation: %d\n", msg->allocated_node_id.elements[0].value); + clCanard.set_canard_node_id((CanardNodeID)msg->allocated_node_id.elements[0].value); + // Richiedo immediata configurazione del modulo al master... + clCanard.flag.set_local_module_ready(false); + // Store the value into the non-volatile storage. + static uavcan_register_Value_1_0 reg = {0}; + uavcan_register_Value_1_0_select_natural16_(®); + reg.natural16.value.elements[0] = msg->allocated_node_id.elements[0].value; + reg.natural16.value.count = 1; + localRegisterAccessLock->Take(); + localRegister->write(REGISTER_UAVCAN_NODE_ID, ®); + localRegisterAccessLock->Give(); + // We no longer need the subscriber, drop it to free up the resources (both memory and CPU time). + clCanard.rxUnSubscribe(CanardTransferKindMessage, + uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_); + } + } + // Otherwise, ignore it: either it is a request from another node or it is a response to another node. +} + +/// @brief Chiamate gestioni RPC remote da master (yakut o altro servizio di controllo) +/// @param clCanard Classe +/// @param req richiesta +/// @param remote_node nodo remoto +/// @return execute_command standard UAVCAN +uavcan_node_ExecuteCommand_Response_1_1 CanTask::processRequestExecuteCommand(canardClass &clCanard, const uavcan_node_ExecuteCommand_Request_1_1* req, + uint8_t remote_node) { + uavcan_node_ExecuteCommand_Response_1_1 resp = {0}; + // System Queue request Structure data + // Command-> Accelerometer Calibrate... + // Send Command directly To Task (Init to Null) + system_message_t system_message = {0}; + + // req->command (Comando esterno ricevuto 2 BYTES RESERVED FFFF-FFFA) + // Gli altri sono liberi per utilizzo interno applicativo con #define interne + // req->parameter (array di byte MAX 255 per i parametri da request) + // Risposta attuale (resp) 1 Bytes RESERVED (0..6) gli altri #define interne + switch (req->command) + { + // **************** Comandi standard UAVCAN GENERIC_SPECIFIC_COMMAND **************** + // Comando di aggiornamento Firmware compatibile con Yakut e specifice UAVCAN + case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_BEGIN_SOFTWARE_UPDATE: + { + // Nodo Server chiamante (Yakut solo Master, Yakut e Master per Slave) + clCanard.master.file.start_request(remote_node, (uint8_t*) req->parameter.elements, + req->parameter.count, true); + clCanard.flag.set_local_fw_uploading(true); + TRACE_INFO_F(F("Firmware update request from node id: %u\r\n"), clCanard.master.file.get_server_node()); + TRACE_INFO_F(F("Filename to download: %s\r\n"), clCanard.master.file.get_name()); + // Avvio la funzione con OK + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; + break; + } + case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_FACTORY_RESET: + { + localRegisterAccessLock->Take(); + localRegister->doFactoryReset(); + localRegisterAccessLock->Give(); + // Istant Reboot for next Register base Setup + NVIC_SystemReset(); + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; + break; + } + case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_RESTART: + { + clCanard.flag.request_system_restart(); + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; + break; + } + case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_STORE_PERSISTENT_STATES: + { + // If your registers are not automatically synchronized with the non-volatile storage, use this command + // to commit them to the storage explicitly. Otherwise it is safe to remove it. + // In this demo, the registers are stored in files, so there is nothing to do. + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; + break; + } + // **************** Comandi personalizzati VENDOR_SPECIFIC_COMMAND **************** + // Comando di download File generico compatibile con specifice UAVCAN, (LOG/CFG altro...) + case canardClass::Command_Private::download_file: + { + // Nodo Server chiamante (Yakut solo Master, Yakut e Master per Slave) + clCanard.master.file.start_request(remote_node, (uint8_t*) req->parameter.elements, + req->parameter.count, false); + clCanard.flag.set_local_fw_uploading(true); + TRACE_INFO_F(F("File standard update request from node id: %u\r\n"), clCanard.master.file.get_server_node()); + TRACE_INFO_F(F("Filename to download: %s\r\n"), clCanard.master.file.get_name()); + // Avvio la funzione con OK + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; + break; + } + case canardClass::Command_Private::calibrate_accelerometer: + { + // Avvia calibrazione accelerometro (reset bolla elettroniuca) + TRACE_INFO_F(F("AVVIA Calibrazione accelerometro e salvataggio parametri\r\n")); + // Send queue command to TASK + system_message.task_dest = ACCELEROMETER_TASK_ID; + system_message.command.do_calib = true; + if(localSystemMessageQueue->Enqueue(&system_message, Ticks::MsToTicks(WAIT_QUEUE_REQUEST_COMMAND_MS))) { + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; + } else { + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_FAILURE; + } + break; + } + case canardClass::Command_Private::module_maintenance: + { + // Avvia/Arresta modalità di manutenzione come comando sul system status + if(req->parameter.elements[0]) { + TRACE_INFO_F(F("AVVIA modalità di manutenzione modulo\r\n")); + } else { + TRACE_INFO_F(F("ARRESTA modalità di manutenzione modulo\r\n")); + } + // Send queue command to TASK + system_message.task_dest = SUPERVISOR_TASK_ID; + system_message.command.do_maint = 1; + system_message.param = req->parameter.elements[0]; + if(localSystemMessageQueue->Enqueue(&system_message, Ticks::MsToTicks(WAIT_QUEUE_REQUEST_COMMAND_MS))) { + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; + } else { + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_FAILURE; + } + break; + } + case canardClass::Command_Private::reset_flags: + { + // Avvia calibrazione accelerometro (reset bolla elettroniuca) + TRACE_INFO_F(F("RESET flags di sistema e salvataggio stati\r\n")); + // Reset counter on new or restored firmware + boot_state->tot_reset = 0; + boot_state->wdt_reset = 0; + // Save info bootloader block + localEeprom->Write(BOOT_LOADER_STRUCT_ADDR, (uint8_t*) boot_state, sizeof(bootloader_t)); + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; + break; + } + case canardClass::Command_Private::enable_publish_rmap: + { + // Abilita pubblicazione fast_loop data_and_metadata modulo locale (test yakut e user master) + clCanard.publisher_enabled.module_leaf = true; + TRACE_INFO_F(F("ATTIVO Trasmissione dati in publish\r\n")); + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; + break; + } + case canardClass::Command_Private::disable_publish_rmap: + { + // Disabilita pubblicazione fast_loop data_and_metadata modulo locale (test yakut e user master) + clCanard.publisher_enabled.module_leaf = false; + TRACE_INFO_F(F("DISATTIVO Trasmissione dati in publish\r\n")); + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; + break; + } + case canardClass::Command_Private::enable_publish_port_list: + { + // Abilita pubblicazione slow_loop elenco porte (Cypal facoltativo) + clCanard.publisher_enabled.port_list = true; + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; + break; + } + case canardClass::Command_Private::disable_publish_port_list: + { + // Disabilita pubblicazione slow_loop elenco porte (Cypal facoltativo) + clCanard.publisher_enabled.port_list = false; + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; + break; + } + case canardClass::Command_Private::remote_test: + { + // Test locale / remoto + resp.status = canardClass::Command_Private::remote_test_value; + break; + } + default: + { + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_BAD_COMMAND; + break; + } + } + return resp; +} + +/// @brief Chiamate gestioni dati remote da master (yakut o altro servizio di controllo) +/// @param clCanard Classe +/// @param req richiesta +/// @param param parametri CAN +/// @return dati rmap_servizio di modulo +rmap_service_module_Leaf_Response_1_0 CanTask::processRequestGetModuleData(canardClass &clCanard, rmap_service_module_Leaf_Request_1_0* req, CanParam_t *param) { + rmap_service_module_Leaf_Response_1_0 resp = {0}; + // Richiesta parametri univoca a tutti i moduli + // req->parametri tipo: rmap_service_setmode_1_0 + // req->parameter.command (Comando esterno ricevuto 3 BIT) + // req->parametri.run_sectime (Timer to run 13 bit) + + request_data_t request_data = {0}; + report_t report_srv = {0}; + bool isRunIdleHookEnabled; + + // Case comandi RMAP su GetModule Data (Da definire con esattezza quali e quanti altri) + switch (req->parameter.command) { + + /// saturated uint3 get_istant = 0 + /// Ritorna il dato come richiesto dal master + case rmap_service_setmode_1_0_get_istant: // saturated uint3 get_istant = 0 + case rmap_service_setmode_1_0_get_current: // saturated uint3 get_current = 1 + case rmap_service_setmode_1_0_get_last: // saturated uint3 get_last = 2 + + // preparo la struttura dati per richiedere i dati al task che li elabora + if(req->parameter.command == rmap_service_setmode_1_0_get_istant) { + // Solo Sample istantaneo + request_data.is_init = false; // No Init, Sample + request_data.report_time_s = 0; // richiedo i dati su 0 secondi (Sample) + request_data.observation_time_s = 0; // richiedo i dati mediati su 0 secondi (Sample) + } + if((req->parameter.command == rmap_service_setmode_1_0_get_current)|| + (req->parameter.command == rmap_service_setmode_1_0_get_last)) { + // Dato corrente con o senza inizializzazione (get_last...) + if(req->parameter.command == rmap_service_setmode_1_0_get_current) + request_data.is_init = false; // utilizza i dati esistenti (continua le elaborazioni precedentemente inizializzate) + else + request_data.is_init = true; // Reinit delle elaborazioni + request_data.report_time_s = req->parameter.run_sectime; // richiedo i dati su request secondi + request_data.observation_time_s = req->parameter.obs_sectime; // richiedo i dati mediati su request secondi + last_req_rpt_time = req->parameter.run_sectime; // report_time_request_backup; + last_req_obs_time = req->parameter.obs_sectime; // observation_time_request_backup; + // SET Dynamic metadata (Request data from master Only Data != Sample) + clCanard.module_leaf.BFT.metadata.timerange.P2 = request_data.report_time_s; + } + + // Preparo il ritorno dei flag event status del sensore (Prima di request/reset) + resp.is_adc_unit_error = param->system_status->events.is_adc_unit_error; + resp.is_adc_unit_overflow = param->system_status->events.is_adc_unit_overflow; + // Preparo gli event Reboot and WDT Event + resp.rbt_event = boot_state->tot_reset; + resp.wdt_event = boot_state->wdt_reset; + + // coda di richiesta dati immediata. Full power for make report + isRunIdleHookEnabled = LowPower.isIdleHookEnabled(); + LowPower.idleHookDisable(); + param->requestDataQueue->Enqueue(&request_data); + // coda di attesa dati (attesa rmap_calc_data) + param->reportDataQueue->Dequeue(&report_srv); + if(isRunIdleHookEnabled) LowPower.idleHookEnable(); + + TRACE_INFO_F(F("--> CAN leaf report\t%d\t%d\r\n"), (rmapdata_t) report_srv.avg, (rmapdata_t) report_srv.quality); + + // Ritorno lo stato (Copia dal comando... con stato maintenence e versioni di modulo) + resp.state = req->parameter.command; // saturated 4BIT (3BITCommand + FlagMaintenance) + if(param->system_status->flags.is_maintenance) resp.state |= CAN_FLAG_IS_MAINTENANCE_MODE; + resp.version = MODULE_MAIN_VERSION; + resp.revision = MODULE_MINOR_VERSION; + // Preparo la risposta con i dati recuperati dalla coda (come da request CAN) + // Esiste il solo caso ::bft sia per get istant che archivio + prepareSensorsDataValue(canardClass::Sensor_Type::bft, &report_srv, &resp); + break; + + // NOT USED + // saturated uint3 stop_acq = 4 + // #define rmap_service_setmode_1_0_stop_acq (4U) + // saturated uint3 loop_acq = 5 + // #define rmap_service_setmode_1_0_loop_acq (5U) + // saturated uint3 continuos_acq = 6 + // #define rmap_service_setmode_1_0_continuos_acq (6U) + + /// saturated uint3 test_acq = 7 (Solo x TEST) + case rmap_service_setmode_1_0_test_acq: + resp.state = req->parameter.command; + break; + + /// NON Gestito, risposta error (undefined) + default: + resp.state = GENERIC_STATE_UNDEFINED; + break; + } + + // Copio i metadati fissi e mobili + resp.BFT.metadata = clCanard.module_leaf.BFT.metadata; + + return resp; +} + +/// @brief Accesso ai registri UAVCAN risposta a richieste +/// @param req Richiesta +/// @return register access UAVCAN +uavcan_register_Access_Response_1_0 CanTask::processRequestRegisterAccess(const uavcan_register_Access_Request_1_0* req) { + char name[uavcan_register_Name_1_0_name_ARRAY_CAPACITY_ + 1] = {0}; + LOCAL_ASSERT(req->name.name.count < sizeof(name)); + memcpy(&name[0], req->name.name.elements, req->name.name.count); + name[req->name.name.count] = '\0'; + + uavcan_register_Access_Response_1_0 resp = {0}; + + // If we're asked to write a new value, do it now: + if (!uavcan_register_Value_1_0_is_empty_(&req->value)) { + uavcan_register_Value_1_0_select_empty_(&resp.value); + localRegisterAccessLock->Take(); + localRegister->read(&name[0], &resp.value); + localRegisterAccessLock->Give(); + // If such register exists and it can be assigned from the request value: + if (!uavcan_register_Value_1_0_is_empty_(&resp.value) && localRegister->assign(&resp.value, &req->value)) { + localRegisterAccessLock->Take(); + localRegister->write(&name[0], &resp.value); + localRegisterAccessLock->Give(); + } + } + + // Regardless of whether we've just wrote a value or not, we need to read the current one and return it. + // The client will determine if the write was successful or not by comparing the request value with response. + uavcan_register_Value_1_0_select_empty_(&resp.value); + localRegisterAccessLock->Take(); + localRegister->read(&name[0], &resp.value); + localRegisterAccessLock->Give(); + + // Currently, all registers we implement are mutable and persistent. This is an acceptable simplification, + // but more advanced implementations will need to differentiate between them to support advanced features like + // exposing internal states via registers, perfcounters, etc. + resp._mutable = true; + resp.persistent = true; + + // Our node does not synchronize its time with the network so we can't populate the timestamp. + resp.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN; + + return resp; +} + +/// @brief Risposta a uavcan.node.GetInfo which Info Node (nome, versione, uniqueID di verifica ecc...) +/// @return Risposta node Get INFO UAVCAN +uavcan_node_GetInfo_Response_1_0 CanTask::processRequestNodeGetInfo() { + uavcan_node_GetInfo_Response_1_0 resp = {0}; + resp.protocol_version.major = CANARD_CYPHAL_SPECIFICATION_VERSION_MAJOR; + resp.protocol_version.minor = CANARD_CYPHAL_SPECIFICATION_VERSION_MINOR; + + // The hardware version is not populated in this demo because it runs on no specific hardware. + // An embedded node would usually determine the version by querying the hardware. + + resp.software_version.major = MODULE_MAIN_VERSION; + resp.software_version.minor = MODULE_MINOR_VERSION; + resp.software_vcs_revision_id = RMAP_PROCOTOL_VERSION; + + getUniqueID(resp.unique_id, StimaV4GetSerialNumber()); + + // The node name is the name of the product like a reversed Internet domain name (or like a Java package). + char stima_name[STIMA_MODULE_NAME_LENGTH] = {0}; + getStimaNameByType(stima_name, MODULE_TYPE); + resp.name.count = strlen(stima_name); + memcpy(&resp.name.elements, stima_name, resp.name.count); + + // The software image CRC and the Certificate of Authenticity are optional so not populated in this demo. + return resp; +} + +// ****************************************************************************************** +// CallBack di classe canardClass ( Gestisce i metodi uavcan sottoscritti ) +// Processo multiplo di ricezione messaggi e comandi. Gestione entrata ed uscita dei messaggi +// Chiamata direttamente nel main loop in ricezione dalla coda RX +// Richiama le funzioni qui sopra di preparazione e risposta alle richieste +// ****************************************************************************************** + +/// @brief Canard CallBACK Receive message to Node (Solo con sottoscrizioni) +/// @param clCanard Classe +/// @param transfer Transfer CanardID +/// @param param Indirizzo di parametri esterni opzionali +void CanTask::processReceivedTransfer(canardClass &clCanard, const CanardRxTransfer* const transfer, void *param) { + + // System Queue request Structure data + // Command-> Accelerometer Calibrate... + // Send Command directly To Task (Init to Null) + system_message_t system_message = {0}; + + // Gestione dei Messaggi in ingresso + if (transfer->metadata.transfer_kind == CanardTransferKindMessage) + { + if (transfer->metadata.port_id == uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_) + { + // Ricevo messaggi Heartbeat per stato rete (Controllo esistenza del MASTER) + // Solo a scopo precauzionale per attività da gestire alla cieca (SAVE QUEUE LOG, DATA ecc...) + size_t size = transfer->payload_size; + uavcan_node_Heartbeat_1_0 msg = {0}; + if (uavcan_node_Heartbeat_1_0_deserialize_(&msg, static_cast(transfer->payload), &size) >= 0) { + // Controllo e gestisco solo il nodo MASTER + if(transfer->metadata.remote_node_id == clCanard.get_canard_master_id()) { + // Entro in OnLine se precedentemente arrivo dall'OffLine + // ed eseguo eventuali operazioni di entrata in attività se necessario + // Opzionale Controllo ONLINE direttamente dal messaggio Interno + // if (!clCanard.master.heartbeat.is_online()) { + // Il master è entrato in modalità ONLine e gestisco + // TRACE_VERBOSE(F("Master controller ONLINE !!! Starting application...")); + // } + // Set PowerMode da comando HeartBeat Remoto remoteVSC.powerMode + // Eventuali alri flag gestiti direttamente quà e SET sulla classe + canardClass::heartbeat_VSC remoteVSC; + // remoteVSC.powerMode + remoteVSC.uint8_val = msg.vendor_specific_status_code; + TRACE_INFO_F(F("RX HeartBeat from master, master power mode SET: -> %u\r\n"), remoteVSC.powerMode); + + // Processo e registro il nodo: stato, OnLine e relativi flag + // Set canard_us local per controllo NodoOffline (validità area di OnLine) + clCanard.master.heartbeat.set_online(MASTER_OFFLINE_TIMEOUT_US); + // SET Modalità Power richiesta dal Master (in risposta ad HeartBeat locale...) + // SErve come conferma al master e come flag locale di azione Power + clCanard.flag.set_local_power_mode(remoteVSC.powerMode); + + // PowerOn, Non faccio nulla o eventuale altra gestione (Tutto ON) + // if(remoteVSC.powerMode == canardClass::Power_Mode::pwr_on) { + // Ipotesi All Power Sensor ON... + // } + + // Gestisco lo stato Power come richiesto dal Master immediatamente se != power_on + if(remoteVSC.powerMode == Power_Mode::pwr_nominal) { + // ENTER STANDARD SLEEP FROM CAN COMMAND + #ifndef _EXIT_SLEEP_FOR_DEBUGGING + system_message.task_dest = ALL_TASK_ID; + system_message.command.do_sleep = true; + localSystemMessageQueue->Enqueue(&system_message, Ticks::MsToTicks(WAIT_QUEUE_REQUEST_COMMAND_MS)); + #endif + } + + // if(remoteVSC.powerMode == canardClass::Power_Mode::pwr_deep_save) { + // Ipotesi CAN ON solo al passaggio del minuto 57..60 per syncroTime, Cmd e DataSend + // } + + // if(remoteVSC.powerMode == canardClass::Power_Mode::pwr_critical) { + // Ipotesi DeepSleep. Save Param in Flash e PowerDown completo 60 Minuti... + // } + } + } + } + else if (transfer->metadata.port_id == uavcan_time_Synchronization_1_0_FIXED_PORT_ID_) + { + // Ricevo messaggi Heartbeat per syncro_time CanardMicrosec (Base dei tempi locali dal MASTER) + // Gestione differenze del tempo tra due comunicazioni Canard time_incro del master (local adjust time) + size_t size = transfer->payload_size; + uavcan_time_Synchronization_1_0 msg = {0}; + if (uavcan_time_Synchronization_1_0_deserialize_(&msg, static_cast(transfer->payload), &size) >= 0) { + // Controllo e gestisco solo il nodo MASTER come SyncroTime (a gestione locale) + // Non sono previsti multi sincronizzatori ma la selezione è esterna alla classe + if(transfer->metadata.remote_node_id == clCanard.get_canard_master_id()) { + bool isSyncronized = false; + CanardMicrosecond timestamp_synchronized_us; + // Controllo coerenza messaggio per consentire e verificare l'aggiornamento timestamp + if(clCanard.master.timestamp.check_valid_syncronization( + transfer->metadata.transfer_id, + msg.previous_transmission_timestamp_microsecond)) { + // Leggo il time stamp sincronizzato da gestire per Setup RTC + timestamp_synchronized_us = clCanard.master.timestamp.get_timestamp_syncronized( + transfer->timestamp_usec, + msg.previous_transmission_timestamp_microsecond); + isSyncronized = true; + } else { + TRACE_VERBOSE_F(F("RX TimeSyncro from master, reset or invalid Value at local_time_stamp (uSec): %u\r\n"), transfer->timestamp_usec); + } + // Aggiorna i temporizzatori locali per il prossimo controllo + CanardMicrosecond last_message_diff_us = clCanard.master.timestamp.update_timestamp_message( + transfer->timestamp_usec, msg.previous_transmission_timestamp_microsecond); + // Stampa e/o SETUP RTC con sincronizzazione valida + if (isSyncronized) { + #if (TRACE_LEVEL >= TRACE_LEVEL_VERBOSE) + uint32_t low_ms = timestamp_synchronized_us % 1000u; + uint32_t high_ms = timestamp_synchronized_us / 1000u; + TRACE_VERBOSE_F(F("RX TimeSyncro from master, syncronized value is (uSec): ")); + TRACE_VERBOSE_F(F("%lu"), high_ms); + TRACE_VERBOSE_F(F("%lu\r\n"), low_ms); + TRACE_VERBOSE_F(F(" from 2 message difference is (uSec): %lu\r\n"), (uint32_t)last_message_diff_us); + #endif + // *********** Adjust Local RTC Time ************* + uint32_t currentSubsecond; + volatile uint64_t canardEpoch_ms; + // Get Millisecond from TimeStampSyncronized + uint64_t timeStampEpoch_ms = timestamp_synchronized_us / 1000ul; + // Second Epoch in Millisecond With Add Subsecond + canardEpoch_ms = (uint64_t) rtc.getEpoch(¤tSubsecond); + canardEpoch_ms *= 1000ul; + canardEpoch_ms += currentSubsecond; + // Refer to milliSecond for TimeStamp ( Set Epoch with abs (ull) difference > 250 mSec ) + if(canardEpoch_ms > timeStampEpoch_ms) { + currentSubsecond = canardEpoch_ms - timeStampEpoch_ms; + } else { + currentSubsecond = timeStampEpoch_ms - canardEpoch_ms; + } + // currentSubsecond -> millisDifference from RTC and TimeStamp_Syncro_ms + if(currentSubsecond > 250) { + // Set RTC Epoch DateTime with timeStampEpoch (round mSec + 1) + TRACE_VERBOSE_F(F("RTC: Adjust time with TimeSyncro from master (difference %lu mS)\r\n"), currentSubsecond); + timeStampEpoch_ms++; + rtc.setEpoch(timeStampEpoch_ms / 1000, timeStampEpoch_ms % 1000); + } + } + } + } + } + else if (transfer->metadata.port_id == uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_) + { + // PLUG AND PLAY (Dovrei ricevere solo in anonimo) + size_t size = transfer->payload_size; + uavcan_pnp_NodeIDAllocationData_1_0 msg = {0}; + if (uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, static_cast(transfer->payload), &size) >= 0) { + processMessagePlugAndPlayNodeIDAllocation(clCanard, &msg); + } + } + else + { + // Gestione di un messaggio senza sottoscrizione. Se arrivo quà è un errore di sviluppo + LOCAL_ASSERT(false); + } + } + else if (transfer->metadata.transfer_kind == CanardTransferKindRequest) + { + // Gestione delle richieste esterne + if (transfer->metadata.port_id == clCanard.port_id.service_module_leaf) { + // Richiesta ai dati e metodi di sensor drive + rmap_service_module_Leaf_Request_1_0 req = {0}; + size_t size = transfer->payload_size; + TRACE_INFO_F(F("<<-- Ricevuto richiesta dati da master\r\n")); + // The request object is empty so we don't bother deserializing it. Just send the response. + if (rmap_service_module_Leaf_Request_1_0_deserialize_(&req, static_cast(transfer->payload), &size) >= 0) { + // I dati e metadati sono direttamente popolati in processRequestGetModuleData + rmap_service_module_Leaf_Response_1_0 module_leaf_resp = processRequestGetModuleData(clCanard, &req, (CanParam_t *) param); + // Serialize and publish the message: + uint8_t serialized[rmap_service_module_Leaf_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_] = {0}; + size_t serialized_size = sizeof(serialized); + const int8_t res = rmap_service_module_Leaf_Response_1_0_serialize_(&module_leaf_resp, &serialized[0], &serialized_size); + if (res >= 0) { + // Risposta standard con validità time out RMAP_DATA personalizzato dal timeStamp Sincronizzato + clCanard.sendResponse(CANARD_RMAPDATA_TRANSFER_ID_TIMEOUT_USEC, &transfer->metadata, serialized_size, &serialized[0]); + } + } + } + else if (transfer->metadata.port_id == uavcan_node_GetInfo_1_0_FIXED_PORT_ID_) + { + // The request object is empty so we don't bother deserializing it. Just send the response. + const uavcan_node_GetInfo_Response_1_0 resp = processRequestNodeGetInfo(); + uint8_t serialized[uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_] = {0}; + size_t serialized_size = sizeof(serialized); + const int8_t res = uavcan_node_GetInfo_Response_1_0_serialize_(&resp, &serialized[0], &serialized_size); + if (res >= 0) { + // Risposta standard ad un secondo dal timeStamp Sincronizzato + clCanard.sendResponse(MEGA, &transfer->metadata, serialized_size, &serialized[0]); + } + } + else if (transfer->metadata.port_id == uavcan_register_Access_1_0_FIXED_PORT_ID_) + { + uavcan_register_Access_Request_1_0 req = {0}; + size_t size = transfer->payload_size; + TRACE_INFO_F(F("<<-- Ricevuto richiesta accesso ai registri\r\n")); + if (uavcan_register_Access_Request_1_0_deserialize_(&req, static_cast(transfer->payload), &size) >= 0) { + const uavcan_register_Access_Response_1_0 resp = processRequestRegisterAccess(&req); + uint8_t serialized[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_] = {0}; + size_t serialized_size = sizeof(serialized); + if (uavcan_register_Access_Response_1_0_serialize_(&resp, &serialized[0], &serialized_size) >= 0) { + // Risposta standard ad un secondo dal timeStamp Sincronizzato + clCanard.sendResponse(MEGA, &transfer->metadata, serialized_size, &serialized[0]); + } + } + } + else if (transfer->metadata.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) + { + uavcan_register_List_Request_1_0 req = {0}; + size_t size = transfer->payload_size; + TRACE_INFO_F(F("<<-- Ricevuto richiesta lettura elenco registri\r\n")); + if (uavcan_register_List_Request_1_0_deserialize_(&req, static_cast(transfer->payload), &size) >= 0) { + const uavcan_register_List_Response_1_0 resp = {.name = localRegister->getNameByIndex(req.index)}; + uint8_t serialized[uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_] = {0}; + size_t serialized_size = sizeof(serialized); + if (uavcan_register_List_Response_1_0_serialize_(&resp, &serialized[0], &serialized_size) >= 0) { + // Risposta standard ad un secondo dal timeStamp Sincronizzato + clCanard.sendResponse(CANARD_REGISTERLIST_TRANSFER_ID_TIMEOUT_USEC, &transfer->metadata, serialized_size, &serialized[0]); + } + } + } + else if (transfer->metadata.port_id == uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_) + { + uavcan_node_ExecuteCommand_Request_1_1 req = {0}; + size_t size = transfer->payload_size; + TRACE_INFO_F(F("<<-- Ricevuto comando esterno\r\n")); + if (uavcan_node_ExecuteCommand_Request_1_1_deserialize_(&req, static_cast(transfer->payload), &size) >= 0) { + const uavcan_node_ExecuteCommand_Response_1_1 resp = processRequestExecuteCommand(clCanard, &req, transfer->metadata.remote_node_id); + uint8_t serialized[uavcan_node_ExecuteCommand_Response_1_1_SERIALIZATION_BUFFER_SIZE_BYTES_] = {0}; + size_t serialized_size = sizeof(serialized); + if (uavcan_node_ExecuteCommand_Response_1_1_serialize_(&resp, &serialized[0], &serialized_size) >= 0) { + // Risposta standard ad un secondo dal timeStamp Sincronizzato + clCanard.sendResponse(MEGA, &transfer->metadata, serialized_size, &serialized[0]); + } + } + } + else + { + // Gestione di una richiesta senza controllore locale. Se arrivo quà è un errore di sviluppo + LOCAL_ASSERT(false); + } + } + else if (transfer->metadata.transfer_kind == CanardTransferKindResponse) + { + if (transfer->metadata.port_id == uavcan_file_Read_1_1_FIXED_PORT_ID_) { + // Accetto solo messaggi indirizzati dal node_id che ha fatto la richiesta di upload + // E' una sicurezza per il controllo dell'upload, ed evità errori di interprete + // Inoltre non accetta messaggi extra standard UAVCAN, necessarià prima la CALL al comando + // SEtFirmwareUpload o SetFileUpload, che impostano il node_id, resettato su EOF dalla classe + if (clCanard.master.file.get_server_node() == transfer->metadata.remote_node_id) { + uavcan_file_Read_Response_1_1 resp = {0}; + size_t size = transfer->payload_size; + if (uavcan_file_Read_Response_1_1_deserialize_(&resp, static_cast(transfer->payload), &size) >= 0) { + if(clCanard.master.file.is_firmware()) { + TRACE_VERBOSE_F(F("RX FIRMWARE READ BLOCK LEN: ")); + } else { + TRACE_VERBOSE_F(F("RX FILE READ BLOCK LEN: ")); + } + TRACE_VERBOSE_F(F("%d\r\n"), resp.data.value.count); + // Save Data in Flash File at Block Position (Init = Rewrite file...) + if(putFlashFile(clCanard.master.file.get_name(), clCanard.master.file.is_firmware(), clCanard.master.file.is_first_data_block(), + resp.data.value.elements, resp.data.value.count)) { + // Reset pending command (Comunico request/Response Serie di comandi OK!!!) + // Uso l'Overload con controllo di EOF (-> EOF se msgLen != UAVCAN_BLOCK_DEFAULT [256 Bytes]) + // Questo Overload gestisce in automatico l'offset del messaggio, per i successivi blocchi + clCanard.master.file.reset_pending(resp.data.value.count); + } else { + // Error Save... Abort request + TRACE_ERROR_F(F("SAVING BLOCK FILE ERROR, ABORT RX !!!")); + clCanard.master.file.download_end(); + } + } + } else { + // Errore Nodo non settato... + TRACE_ERROR_F(F("RX FILE READ BLOCK REJECT: Node_Id not valid or not set\r\n")); + } + } + } + else + { + // Se arrivo quà è un errore di sviluppo, controllare setup sottoscrizioni e Rqst (non gestito slave) + LOCAL_ASSERT(false); + } +} + +/// @brief Construct the Can Task::CanTask object Main TASK && INIT TASK --- UAVCAN +/// @param taskName name of the task +/// @param stackSize size of the stack +/// @param priority priority of the task +/// @param canParam parameters for the task +CanTask::CanTask(const char *taskName, uint16_t stackSize, uint8_t priority, CanParam_t canParam) : Thread(taskName, stackSize, priority), param(canParam) +{ + // Start WDT controller and TaskState Flags + TaskWatchDog(WDT_STARTING_TASK_MS); + TaskState(CAN_STATE_CREATE, UNUSED_SUB_POSITION, task_flag::normal); + + // Setup register mode + localRegister = param.clRegister; + + // Setup Flash Access + localFlash = param.flash; + + // Local static access to global queue and Semaphore + localSystemMessageQueue = param.systemMessageQueue; + localQspiLock = param.qspiLock; + localRegisterAccessLock = param.registerAccessLock; + + boot_state = param.boot_request; + localEeprom = param.eeprom; + + // FullChip Power Mode after Startup + // Resume from LowPower or reset the controller TJA1443ATK + // Need FullPower for bxCan Programming (Otherwise Handler_Error()!) + HW_CAN_Power(CAN_ModePower::CAN_INIT); + + TRACE_INFO_F(F("Starting CAN Configuration\r\n")); + + // ******************* CANARD MTU CLASSIC (FOR UAVCAN REQUIRE) ******************* + // Open Register in Write se non inizializzati correttamente... + // Populate INIT Default Value + static uavcan_register_Value_1_0 val = {0}; + uavcan_register_Value_1_0_select_natural16_(&val); + val.natural16.value.count = 1; + val.natural16.value.elements[0] = CAN_MTU_BASE; // CAN_CLASSIC MTU 8 + localRegisterAccessLock->Take(); + localRegister->read(REGISTER_UAVCAN_MTU, &val); + localRegisterAccessLock->Give(); + LOCAL_ASSERT(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == 1)); + + // ******************* CANARD SETUP TIMINGS AND SPEED ******************* + // CAN BITRATE Dinamico su LoadRegister (CAN_FD 2xREG natural32 0=Speed, 1=0 (Not Used)) + uavcan_register_Value_1_0_select_natural32_(&val); + val.natural32.value.count = 2; + val.natural32.value.elements[0] = CAN_BIT_RATE; + val.natural32.value.elements[1] = 0ul; // Ignored for CANARD_MTU_CAN_CLASSIC + localRegisterAccessLock->Take(); + localRegister->read(REGISTER_UAVCAN_BITRATE, &val); + localRegisterAccessLock->Give(); + LOCAL_ASSERT(uavcan_register_Value_1_0_is_natural32_(&val) && (val.natural32.value.count == 2)); + + // Dynamic BIT RATE Change CAN Speed to CAN_BIT_RATE (register default/defined) + BxCANTimings timings; + bool result = bxCANComputeTimings(HAL_RCC_GetPCLK1Freq(), val.natural32.value.elements[0], &timings); + if (!result) { + TRACE_VERBOSE_F(F("Error redefinition bxCANComputeTimings, try loading default...\r\n")); + val.natural32.value.count = 2; + val.natural32.value.elements[0] = CAN_BIT_RATE; + val.natural32.value.elements[1] = 0ul; // Ignored for CANARD_MTU_CAN_CLASSIC + localRegisterAccessLock->Take(); + localRegister->write(REGISTER_UAVCAN_BITRATE, &val); + localRegisterAccessLock->Give(); + result = bxCANComputeTimings(HAL_RCC_GetPCLK1Freq(), val.natural32.value.elements[0], &timings); + if (!result) { + TRACE_ERROR_F(F("Error initialization bxCANComputeTimings\r\n")); + LOCAL_ASSERT(false); + return; + } + } + + // HW Setup solo con modulo CAN Attivo + #if (ENABLE_CAN) + + // Configurea bxCAN speed && mode + result = bxCANConfigure(0, timings, false); + if (!result) { + TRACE_ERROR_F(F("Error initialization bxCANConfigure\r\n")); + LOCAL_ASSERT(false); + return; + } + // ******************* CANARD SETUP TIMINGS AND SPEED COMPLETE ******************* + + // Check error starting CAN + if (HAL_CAN_Start(&hcan1) != HAL_OK) { + TRACE_ERROR_F(F("CAN startup ERROR!!!\r\n")); + LOCAL_ASSERT(false); + return; + } + + // Enable Interrupt RX Standard CallBack -> CAN1_RX0_IRQHandler + if (HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK) { + TRACE_ERROR_F(F("Error initialization interrupt CAN base\r\n")); + LOCAL_ASSERT(false); + return; + } + // Setup Priority e CB CAN_IRQ_RX Enable + HAL_NVIC_SetPriority(CAN1_RX0_IRQn, CAN_NVIC_INT_PREMPT_PRIORITY, 0); + HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn); + + // Setup Complete + TRACE_VERBOSE_F(F("CAN Configuration complete...\r\n")); + + #endif + + // Run Task Init + state = CAN_STATE_INIT; + Start(); +}; + +#if (ENABLE_STACK_USAGE) +/// @brief local stack Monitor (optional) +void CanTask::TaskMonitorStack() +{ + uint16_t stackUsage = (uint16_t)uxTaskGetStackHighWaterMark( NULL ); + if((stackUsage) && (stackUsage < param.system_status->tasks[LOCAL_TASK_ID].stack)) { + param.systemStatusLock->Take(); + param.system_status->tasks[LOCAL_TASK_ID].stack = stackUsage; + param.systemStatusLock->Give(); + } +} +#endif + +/// @brief local watchDog and Sleep flag Task (optional) +/// @param millis_standby time in ms to perfor check of WDT. If longer than WDT Reset, WDT is temporanly suspend +void CanTask::TaskWatchDog(uint32_t millis_standby) +{ + // Local TaskWatchDog update + param.systemStatusLock->Take(); + // Update WDT Signal (Direct or Long function Timered) + if(millis_standby) + { + // Check 1/2 Freq. controller ready to WDT only SET flag + if((millis_standby) < WDT_CONTROLLER_MS / 2) { + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::set; + } else { + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::timer; + // Add security milimal Freq to check + param.system_status->tasks[LOCAL_TASK_ID].watch_dog_ms = millis_standby + WDT_CONTROLLER_MS; + } + } + else + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::set; + param.systemStatusLock->Give(); +} + +/// @brief local suspend flag and positor running state Task (optional) +/// @param state_position Sw_Position (Local STATE) +/// @param state_subposition Sw_SubPosition (Optional Local SUB_STATE Position Monitor) +/// @param state_operation operative mode flag status for this task +void CanTask::TaskState(uint8_t state_position, uint8_t state_subposition, task_flag state_operation) +{ + // Local TaskWatchDog update + param.systemStatusLock->Take(); + // Signal Task sleep/disabled mode from request (Auto SET WDT on Resume) + if((param.system_status->tasks[LOCAL_TASK_ID].state == task_flag::suspended)&& + (state_operation==task_flag::normal)) + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::set; + param.system_status->tasks[LOCAL_TASK_ID].state = state_operation; + param.system_status->tasks[LOCAL_TASK_ID].running_pos = state_position; + param.system_status->tasks[LOCAL_TASK_ID].running_sub = state_subposition; + param.systemStatusLock->Give(); +} + +/// @brief RUN Task +void CanTask::Run() { + + // Data Local Task (Class + Registro) + // Avvia l'istanza alla classe State_Canard ed inizializza Ram, Buffer e variabili base + canardClass clCanard; + static uavcan_register_Value_1_0 val = {0}; + + // System message data queue structured + system_message_t system_message; + + // LoopTimer Publish + CanardMicrosecond last_pub_rmap_data; + CanardMicrosecond last_pub_heartbeat; + CanardMicrosecond last_pub_port_list; + + // Set when Firmware Upgrade is required + bool start_firmware_upgrade = false; + + // OnlineMaster (Set/Reset Application Code Function Here Enter/Exit Function OnLine) + bool masterOnline = false; + + // Start Running Monitor and First WDT normal state + #if (ENABLE_STACK_USAGE) + TaskMonitorStack(); + #endif + TaskState(state, UNUSED_SUB_POSITION, task_flag::normal); + + // Main Loop TASK + while (true) { + + // ********* SYSTEM QUEUE MESSAGE *********** + // enqueud system message from caller task + if (!param.systemMessageQueue->IsEmpty()) { + // Read queue in test mode + if (param.systemMessageQueue->Peek(&system_message, 0)) + { + // Its request addressed into ALL TASK... -> no pull (only SUPERVISOR or exernal gestor) + if(system_message.task_dest == ALL_TASK_ID) + { + // Pull && elaborate command, + if(system_message.command.do_sleep) + { + // Enter module sleep procedure (OFF Module) + HW_CAN_Power(CAN_ModePower::CAN_SLEEP); + // Enter sleep module OK and update WDT + TaskWatchDog(CAN_TASK_SLEEP_DELAY_MS); + TaskState(state, UNUSED_SUB_POSITION, task_flag::sleepy); + Delay(Ticks::MsToTicks(CAN_TASK_SLEEP_DELAY_MS)); + TaskState(state, UNUSED_SUB_POSITION, task_flag::normal); + // Restore module from Sleep + HW_CAN_Power(CAN_ModePower::CAN_NORMAL); + } + } + } + } + + // ******************************************************************************** + // SETUP CONFIG CYPAL, CLASS, REGISTER, DATA + // ******************************************************************************** + switch (state) { + // Setup Class CB and NodeId + case CAN_STATE_INIT: + + // Avvio inizializzazione (Standard UAVCAN MSG). Reset su INIT END OK + // Segnale al Master necessità di impostazioni ev. parametri, Data/Ora ecc.. + clCanard.flag.set_local_node_mode(uavcan_node_Mode_1_0_INITIALIZATION); + + // Attiva il callBack su RX Messaggio Canard sulla funzione interna processReceivedTransfer + clCanard.setReceiveMessage_CB(processReceivedTransfer, (void *) ¶m); + + // ******************** Lettura Registri standard UAVCAN ******************** + // Restore the node-ID from the corresponding standard regioster. Default to anonymous. + #ifdef USE_NODE_SLAVE_ID_FIXED + // Canard Slave NODE ID Fixed dal defined value in module_config + clCanard.set_canard_node_id((CanardNodeID)NODE_SLAVE_ID); + #else + uavcan_register_Value_1_0_select_natural16_(&val); + val.natural16.value.count = 1; + val.natural16.value.elements[0] = UINT16_MAX; // This means undefined (anonymous), per Specification/libcanard. + localRegisterAccessLock->Take(); + localRegister->read(REGISTER_UAVCAN_NODE_ID, &val); // The names of the standard registers are regulated by the Specification. + localRegisterAccessLock->Give(); + LOCAL_ASSERT(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == 1)); + if (val.natural16.value.elements[0] <= CANARD_NODE_ID_MAX) { + clCanard.set_canard_node_id((CanardNodeID)val.natural16.value.elements[0]); + } + #endif + + // The description register is optional but recommended because it helps constructing/maintaining large networks. + // It simply keeps a human-readable description of the node that should be empty by default. + uavcan_register_Value_1_0_select_string_(&val); + val._string.value.count = 0; + localRegisterAccessLock->Take(); + localRegister->read(REGISTER_UAVCAN_NODE_DESCR, &val); // We don't need the value, we just need to ensure it exists. + localRegisterAccessLock->Give(); + + // ******************** Lettura Registri personalizzati RMAP ******************** + // Restore the master-ID from the corresponding standard register -> Default to anonymous. + #ifdef USE_NODE_MASTER_ID_FIXED + // Canard Slave NODE ID Fixed dal defined value in module_config + clCanard.set_canard_master_id((CanardNodeID)NODE_MASTER_ID); + #else + uavcan_register_Value_1_0_select_natural16_(&val); + val.natural16.value.count = 1; + val.natural16.value.elements[0] = NODE_MASTER_ID; // Setting default Master ID Value + localRegisterAccessLock->Take(); + localRegister->read(REGISTER_RMAP_MASTER_ID, &val); // The names of the standard registers are regulated by the Specification. + localRegisterAccessLock->Give(); + LOCAL_ASSERT(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == 1)); + if (val.natural16.value.elements[0] <= CANARD_NODE_ID_MAX) { + clCanard.set_canard_master_id((CanardNodeID)val.natural16.value.elements[0]); + } + #endif + + // Carico i/il port-ID/subject-ID del modulo locale dai registri relativi associati nel namespace UAVCAN + #ifdef USE_PORT_PUBLISH_RMAP_FIXED + clCanard.port_id.publisher_module_leaf = (CanardPortID)SUBJECTID_PUBLISH_RMAP; + #else + clCanard.port_id.publisher_module_leaf = + getModeAccessID(canardClass::Introspection_Port::PublisherSubjectID, + REGISTER_DATA_PUBLISH, rmap_module_Leaf_1_0_FULL_NAME_AND_VERSION_); + #endif + #ifdef USE_PORT_SERVICE_RMAP_FIXED + clCanard.port_id.service_module_leaf = (CanardPortID)PORT_SERVICE_RMAP; + #else + clCanard.port_id.service_module_leaf = + getModeAccessID(canardClass::Introspection_Port::ServicePortID, + REGISTER_DATA_SERVICE, rmap_service_module_Leaf_1_0_FULL_NAME_AND_VERSION_); + #endif + + // Set configured Module Ready (IF service_id is valid) + // If Not Master receive flag module not ready and start configuration aumatically + if(clCanard.port_id.service_module_leaf != UINT16_MAX) + clCanard.flag.set_local_module_ready(true); + + // ************************* LETTURA REGISTRI METADATI RMAP **************************** + // POSITION ARRAY METADATA CONFIG: (TOT ELEMENTS = SENSOR_METADATA_COUNT) + // SENSOR_METADATA_BFT (0) + // *********************************** L1 ********************************************* + uavcan_register_Value_1_0_select_natural16_(&val); + val.natural16.value.count = SENSOR_METADATA_COUNT; + for(uint8_t id=0; idTake(); + localRegister->read(REGISTER_METADATA_LEVEL_L1, &val); + localRegisterAccessLock->Give(); + LOCAL_ASSERT(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == SENSOR_METADATA_COUNT)); + clCanard.module_leaf.BFT.metadata.level.L1.value = val.natural16.value.elements[SENSOR_METADATA_BFT]; + // *********************************** L2 ********************************************* + uavcan_register_Value_1_0_select_natural16_(&val); + val.natural16.value.count = SENSOR_METADATA_COUNT; + for(uint8_t id=0; idTake(); + localRegister->read(REGISTER_METADATA_LEVEL_L2, &val); + localRegisterAccessLock->Give(); + LOCAL_ASSERT(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == SENSOR_METADATA_COUNT)); + clCanard.module_leaf.BFT.metadata.level.L2.value = val.natural16.value.elements[SENSOR_METADATA_BFT]; + // ******************************* LevelType1 ***************************************** + uavcan_register_Value_1_0_select_natural16_(&val); + val.natural16.value.count = SENSOR_METADATA_COUNT; + for(uint8_t id=0; idTake(); + localRegister->read(REGISTER_METADATA_LEVEL_TYPE1, &val); + localRegisterAccessLock->Give(); + LOCAL_ASSERT(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == SENSOR_METADATA_COUNT)); + clCanard.module_leaf.BFT.metadata.level.LevelType1.value = val.natural16.value.elements[SENSOR_METADATA_BFT]; + // ******************************* LevelType2 ***************************************** + uavcan_register_Value_1_0_select_natural16_(&val); + val.natural16.value.count = SENSOR_METADATA_COUNT; + for(uint8_t id=0; idTake(); + localRegister->read(REGISTER_METADATA_LEVEL_TYPE2, &val); + localRegisterAccessLock->Give(); + LOCAL_ASSERT(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == SENSOR_METADATA_COUNT)); + clCanard.module_leaf.BFT.metadata.level.LevelType2.value = val.natural16.value.elements[SENSOR_METADATA_BFT]; + // *********************************** P1 ********************************************* + uavcan_register_Value_1_0_select_natural16_(&val); + val.natural16.value.count = SENSOR_METADATA_COUNT; + for(uint8_t id=0; idTake(); + localRegister->read(REGISTER_METADATA_TIME_P1, &val); + localRegisterAccessLock->Give(); + LOCAL_ASSERT(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == SENSOR_METADATA_COUNT)); + clCanard.module_leaf.BFT.metadata.timerange.P1.value = val.natural16.value.elements[SENSOR_METADATA_BFT]; + // *********************************** P2 ********************************************* + // P2 Non memorizzato sul modulo, parametro dipendente dall'acquisizione locale + clCanard.module_leaf.BFT.metadata.timerange.P2 = 0; + // *********************************** P2 ********************************************* + uavcan_register_Value_1_0_select_natural8_(&val); + val.natural8.value.count = SENSOR_METADATA_COUNT; + // Default are single different value for type sensor + val.natural8.value.elements[SENSOR_METADATA_BFT] = SENSOR_METADATA_LEVEL_P_IND_BFT; + localRegisterAccessLock->Take(); + localRegister->read(REGISTER_METADATA_TIME_PIND, &val); + localRegisterAccessLock->Give(); + LOCAL_ASSERT(uavcan_register_Value_1_0_is_natural8_(&val) && (val.natural8.value.count == SENSOR_METADATA_COUNT)); + clCanard.module_leaf.BFT.metadata.timerange.Pindicator.value = val.natural8.value.elements[SENSOR_METADATA_BFT]; + + // Passa alle sottoscrizioni + state = CAN_STATE_SETUP; + break; + + // ******************************************************************************** + // AVVIA SOTTOSCRIZIONI ai messaggi per servizi RPC ecc... + // ******************************************************************************** + case CAN_STATE_SETUP: + + // Plug and Play Versione 1_0 CAN_CLASSIC senza nodo ID valido + if (clCanard.is_canard_node_anonymous()) { + // PnP over Classic CAN, use message v1.0 + if (!clCanard.rxSubscribe(CanardTransferKindMessage, + uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, + uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC)) { + LOCAL_ASSERT(false); + } + } + + // Service Client: -> Verifica della presenza Heartbeat del MASTER [Networks OffLine] + if (!clCanard.rxSubscribe(CanardTransferKindMessage, + uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_, + uavcan_node_Heartbeat_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC)) { + LOCAL_ASSERT(false); + } + + // Service Client: -> Sincronizzazione timestamp Microsecond del MASTER [su base time local] + if (!clCanard.rxSubscribe(CanardTransferKindMessage, + uavcan_time_Synchronization_1_0_FIXED_PORT_ID_, + uavcan_time_Synchronization_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC)) { + LOCAL_ASSERT(false); + } + + // Service servers: -> Risposta per GetNodeInfo richiesta esterna master (Yakut, Altri) + if (!clCanard.rxSubscribe(CanardTransferKindRequest, + uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, + uavcan_node_GetInfo_Request_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC)) { + LOCAL_ASSERT(false); + } + + // Service servers: -> Risposta per ExecuteCommand richiesta esterna master (Yakut, Altri) + if (!clCanard.rxSubscribe(CanardTransferKindRequest, + uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_, + uavcan_node_ExecuteCommand_Request_1_1_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC)) { + LOCAL_ASSERT(false); + } + + // Service servers: -> Risposta per Accesso ai registri richiesta esterna master (Yakut, Altri) + if (!clCanard.rxSubscribe(CanardTransferKindRequest, + uavcan_register_Access_1_0_FIXED_PORT_ID_, + uavcan_register_Access_Request_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC)) { + LOCAL_ASSERT(false); + } + + // Service servers: -> Risposta per Lista dei registri richiesta esterna master (Yakut, Altri) + // Time OUT Canard raddoppiato per elenco registri (Con molte Call vado in TimOut) + // Con raddoppio del tempo Default problema risolto + if (!clCanard.rxSubscribe(CanardTransferKindRequest, + uavcan_register_List_1_0_FIXED_PORT_ID_, + uavcan_register_List_Request_1_0_EXTENT_BYTES_, + CANARD_REGISTERLIST_TRANSFER_ID_TIMEOUT_USEC)) { + LOCAL_ASSERT(false); + } + + // Service servers: -> Risposta per dati e metadati sensore modulo corrente da master (Yakut, Altri) + if (!clCanard.rxSubscribe(CanardTransferKindRequest, + clCanard.port_id.service_module_leaf, + rmap_service_module_Leaf_Request_1_0_EXTENT_BYTES_, + CANARD_RMAPDATA_TRANSFER_ID_TIMEOUT_USEC)) { + LOCAL_ASSERT(false); + } + + // Service client: -> Risposta per Read (Receive) File local richiesta esterna (Yakut, Altri) + if (!clCanard.rxSubscribe(CanardTransferKindResponse, + uavcan_file_Read_1_1_FIXED_PORT_ID_, + uavcan_file_Read_Response_1_1_EXTENT_BYTES_, + CANARD_READFILE_TRANSFER_ID_TIMEOUT_USEC)) { + LOCAL_ASSERT(false); + } + + // Avvio il modulo UAVCAN in modalità operazionale normale + // Eventuale SET Flag dopo acquisizione di configurazioni e/o parametri da Remoto + clCanard.flag.set_local_node_mode(uavcan_node_Mode_1_0_OPERATIONAL); + + // Set PNP pseudo Random Generator on enabled PinMapped Analog Input + randomSeed(analogRead(PA_4)); + + // Set START Timetable LOOP RX/TX. Set Canard microsecond start, per le sincronizzazioni + clCanard.getMicros(clCanard.start_syncronization); + last_pub_rmap_data = clCanard.getMicros(clCanard.syncronized_time); + last_pub_heartbeat = clCanard.getMicros(clCanard.syncronized_time); + last_pub_port_list = last_pub_heartbeat + MEGA * (0.1); + + // Passo alla gestione Main + state = CAN_STATE_CHECK; + break; + + // ******************************************************************************** + // AVVIA LOOP CANARD PRINCIPALE gestione TX/RX Code -> Messaggi + // ******************************************************************************** + case CAN_STATE_CHECK: + + // Set Canard microsecond corrente monotonic, per le attività temporanee di ciclo + clCanard.getMicros(clCanard.start_syncronization); + + // TEST VERIFICA sincronizzazione time_stamp locale con remoto... (LED/OUT sincronizzati) + // Test con reboot e successiva risincronizzazione automatica (FAKE RTC!!!!) + #if defined(LED_ON_SYNCRO_TIME) && defined(LED2_PIN) + // Utilizzo di RTC o locale o generato dal tickMicros locale a partire dall' ultimo SetTimeStamp + // E' utilizzabile come RTC_FAKE e/o come Setup Perfetto per regolazione RTC al cambio del secondo + // RTC Infatti non permette la regolazione dei microsecondi, e questa tecnica lo consente + // Verifico LED al secondo... su timeSTamp sincronizzato remoto + if((clCanard.master.timestamp.get_timestamp_syncronized() / MEGA) % 2) { + digitalWrite(LED2_PIN, HIGH); + } else { + digitalWrite(LED2_PIN, LOW); + } + #endif + + // ************************************************************************ + // *********** CHECK OFFLINE/ONLINE *********** + // ************************************************************************ + + // Check eventuale Nodo Master OFFLINE (Ultimo comando sempre perchè posso) + // Effettuare eventuali operazioni di SET,RESET Cmd in sicurezza + // Entro in OffLine ed eseguo eventuali operazioni di entrata + if (clCanard.master.heartbeat.is_online(false)) { + // Solo quando passo da OffLine ad OnLine controllo con VarLocale + if (masterOnline != true) { + TRACE_INFO_F(F("Master controller ONLINE !!! -> OnLineFunction()\r\n")); + // .... Codice OnLineFunction() + } + masterOnline = true; + // ************************************************************************** + // STATO MODULO (Decisionale in funzione di stato remoto) + // Gestione continuativa del modulo di acquisizione master.clCanard (flag remoto) + // ************************************************************************** + // Eventuale codice di gestione MasterOnline OK... + } else { + // Solo quando passo da OffLine ad OnLine + if (masterOnline) { + TRACE_INFO_F(F("Master controller OFFLINE !!! ALERT -> OffLineFunction()\r\n")); + // When enter off_line Master. Full power are automatic resetted for slave module + clCanard.flag.set_local_power_mode(Power_Mode::pwr_on); + } + masterOnline = false; + // Gestisco situazione con Master OFFLine... + } + + // ************************************************************************** + // *********************** FILE UPLOAD PROCESS HANDLER ********************** + // ************************************************************************** + + // Verifica file download in corso (entro se in download) + // Attivato da ricezione comando appropriato rxFile o rxFirmware + if(clCanard.master.file.download_request()) { + if(clCanard.master.file.is_firmware()) + // Set Flag locale per comunicazione HeartBeat... uploading OK in corso + // Utilizzo locale per blocco procedure, sleep ecc. x Uploading + clCanard.flag.set_local_fw_uploading(true); + // Controllo eventuale timeOut del comando o RxBlocco e gestisco le Retry + // Verifica TimeOUT Occurs per File download + if(clCanard.master.file.event_timeout()) { + TRACE_ERROR_F(F("Time OUT File... event occurs\r\n")); + // Gestione Retry previste dal comando per poi abbandonare + uint8_t retry; // In overload x LOGGING + if(clCanard.master.file.next_retry(&retry)) { + TRACE_VERBOSE_F(F("Next Retry File read: %u\r\n"), retry); + } else { + TRACE_VERBOSE_F(F("MAX Retry File occurs, download file ABORT !!!\r\n")); + clCanard.master.file.download_end(); + } + } + // Se messaggio in pending non faccio niente è attendo la fine del comando in run + // In caso di errore subentrerà il TimeOut e verrà essere gestita la retry + if(!clCanard.master.file.is_pending()) { + // Fine pending, con messaggio OK. Verifico se EOF o necessario altro blocco + if(clCanard.master.file.is_download_complete()) { + if(clCanard.master.file.is_firmware()) { + TRACE_INFO_F(F("RX FIRMWARE COMPLETED !!!\r\n")); + } else { + TRACE_INFO_F(F("RX FILE COMPLETED !!!\r\n")); + } + TRACE_VERBOSE_F(F("File name: %s\r\n"), clCanard.master.file.get_name()); + // GetInfo && Verify Start Updating... + if(clCanard.master.file.is_firmware()) { + // Module type also checked before startin firmware_upgrade + uint8_t module_type; + uint8_t version; + uint8_t revision; + uint64_t fwFileLen = 0; + getFlashFwInfoFile(&module_type, &version, &revision, &fwFileLen); + TRACE_VERBOSE_F(F("Firmware V%d.%d, Size: %lu bytes is ready for flash updating\r\n"),version, revision, (uint32_t) fwFileLen); + } // Nessun altro evento necessario, chiudo File e stati + // procedo all'aggiornamento Firmware dopo le verifiche di conformità + // Ovviamente se si tratta di un file firmware + clCanard.master.file.download_end(); + // Comunico a HeartBeat (Yakut o Altri) l'avvio dell'aggiornamento (se il file è un firmware...) + // Per Yakut Pubblicare un HeartBeat prima dell'Avvio quindi con il flag + // clCanard.local_node.file.updating_run = true >> HeartBeat Comunica Upgrade... + if(clCanard.master.file.is_firmware()) { + clCanard.flag.set_local_node_mode(uavcan_node_Mode_1_0_SOFTWARE_UPDATE); + start_firmware_upgrade = true; + // Preparo la struttua per informare il Boot Loader + param.boot_request->app_executed_ok = false; + param.boot_request->backup_executed = false; + param.boot_request->app_forcing_start = false; + param.boot_request->rollback_executed = false; + param.boot_request->request_upload = true; + param.eeprom->Write(BOOT_LOADER_STRUCT_ADDR, (uint8_t*) param.boot_request, sizeof(bootloader_t)); + } + // Il Firmware Upload dovrà partire necessariamente almeno dopo l'invio completo + // di HeartBeat (svuotamento coda), quindi via subito in heart_beat send + // Counque non rispondo più ai comandi di update con file.updating_run = true + } else { + // Avvio prima request o nuovo blocco (Set Flag e TimeOut) + // Prima request (clCanard.local_node.file.offset == 0) + // Firmmware Posizione blocco gestito automaticamente in sequenza Request/Read + // Gestione retry (incremento su TimeOut/Error) Automatico in Init/Request-Response + // Esco se raggiunga un massimo numero di retry x Frame... sopra + // Get Data Block per popolare il File + // Se il Buffer è pieno = 256 Caratteri è necessario continuare + // Altrimenti se inferiore o (0 Compreso) il trasferimento file termina. + // Se = 0 il blocco finale non viene considerato ma serve per il protocollo + // Se l'ultimo buffer dovesse essere pieno discrimina l'eventualità di MaxBuf==Eof + clCanard.master_file_read_block_pending(NODE_GETFILE_TIMEOUT_US); + } + } + } + // ************************************************************************** + + // -> Scheduler temporizzato dei messaggi standard da inviare alla rete UAVCAN + + // *************************** RMAP DATA PUBLISHER ************************** + // Pubblica i dati del nodo corrente se configurata e abilitata la funzione + // Ovviamente il nodo non può essere anonimo per la pubblicazione... + if ((!clCanard.is_canard_node_anonymous()) && + (clCanard.publisher_enabled.module_leaf) && + (clCanard.port_id.publisher_module_leaf <= CANARD_SUBJECT_ID_MAX)) { + if (clCanard.getMicros(clCanard.syncronized_time) >= last_pub_rmap_data) + { + // Update publisher + last_pub_rmap_data += MEGA * TIME_PUBLISH_MODULE_DATA; + // Funzione locale privata + publish_rmap_data(clCanard, ¶m); + } + } + + // ************************* HEARTBEAT DATA PUBLISHER *********************** + if((start_firmware_upgrade)|| + (clCanard.getMicros(clCanard.syncronized_time) >= last_pub_heartbeat)) { + if(clCanard.is_canard_node_anonymous()) { + TRACE_INFO_F(F("Publish SLAVE PNP Request Message -->> [ Random over %u sec ]\r\n"), TIME_PUBLISH_PNP_REQUEST); + clCanard.slave_pnp_send_request(param.configuration->serial_number); + last_pub_heartbeat += random(MEGA * TIME_PUBLISH_PNP_REQUEST); + } else { + TRACE_INFO_F(F("Publish SLAVE Heartbeat -->> [ %u sec]\r\n"), TIME_PUBLISH_HEARTBEAT); + clCanard.slave_heartbeat_send_message(); + // Update publisher when real syncro onLine Master (Synch with master publish HeartBeat) + if(clCanard.master.heartbeat.is_online(true)) + last_pub_heartbeat = clCanard.master.heartbeat.last_online() + MEGA * TIME_PUBLISH_HEARTBEAT; + else + last_pub_heartbeat = clCanard.getMicros(clCanard.syncronized_time) + MEGA * TIME_PUBLISH_HEARTBEAT; + } + } + + // ********************** SERVICE PORT LIST PUBLISHER *********************** + if (clCanard.flag.get_local_power_mode() == Power_Mode::pwr_on) { + if (clCanard.getMicros(clCanard.syncronized_time) >= last_pub_port_list) { + // Publish list service only if full power mode are selected + TRACE_INFO_F(F("Publish Local PORT LIST -->> [ %u sec]\r\n"), TIME_PUBLISH_PORT_LIST); + last_pub_port_list = clCanard.getMicros(clCanard.syncronized_time) + MEGA * TIME_PUBLISH_PORT_LIST; + // Update publisher + clCanard.slave_servicelist_send_message(); + } + } else { + // Mantengo il publish Port avanti nel tempo e Avvio solo a MAX Power e tempo + // di pubblicazione interamente trascorso. Evita Publish inutile in Request PowerUP + // da Master, per semplici operazioni di lettura e/o configurazione ( In Power::Sleep ) + last_pub_port_list = clCanard.getMicros(clCanard.syncronized_time); + } + + // *************************************************************************** + // Gestione Coda messaggi in trasmissione (ciclo di svuotamento messaggi) + // *************************************************************************** + // Transmit pending frames, avvia la trasmissione gestita da canard a priorità. + // Il metodo può essere chiamato direttamente in preparazione messaggio x coda + if (clCanard.transmitQueueDataPresent()) { + // Access driver con semaforo + if(param.canLock->Take(Ticks::MsToTicks(CAN_SEMAPHORE_MAX_WAITING_TIME_MS))) { + clCanard.transmitQueue(); + param.canLock->Give(); + } + } + + // *************************************************************************** + // Gestione Coda messaggi in ricezione (ciclo di caricamento messaggi) + // *************************************************************************** + // Gestione con Intererupt RX Only esterna (verifica dati in coda gestionale) + if (clCanard.receiveQueueDataPresent()) { + // Log Packet + #ifdef LOG_RX_PACKET + char logMsg[50]; + clCanard.receiveQueue(logMsg); + TRACE_DEBUG_F(F("RX [ %s ]\r\n"), logMsg); + #else + clCanard.receiveQueue(); + #endif + } + + // Request Reboot (Firmware upgrade... Or Reset) + if (clCanard.flag.is_requested_system_restart() || (start_firmware_upgrade)) { + TRACE_INFO_F(F("Send signal to system Reset...\r\n")); + delay(250); // Waiting security queue empty send HB (Updating start...) + NVIC_SystemReset(); + } + break; + } + + #if (ENABLE_STACK_USAGE) + TaskMonitorStack(); + #endif + + // Local TaskWatchDog update; + TaskWatchDog(CAN_TASK_WAIT_DELAY_MS); + + // Run switch TASK CAN one STEP every... + // If File Uploading MIN TimeOut For Task for Increse Speed Transfer RATE + if(clCanard.master.file.download_request()) { + DelayUntil(Ticks::MsToTicks(CAN_TASK_WAIT_MAXSPEED_DELAY_MS)); + } + else + { + DelayUntil(Ticks::MsToTicks(CAN_TASK_WAIT_DELAY_MS)); + } + + } +} diff --git a/platformio/stima_v4/slave-leaf/src/tasks/elaborate_data_task.cpp b/platformio/stima_v4/slave-leaf/src/tasks/elaborate_data_task.cpp new file mode 100644 index 000000000..16da4028f --- /dev/null +++ b/platformio/stima_v4/slave-leaf/src/tasks/elaborate_data_task.cpp @@ -0,0 +1,429 @@ +/** + ****************************************************************************** + * @file elaborate_data_task.h + * @author Marco Baldinetti + * @author Moreno Gasperini + * @brief Elaborate data sensor to CAN header file + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#define TRACE_LEVEL ELABORATE_DATA_TASK_TRACE_LEVEL +#define LOCAL_TASK_ID ELABORATE_TASK_ID + +#include "tasks/elaborate_data_task.h" + +using namespace cpp_freertos; + +/// @brief Construct the Elaborate Data Task::ElaborateDataTask object +/// @param taskName name of the task +/// @param stackSize size of the stack +/// @param priority priority of the task +/// @param elaborateDataParam parameters for the task +ElaborateDataTask::ElaborateDataTask(const char *taskName, uint16_t stackSize, uint8_t priority, ElaborateDataParam_t elaborateDataParam) : Thread(taskName, stackSize, priority), param(elaborateDataParam) +{ + // Start WDT controller and TaskState Flags + TaskWatchDog(WDT_STARTING_TASK_MS); + TaskState(ELABORATE_DATA_CREATE, UNUSED_SUB_POSITION, task_flag::normal); + + state = ELABORATE_DATA_INIT; + Start(); +}; + +#if (ENABLE_STACK_USAGE) +/// @brief local stack Monitor (optional) +void ElaborateDataTask::TaskMonitorStack() +{ + uint16_t stackUsage = (uint16_t)uxTaskGetStackHighWaterMark( NULL ); + if((stackUsage) && (stackUsage < param.system_status->tasks[LOCAL_TASK_ID].stack)) { + param.systemStatusLock->Take(); + param.system_status->tasks[LOCAL_TASK_ID].stack = stackUsage; + param.systemStatusLock->Give(); + } +} +#endif + +/// @brief local watchDog and Sleep flag Task (optional) +/// @param millis_standby time in ms to perfor check of WDT. If longer than WDT Reset, WDT is temporanly suspend +void ElaborateDataTask::TaskWatchDog(uint32_t millis_standby) +{ + // Local TaskWatchDog update + param.systemStatusLock->Take(); + // Update WDT Signal (Direct or Long function Timered) + if(millis_standby) + { + // Check 1/2 Freq. controller ready to WDT only SET flag + if((millis_standby) < WDT_CONTROLLER_MS / 2) { + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::set; + } else { + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::timer; + // Add security milimal Freq to check + param.system_status->tasks[LOCAL_TASK_ID].watch_dog_ms = millis_standby + WDT_CONTROLLER_MS; + } + } + else + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::set; + param.systemStatusLock->Give(); +} + +/// @brief local suspend flag and positor running state Task (optional) +/// @param state_position Sw_Position (Local STATE) +/// @param state_subposition Sw_SubPosition (Optional Local SUB_STATE Position Monitor) +/// @param state_operation operative mode flag status for this task +void ElaborateDataTask::TaskState(uint8_t state_position, uint8_t state_subposition, task_flag state_operation) +{ + // Local TaskWatchDog update + param.systemStatusLock->Take(); + // Signal Task sleep/disabled mode from request (Auto SET WDT on Resume) + if((param.system_status->tasks[LOCAL_TASK_ID].state == task_flag::suspended)&& + (state_operation==task_flag::normal)) + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::set; + param.system_status->tasks[LOCAL_TASK_ID].state = state_operation; + param.system_status->tasks[LOCAL_TASK_ID].running_pos = state_position; + param.system_status->tasks[LOCAL_TASK_ID].running_sub = state_subposition; + param.systemStatusLock->Give(); +} + +/// @brief RUN Task +void ElaborateDataTask::Run() { + // Queue for data + elaborate_data_t edata; + request_data_t request_data; + // System message data queue structured + system_message_t system_message; + + bufferReset(&maintenance_samples, SAMPLES_COUNT_MAX); + bufferReset(&leaf_samples, SAMPLES_COUNT_MAX); + + // Start Running Monitor and First WDT normal state + #if (ENABLE_STACK_USAGE) + TaskMonitorStack(); + #endif + TaskState(state, UNUSED_SUB_POSITION, task_flag::normal); + + #if defined(USE_SIMULATOR) && defined(INIT_SIMULATOR) + for(uint16_t iInit=0; iInit<900; iInit++) { + edata.value = 500 + random(200); + TRACE_VERBOSE_F(F("Leaf: %d\r\n"), edata.value); + addValue(&maintenance_samples, SAMPLES_COUNT_MAX, false); + addValue(&leaf_samples, SAMPLES_COUNT_MAX, edata.value); + } + #endif + + while (true) { + + // ********* SYSTEM QUEUE MESSAGE *********** + // enqueud system message from caller task + if (!param.systemMessageQueue->IsEmpty()) { + // Read queue in test mode + if (param.systemMessageQueue->Peek(&system_message, 0)) + { + // Its request addressed into ALL TASK... -> no pull (only SUPERVISOR or exernal gestor) + if(system_message.task_dest == ALL_TASK_ID) + { + // Pull && elaborate command, + if(system_message.command.do_sleep) + { + // Enter sleep module OK and update WDT + TaskWatchDog(ELABORATE_TASK_SLEEP_DELAY_MS); + TaskState(state, UNUSED_SUB_POSITION, task_flag::sleepy); + Delay(Ticks::MsToTicks(ELABORATE_TASK_SLEEP_DELAY_MS)); + TaskState(state, UNUSED_SUB_POSITION, task_flag::normal); + } + } + } + } + + // enqueud from th sensors task (populate data) + if (!param.elaborateDataQueue->IsEmpty()) { + if (param.elaborateDataQueue->Peek(&edata, 0)) + { + param.elaborateDataQueue->Dequeue(&edata); + switch (edata.index) + { + case LEAF_INDEX: + // Data Simulator + #ifdef USE_SIMULATOR + edata.value = 500 + random(200); + #endif + TRACE_VERBOSE_F(F("Leaf: %d\r\n"), edata.value); + addValue(&maintenance_samples, SAMPLES_COUNT_MAX, param.system_status->flags.is_maintenance); + addValue(&leaf_samples, SAMPLES_COUNT_MAX, edata.value); + break; + } + } + } + + // enqueued from can task (get data, start command...) + if (!param.requestDataQueue->IsEmpty()) { + if (param.requestDataQueue->Peek(&request_data, 0)) + { + // send request to elaborate task (all data is present verified on elaborate_task) + param.requestDataQueue->Dequeue(&request_data); + make_report(request_data.is_init, request_data.report_time_s, request_data.observation_time_s); + param.reportDataQueue->Enqueue(&report); + } + } + + #if (ENABLE_STACK_USAGE) + TaskMonitorStack(); + #endif + + // Local TaskWatchDog update; + TaskWatchDog(ELABORATE_TASK_WAIT_DELAY_MS); + + DelayUntil(Ticks::MsToTicks(ELABORATE_TASK_WAIT_DELAY_MS)); + } +} + +/// @brief Check data in and perform calculate of Optional Quality value +/// @param leaf real value readed from sensor +/// @return value uint_8 percent data quality value +uint8_t ElaborateDataTask::checkLeaf(rmapdata_t leaf) { + // Optional check quality data function + uint8_t quality = 100; + return quality; +} + +/// @brief Create a report from buffered sample +/// @param is_init Bool optional backup ptr_wr calc +/// @param report_time_s time of report +/// @param observation_time_s time to make an observation +void ElaborateDataTask::make_report (bool is_init, uint16_t report_time_s, uint8_t observation_time_s) { + // Generic and shared var + bool measures_maintenance = false; // Maintenance mode? + float valid_data_calc_perc; // Shared calculate valid % of measure (Data Valid or not?) + bool is_observation = false; // Is an observation (when calculate is requested) + uint16_t n_sample = 0; // Sample elaboration number... (incremented on calc development) + + // Elaborate suffix description: _s(sample) _o(observation) _t(total) [Optional for debug] + + // Elaboration leaf + rmapdata_t leaf_s = 0; + float avg_leaf_s = 0; + float avg_leaf_o = 0; + float min_leaf_o = FLT_MAX; + float max_leaf_o = FLT_MIN; + float avg_leaf_quality_s = 0; + float avg_leaf_quality_o = 0; + #if TRACE_LEVEL >= TRACE_LEVEL_DEBUG + uint16_t valid_count_leaf_t = 0; + #endif + uint16_t valid_count_leaf_s = 0; + uint16_t total_count_leaf_s = 0; + uint16_t valid_count_leaf_o = 0; + + // Elaboration timings calculation + uint16_t report_sample_count = round((report_time_s * 1.0) / (param.configuration->sensor_acquisition_delay_ms / 1000.0)); + uint16_t observation_sample_count = round((observation_time_s * 1.0) / (param.configuration->sensor_acquisition_delay_ms / 1000.0)); + uint16_t report_observations_count = 0; + if(report_time_s && observation_sample_count) report_observations_count = report_sample_count / observation_sample_count; + + // Request to calculate is correct? Trace request + if (report_time_s == 0) + { + // Request an direct sample value for istant measure + TRACE_INFO_F(F("Elaborate: Requested an istant value\r\n")); + } + else + { + TRACE_INFO_F(F("Elaborate: Requested an report on %d seconds\r\n"), report_time_s); + TRACE_DEBUG_F(F("-> %d samples counts need for report\r\n"), report_sample_count); + TRACE_DEBUG_F(F("-> %d samples counts need for observation\r\n"), observation_sample_count); + TRACE_DEBUG_F(F("-> %d observation counts need for report\r\n"), report_observations_count); + TRACE_DEBUG_F(F("-> %d available leaf samples count\r\n"), leaf_samples.count); + } + + // Default value to RMAP Limit error value + report.avg = RMAPDATA_MAX; + report.quality = RMAPDATA_MAX; + + // Ptr for maintenance + bufferPtrResetBack(&maintenance_samples, SAMPLES_COUNT_MAX); + // Ptr for value sample + bufferPtrResetBack(&leaf_samples, SAMPLES_COUNT_MAX); + + // align all sensor's data to last common acquired sample + uint16_t samples_count = leaf_samples.count; + + // it's a simple istant or report request? + if (report_time_s == 0) + { + // Make last data value to Get Istant show value + report.avg = bufferReadBack(&leaf_samples, SAMPLES_COUNT_MAX); + } + else + { + for (uint16_t i = 0; i < samples_count; i++) + { + // End of Sample in calculation (Completed with the request... Exit on Full Buffer ReadBack executed) + if(n_sample >= report_sample_count) break; + // Get base operation for any record... + n_sample++; // Elaborate next sample... (Initzialize with 0, Sample 1 is first. Exit UP if buffer completed) + // Check if is an observation + is_observation = (n_sample % observation_sample_count) == 0; + // Is Maintenance mode? (Excluding measure from elaboration value) + // Maintenance is sytemic value for all measure (always pushed into module for excuding value with maintenance) + measures_maintenance = bufferReadBack(&maintenance_samples, SAMPLES_COUNT_MAX); + + // *************************************************************************************************** + // ************* GET SAMPLE VALUE DATA FROM AND CREATE OBSERVATION VALUES FOR TYPE SENSOR ************ + // *************************************************************************************************** + + leaf_s = bufferReadBack(&leaf_samples, SAMPLES_COUNT_MAX); + #if (USE_REDUNDANT_SENSOR) + redundant_temperature_s = bufferReadBack(&temperature_redundant_samples, SAMPLES_COUNT_MAX); + #endif + total_count_leaf_s++; + avg_leaf_quality_s += (float)(((float)checkLeaf(leaf_s) - avg_leaf_quality_s) / total_count_leaf_s); + if ((ISVALID_RMAPDATA(leaf_s)) && !measures_maintenance) + { + valid_count_leaf_s++; + #if TRACE_LEVEL >= TRACE_LEVEL_DEBUG + valid_count_leaf_t++; + #endif + avg_leaf_s += (float)(((float)leaf_s - avg_leaf_s) / valid_count_leaf_s); + } + + // *************************************************************************************************** + // ************* ELABORATE OBSERVATION VALUES FOR TYPE SENSOR FOR PREPARE REPORT RESPONSE ************ + // *************************************************************************************************** + if(is_observation) { + + // leaf, sufficient number of valid samples? + valid_data_calc_perc = (float)(valid_count_leaf_s) / (float)(total_count_leaf_s) * 100.0; + if (valid_data_calc_perc >= SAMPLE_ERROR_PERCENTAGE_MIN) + { + valid_count_leaf_o++; + avg_leaf_o += (avg_leaf_s - avg_leaf_o) / valid_count_leaf_o; + avg_leaf_quality_o += (avg_leaf_quality_s - avg_leaf_quality_o) / valid_count_leaf_o; + // Elaboration MIN and MAX for observation + if(avg_leaf_s < min_leaf_o) min_leaf_o = avg_leaf_s; + if(avg_leaf_s > max_leaf_o) max_leaf_o = avg_leaf_s; + } + // Reset Buffer sample for calculate next observation + avg_leaf_quality_s = 0; + avg_leaf_s = 0; + valid_count_leaf_s = 0; + total_count_leaf_s = 0; + } + } + + // *************************************************************************************************** + // ******* GENERATE REPORT RESPONSE WITH ALL DATA AVAIABLE AND VALID WITH EXPECETD OBSERVATION ******* + // *************************************************************************************************** + + // temperature, elaboration final + valid_data_calc_perc = (float)(valid_count_leaf_o) / (float)(report_observations_count) * 100.0; + TRACE_DEBUG_F(F("-> %d leaf sample error (%d%%)\r\n"), (n_sample - valid_count_leaf_t), (uint8_t)(((float)n_sample - (float)valid_count_leaf_t)/(float)n_sample * 100.0)); + TRACE_DEBUG_F(F("-> %d leaf observation avaiable (%d%%)\r\n"), valid_count_leaf_o, (uint8_t)valid_data_calc_perc); + if (valid_data_calc_perc >= OBSERVATION_ERROR_PERCENTAGE_MIN) + { + // report.temperature.ist (already assigned) + report.avg = (rmapdata_t)avg_leaf_o; + // report.min = (rmapdata_t)min_leaf_o; + // report.max = (rmapdata_t)max_leaf_o; + report.quality = (rmapdata_t)avg_leaf_quality_o; + } + + // Trace report final + TRACE_INFO_F(F("--> leaf report\t%d\t%d\r\n"), (rmapdata_t)report.avg, (rmapdata_t)report.quality); + } +} + +template +value_v bufferRead(buffer_g *buffer, length_v length) +{ + value_v value = *buffer->read_ptr; + + if (buffer->read_ptr == buffer->values+length-1) { + buffer->read_ptr = buffer->values; + } + else buffer->read_ptr++; + + return value; +} + +template +value_v bufferReadBack(buffer_g *buffer, length_v length) +{ + value_v value = *buffer->read_ptr; + + if (buffer->read_ptr == buffer->values) { + buffer->read_ptr = buffer->values+length-1; + } + else buffer->read_ptr--; + + return value; +} + +template +void bufferWrite(buffer_g *buffer, value_v value) +{ + *buffer->write_ptr = value; +} + +template +void bufferPtrReset(buffer_g *buffer) +{ + buffer->read_ptr = buffer->values; +} + +template +void bufferPtrResetBack(buffer_g *buffer, length_v length) +{ + if (buffer->write_ptr == buffer->values) + { + buffer->read_ptr = buffer->values+length-1; + } + else buffer->read_ptr = buffer->write_ptr-1; +} + +template +void incrementBuffer(buffer_g *buffer, length_v length) +{ + if (buffer->count < length) + { + buffer->count++; + } + + if (buffer->write_ptr+1 < buffer->values + length) { + buffer->write_ptr++; + } else buffer->write_ptr = buffer->values; +} + +template +void bufferReset(buffer_g *buffer, length_v length) +{ + memset(buffer->values, 0xFF, length * sizeof(value_v)); + buffer->count = 0; + buffer->read_ptr = buffer->values; + buffer->write_ptr = buffer->values; +} + +template +void addValue(buffer_g *buffer, length_v length, value_v value) +{ + *buffer->write_ptr = (value_v)value; + incrementBuffer(buffer, length); +} diff --git a/platformio/stima_v4/slave-leaf/src/tasks/leaf_sensor_task.cpp b/platformio/stima_v4/slave-leaf/src/tasks/leaf_sensor_task.cpp new file mode 100644 index 000000000..e2c1560c0 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/src/tasks/leaf_sensor_task.cpp @@ -0,0 +1,460 @@ +/** + ****************************************************************************** + * @file leaf_sensor_task.cpp + * @author Marco Baldinetti + * @author Moreno Gasperini + * @brief Module sensor source file + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#define TRACE_LEVEL LEAF_SENSOR_TASK_TRACE_LEVEL +#define LOCAL_TASK_ID SENSOR_TASK_ID + +#include "tasks/leaf_sensor_task.h" + +#if (MODULE_TYPE == STIMA_MODULE_TYPE_LEAF) + +using namespace cpp_freertos; + +/// @brief Constructor for the sensor Task +/// @param taskName name of the task +/// @param stackSize size of the stack +/// @param priority priority of the task +/// @param leafSensorParam local parameters for the task +LeafSensorTask::LeafSensorTask(const char *taskName, uint16_t stackSize, uint8_t priority, LeafSensorParam_t leafSensorParam) : Thread(taskName, stackSize, priority), param(leafSensorParam) +{ + // Start WDT controller and TaskState Flags + TaskWatchDog(WDT_STARTING_TASK_MS); + TaskState(SENSOR_STATE_CREATE, UNUSED_SUB_POSITION, task_flag::normal); + + // Set analog resolution + analogReadResolution(ADC_RESOLUTION); + + state = SENSOR_STATE_WAIT_CFG; + Start(); +}; + +#if (ENABLE_STACK_USAGE) +/// @brief local stack Monitor (optional) +void LeafSensorTask::TaskMonitorStack() +{ + uint16_t stackUsage = (uint16_t)uxTaskGetStackHighWaterMark( NULL ); + if((stackUsage) && (stackUsage < param.system_status->tasks[LOCAL_TASK_ID].stack)) { + param.systemStatusLock->Take(); + param.system_status->tasks[LOCAL_TASK_ID].stack = stackUsage; + param.systemStatusLock->Give(); + } +} +#endif + +/// @brief local watchDog and Sleep flag Task (optional) +/// @param millis_standby time in ms to perfor check of WDT. If longer than WDT Reset, WDT is temporanly suspend +void LeafSensorTask::TaskWatchDog(uint32_t millis_standby) +{ + // Local TaskWatchDog update + param.systemStatusLock->Take(); + // Update WDT Signal (Direct or Long function Timered) + if(millis_standby) + { + // Check 1/2 Freq. controller ready to WDT only SET flag + if((millis_standby) < WDT_CONTROLLER_MS / 2) { + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::set; + } else { + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::timer; + // Add security milimal Freq to check + param.system_status->tasks[LOCAL_TASK_ID].watch_dog_ms = millis_standby + WDT_CONTROLLER_MS; + } + } + else + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::set; + param.systemStatusLock->Give(); +} + +/// @brief local suspend flag and positor running state Task (optional) +/// @param state_position Sw_Position (Local STATE) +/// @param state_subposition Sw_SubPosition (Optional Local SUB_STATE Position Monitor) +/// @param state_operation operative mode flag status for this task +void LeafSensorTask::TaskState(uint8_t state_position, uint8_t state_subposition, task_flag state_operation) +{ + // Local TaskWatchDog update + param.systemStatusLock->Take(); + // Signal Task sleep/disabled mode from request (Auto SET WDT on Resume) + if((param.system_status->tasks[LOCAL_TASK_ID].state == task_flag::suspended)&& + (state_operation==task_flag::normal)) + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::set; + param.system_status->tasks[LOCAL_TASK_ID].state = state_operation; + param.system_status->tasks[LOCAL_TASK_ID].running_pos = state_position; + param.system_status->tasks[LOCAL_TASK_ID].running_sub = state_subposition; + param.systemStatusLock->Give(); +} + +/// @brief RUN Task +void LeafSensorTask::Run() { + rmapdata_t values_readed_from_sensor[VALUES_TO_READ_FROM_SENSOR_COUNT]; + elaborate_data_t edata; + // Request response for system queue Task controlled... + system_message_t system_message; + + bool is_adc_overflow; + bool is_adc_error; + uint8_t perc_error_adc; + uint8_t adc_channel; + float value; + + // Start Running Monitor and First WDT normal state + #if (ENABLE_STACK_USAGE) + TaskMonitorStack(); + #endif + TaskState(state, UNUSED_SUB_POSITION, task_flag::normal); + + // Using powered rest mode (comparator and optional sesnsor chanel power) + #if (LEAF_TASK_LOW_POWER_ENABLED) + powerOff(); + #else + digitalWrite(PIN_EN_5VA, HIGH); // Fixed powered Analog Comparator (xIAN) Get value + #endif + + // Fixed Chanel Powered used define (Sensor do not switching power, always on from define) + #if(LEAF_FIXED_POWER_CHANEL_0) + digitalWrite(PIN_OUT0, HIGH); // Enable relative fixed Output Chanel alim + #endif + #if(LEAF_FIXED_POWER_CHANEL_1) + digitalWrite(PIN_OUT1, HIGH); // Enable relative fixed Output Chanel alim + #endif + #if(LEAF_FIXED_POWER_CHANEL_2) + digitalWrite(PIN_OUT2, HIGH); // Enable relative fixed Output Chanel alim + #endif + #if(LEAF_FIXED_POWER_CHANEL_3) + digitalWrite(PIN_OUT3, HIGH); // Enable relative fixed Output Chanel alim + #endif + + while (true) + { + switch (state) + { + case SENSOR_STATE_WAIT_CFG: + // check if configuration is done loaded + if (param.system_status->flags.is_cfg_loaded) + { + // Select read chanel first from config + for(uint8_t idx=0; idxsensors[idx].is_active) { + // Select first active chanel selected (only one is used on this module) + adc_channel = idx; + break; + } + } + TRACE_VERBOSE_F(F("WAIT -> INIT\r\n")); + state = SENSOR_STATE_INIT; + } + // do something else with non-blocking wait .... + TaskWatchDog(LEAF_TASK_WAIT_DELAY_MS); + Delay(Ticks::MsToTicks(LEAF_TASK_WAIT_DELAY_MS)); + break; + + case SENSOR_STATE_INIT: + TRACE_INFO_F(F("Initializing sensors...\r\n")); + #if (LEAF_TASK_LOW_POWER_ENABLED) // Auto power chanel with switching mode and power time waiting... + powerOn(adc_channel); + #endif + resetADCData(adc_channel); + state = SENSOR_STATE_SET; + TRACE_INFO_F(F("SENSOR_STATE_INIT --> SENSOR_STATE_SET\r\n")); + break; + + case SENSOR_STATE_SET: + + // Add sample ADC value to data + if(addADCData(adc_channel) >= SAMPLES_REPETED_ADC) { + // Read real value + value = getADCData(adc_channel, &perc_error_adc); + // ?Error ADC reading < MIN_PERRCENT_READING_VALUE_OK + is_adc_error = (perc_error_adc < ADC_ERROR_PERCENTAGE_MIN); + state = SENSOR_STATE_EVALUATE; + break; + } + break; + + case SENSOR_STATE_EVALUATE: + + // With ADC Error nothing to do... + if(is_adc_error) { + // Error code data + TRACE_ERROR_F(F("Sensor analog: Error reading ADC\r\n")); + value = (float)UINT16_MAX; + } else { + // Gain - offset to ADC to real value, and connvert to scale used (mV for LEAF for each ADC Type method used) + value = getAdcCalibratedValue(value, param.configuration->sensors[adc_channel].adc_offset, param.configuration->sensors[adc_channel].adc_gain); + value = getAdcAnalogValue(value, param.configuration->sensors[adc_channel].adc_type); + if (param.configuration->sensors[adc_channel].adc_type == Adc_Mode::Volt) value *= 1000.0; + TRACE_DEBUG_F(F("Sensor analog value %d (mV)\r\n"), (uint16_t)round(value)); + // Read value into U.M. Real Leaf (Sample value) + value = getLeaf(value, param.configuration->sensors[adc_channel].analog_min, param.configuration->sensors[adc_channel].analog_max, &is_adc_overflow); + } + + // Inform system state if ADC error event ( reading measure % error < MIN_VALID_PERCENTAGE ) + if(param.system_status->events.is_adc_unit_error != (perc_error_adc < ADC_ERROR_PERCENTAGE_MIN)) { + param.systemStatusLock->Take(); + param.system_status->events.is_adc_unit_error = (perc_error_adc < ADC_ERROR_PERCENTAGE_MIN); + param.systemStatusLock->Give(); + } + // Inform system state if ADC oveflow analog sensor limits + if((param.system_status->events.is_adc_unit_overflow != is_adc_overflow) || + (param.system_status->events.is_adc_unit_error != is_adc_error)) { + param.systemStatusLock->Take(); + param.system_status->events.is_adc_unit_overflow = is_adc_overflow; + param.system_status->events.is_adc_unit_error = is_adc_error; + param.systemStatusLock->Give(); + } + + state = SENSOR_STATE_READ; + TRACE_INFO_F(F("SENSOR_STATE_EVALUATE --> SENSOR_STATE_READ\r\n")); + break; + + case SENSOR_STATE_READ: + edata.value = value; + edata.index = LEAF_INDEX; + + param.elaborateDataQueue->Enqueue(&edata, Ticks::MsToTicks(WAIT_QUEUE_REQUEST_PUSHDATA_MS)); + TRACE_INFO_F(F("SENSOR_STATE_READ --> SENSOR_STATE_END\r\n")); + state = SENSOR_STATE_END; + break; + + case SENSOR_STATE_END: + #if (LEAF_TASK_LOW_POWER_ENABLED) + powerOff(); + #endif + +#if (ENABLE_STACK_USAGE) + TaskMonitorStack(); +#endif + + // Local TaskWatchDog update and Sleep Activate before Next Read + TaskWatchDog(param.configuration->sensor_acquisition_delay_ms); + TaskState(state, UNUSED_SUB_POSITION, task_flag::sleepy); + DelayUntil(Ticks::MsToTicks(param.configuration->sensor_acquisition_delay_ms)); + TaskState(state, UNUSED_SUB_POSITION, task_flag::normal); + + state = SENSOR_STATE_INIT; + break; + } // End Of Switch + + } + +} + +/// @brief Power ON Method (rest power mode) +/// @param chanel_out chanel To be powered (Fixed or switching depending from define into header_file) +void LeafSensorTask::powerOn(uint8_t chanel_out) +{ + if (!is_power_on) { + digitalWrite(PIN_EN_5VA, HIGH); // Enable Analog Comparator (xIAN) + // Switching powered chanel mode + #if(LEAF_TASK_SWITCH_POWER_ENABLED) + if(chanel_out == 0) digitalWrite(PIN_OUT0, HIGH); // Enable relative Output Chanel alim + if(chanel_out == 1) digitalWrite(PIN_OUT1, HIGH); // Enable relative Output Chanel alim + if(chanel_out == 2) digitalWrite(PIN_OUT2, HIGH); // Enable relative Output Chanel alim + if(chanel_out == 3) digitalWrite(PIN_OUT3, HIGH); // Enable relative Output Chanel alim + #endif + // WDT + TaskWatchDog(LEAF_TASK_POWER_ON_WAIT_DELAY_MS); + Delay(Ticks::MsToTicks(LEAF_TASK_POWER_ON_WAIT_DELAY_MS)); + is_power_on = true; + } +} + +/// @brief Power OFF Method (rest power mode) +void LeafSensorTask::powerOff() +{ + digitalWrite(PIN_EN_5VA, LOW); // Disable Analog Comparator (xIAN) + #if(LEAF_TASK_SWITCH_POWER_ALIM_SENS) + digitalWrite(PIN_OUT0, LOW); // Disable Output Chanel alim + digitalWrite(PIN_OUT1, LOW); // Disable Output Chanel alim + digitalWrite(PIN_OUT2, LOW); // Disable Output Chanel alim + digitalWrite(PIN_OUT3, LOW); // Disable Output Chanel alim + #endif + is_power_on = false; +} + +/// @brief Add Data ADC to counter +/// @param chanel_out Chanel to be getted +uint8_t LeafSensorTask::addADCData(uint8_t chanel_out) +{ + int32_t analogReadVal; + adc_in_count[chanel_out]++; + switch(chanel_out) { + case 0: + analogReadVal = analogRead(PIN_ANALOG_01); + break; + case 1: + analogReadVal = analogRead(PIN_ANALOG_02); + break; + case 2: + analogReadVal = analogRead(PIN_ANALOG_03); + break; + case 3: + analogReadVal = analogRead(PIN_ANALOG_04); + break; + } + // Add data only if not an error (count error as uint 32 FFFFFFFF = -1, < 0) + if(analogReadVal >= 0) { + adc_in[chanel_out] += (uint32_t) analogReadVal; + } else { + adc_err_count[chanel_out]++; + } + return adc_in_count[chanel_out]; +} + +/// @brief Reset ADC Counter +/// @param chanel_out Chanel to be resetted value +void LeafSensorTask::resetADCData(uint8_t chanel_out) +{ + adc_in_count[chanel_out] = 0; + adc_err_count[chanel_out] = 0; + adc_in[chanel_out] = 0; +} + +/// @brief Get real ADC Data +/// @param chanel_out Chanel to be resetted value +/// @param quality_data Return quality perc of good measure in current cycle reading +/// @return ADC Value (float conversion) +float LeafSensorTask::getADCData(uint8_t chanel_out, uint8_t *quality_data) +{ + *quality_data = 0; + // All measure are error? + if(adc_in_count[chanel_out] == adc_err_count[chanel_out]) return 0; + // Get quality of repeted measure + if(adc_err_count[chanel_out] == 0) { + *quality_data = 100; + } else { + *quality_data = (uint8_t) (100.0 - (((float)adc_err_count[chanel_out] / (float)adc_in_count[chanel_out]) * 100.0)); + } + // Return measure, eliminating the wrong measurements + return adc_in[chanel_out] / (adc_in_count[chanel_out] - adc_err_count[chanel_out]); +} + +/// @brief Get System VREF internal temperature +/// @return ambient external CPU temperature value +int32_t LeafSensorTask::getVrefTemp(void) +{ + int32_t vRefVoltage = (__LL_ADC_CALC_VREFANALOG_VOLTAGE(analogRead(AVREF), LL_ADC_RESOLUTION)); + return (__LL_ADC_CALC_TEMPERATURE(vRefVoltage, analogRead(ATEMP), LL_ADC_RESOLUTION)); +} + +/// @brief Get ADC value with param optional calibration +/// @param adc_value adc in value +/// @param offset offset adjust value +/// @param gain gain adjust value +/// @return ADC value calibrated and checked with input area validation limit +float LeafSensorTask::getAdcCalibratedValue(float adc_value, float offset, float gain) +{ + float value = (float)UINT16_MAX; + + if (!isnan(adc_value) && (adc_value >= ADC_MIN) && (adc_value <= ADC_MAX)) + { + value = adc_value; + value += offset; + value *= gain; + if(value <= ADC_MIN) value = ADC_MIN; + if(value >= ADC_MAX) value = ADC_MAX; + } + + return value; +} + +/// @brief Get analog value converted from ADC with type input selected +/// @param adc_value value read from adc +/// @param adc_type ADC_Mode as type of input value selected type +/// @return ADC value to real analog value +float LeafSensorTask::getAdcAnalogValue(float adc_value, Adc_Mode adc_type) +{ + float min, max; + float value = (float)UINT16_MAX; + + switch (adc_type) + { + case Adc_Mode::mVolt: + min = (float)(ADC_VOLTAGE_MIN_MV); + max = (float)(ADC_VOLTAGE_MAX_MV); + /* code */ + break; + + case Adc_Mode::Volt: + min = (float)(ADC_VOLTAGE_MIN_V); + max = (float)(ADC_VOLTAGE_MAX_V); + /* code */ + break; + + case Adc_Mode::mA: + min = (float)(ADC_CURRENT_MIN_MA); + max = (float)(ADC_CURRENT_MAX_MA); + /* code */ + break; + + default: + break; + } + + if (!isnan(adc_value)) + { + value = adc_value; + value *= (((max - min) / (float)(ADC_MAX))); + value += min; + } + + return value; +} + +/// @brief Get real data Leaf +/// @param adc_value in adc value calibrated +/// @param adc_voltage_min VMIN Ref range of sensor +/// @param adc_voltage_max VMAX Ref range of sensor +/// @param adc_overflow bool if ADC get an overflow range value (setted to true) +/// @return Real scaled data Leaf from analog adc calibrated value +float LeafSensorTask::getLeaf(float adc_value, float adc_voltage_min, float adc_voltage_max, bool *adc_overflow) +{ + float value = adc_value; + *adc_overflow = false; + + if ((value < (adc_voltage_min + LEAF_ERROR_VOLTAGE_MIN)) || (value > (adc_voltage_max + LEAF_ERROR_VOLTAGE_MAX))) + { + *adc_overflow = true; + value = UINT16_MAX; + } + else + { + value = ((value - adc_voltage_min) / (adc_voltage_max - adc_voltage_min) * LEAF_MAX); + + if (value <= LEAF_ERROR_MIN) value = LEAF_MIN; + if (value >= LEAF_ERROR_MAX) value = LEAF_MAX; + } + + if((value >= LEAF_MIN)&&(value <= LEAF_SENSIBILITY)) { + value = LEAF_MIN; + } + + return round(value); +} + +#endif diff --git a/platformio/stima_v4/slave-leaf/src/tasks/supervisor_task.cpp b/platformio/stima_v4/slave-leaf/src/tasks/supervisor_task.cpp new file mode 100644 index 000000000..35695c4cc --- /dev/null +++ b/platformio/stima_v4/slave-leaf/src/tasks/supervisor_task.cpp @@ -0,0 +1,603 @@ +/**@file supervisor_task.cpp */ + +/********************************************************************* +

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+authors: +Marco Baldinetti + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +. +**********************************************************************/ + +#define TRACE_LEVEL SUPERVISOR_TASK_TRACE_LEVEL +#define LOCAL_TASK_ID SUPERVISOR_TASK_ID + +#include "tasks/supervisor_task.h" + +using namespace cpp_freertos; + +/// @brief Construct the Supervisor Task::SupervisorTask object +/// @param taskName name of the task +/// @param stackSize size of the stack +/// @param priority priority of the task +/// @param supervisorParam parameters for the task +SupervisorTask::SupervisorTask(const char *taskName, uint16_t stackSize, uint8_t priority, SupervisorParam_t supervisorParam) : Thread(taskName, stackSize, priority), param(supervisorParam) +{ + // Start WDT controller and TaskState Flags + TaskWatchDog(WDT_STARTING_TASK_MS); + TaskState(SUPERVISOR_STATE_CREATE, UNUSED_SUB_POSITION, task_flag::normal); + + // Immediate load Config for all Task info + loadConfiguration(); + + state = SUPERVISOR_STATE_INIT; + Start(); +}; + +#if (ENABLE_STACK_USAGE) +/// @brief local stack Monitor (optional) +void SupervisorTask::TaskMonitorStack() +{ + uint16_t stackUsage = (uint16_t)uxTaskGetStackHighWaterMark( NULL ); + if((stackUsage) && (stackUsage < param.system_status->tasks[LOCAL_TASK_ID].stack)) { + param.systemStatusLock->Take(); + param.system_status->tasks[LOCAL_TASK_ID].stack = stackUsage; + param.systemStatusLock->Give(); + } +} +#endif + +/// @brief local watchDog and Sleep flag Task (optional) +/// @param millis_standby time in ms to perfor check of WDT. If longer than WDT Reset, WDT is temporanly suspend +void SupervisorTask::TaskWatchDog(uint32_t millis_standby) +{ + // Local TaskWatchDog update + param.systemStatusLock->Take(); + // Update WDT Signal (Direct or Long function Timered) + if(millis_standby) + { + // Check 1/2 Freq. controller ready to WDT only SET flag + if((millis_standby) < WDT_CONTROLLER_MS / 2) { + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::set; + } else { + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::timer; + // Add security milimal Freq to check + param.system_status->tasks[LOCAL_TASK_ID].watch_dog_ms = millis_standby + WDT_CONTROLLER_MS; + } + } + else + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::set; + param.systemStatusLock->Give(); +} + +/// @brief local suspend flag and positor running state Task (optional) +/// @param state_position Sw_Position (Local STATE) +/// @param state_subposition Sw_SubPosition (Optional Local SUB_STATE Position Monitor) +/// @param state_operation operative mode flag status for this task +void SupervisorTask::TaskState(uint8_t state_position, uint8_t state_subposition, task_flag state_operation) +{ + // Local TaskWatchDog update + param.systemStatusLock->Take(); + // Signal Task sleep/disabled mode from request (Auto SET WDT on Resume) + if((param.system_status->tasks[LOCAL_TASK_ID].state == task_flag::suspended)&& + (state_operation==task_flag::normal)) + param.system_status->tasks[LOCAL_TASK_ID].watch_dog = wdt_flag::set; + param.system_status->tasks[LOCAL_TASK_ID].state = state_operation; + param.system_status->tasks[LOCAL_TASK_ID].running_pos = state_position; + param.system_status->tasks[LOCAL_TASK_ID].running_sub = state_subposition; + param.systemStatusLock->Give(); +} + +/// @brief RUN Task +void SupervisorTask::Run() +{ + // Request response for system queue Task controlled... + system_message_t system_message; + + // Start Running Monitor and First WDT normal state + #if (ENABLE_STACK_USAGE) + TaskMonitorStack(); + #endif + TaskState(state, UNUSED_SUB_POSITION, task_flag::normal); + + while (true) + { + switch (state) + { + case SUPERVISOR_STATE_INIT: + // Load configuration is also done... + // Seting Startup param TASK and INIT Function Here... + param.systemStatusLock->Take(); + // Done Load config && Starting function + param.system_status->flags.is_cfg_loaded = true; + param.systemStatusLock->Give(); + + state = SUPERVISOR_STATE_CHECK_OPERATION; + TRACE_ERROR_F(F("SUPERVISOR_STATE_INIT -> SUPERVISOR_STATE_CHECK_OPERATION\r\n")); + break; + + case SUPERVISOR_STATE_CHECK_OPERATION: + // true if configuration ok and loaded -> + if (param.system_status->flags.is_cfg_loaded) + { + // Security autoremove maintenance after 1 hour from calling starting method + if(param.system_status->flags.is_maintenance) { + if((rtc.getEpoch() - param.system_status->datetime.time_start_maintenance) > SUPERVISOR_AUTO_END_MAINTENANCE_SEC) { + param.systemStatusLock->Take(); + param.system_status->flags.is_maintenance = false; + param.systemStatusLock->Give(); + } + } + // Standard SUPERVISOR_OPERATION SYSTEM CHECK... + // ********* SYSTEM QUEUE REQUEST *********** + // Check Queue command system status + if(!param.systemMessageQueue->IsEmpty()) { + if(param.systemMessageQueue->Peek(&system_message, 0)) { + if(system_message.task_dest == SUPERVISOR_TASK_ID) { + // Command direct for TASK remove from queue + param.systemMessageQueue->Dequeue(&system_message); + if(system_message.command.do_maint) { + param.systemStatusLock->Take(); + if(system_message.param != 0) { + param.system_status->flags.is_maintenance = true; + // Save maintenance start epoch (reset automatic) + param.system_status->datetime.time_start_maintenance = rtc.getEpoch(); + } else { + param.system_status->flags.is_maintenance = false; + } + param.systemStatusLock->Give(); + } + } + // Its request addressed into ALL TASK... -> no pull (only SUPERVISOR or exernal gestor) + else if(system_message.task_dest == ALL_TASK_ID) { + // Pull && elaborate command, + // Sleep Global was called from CAN TASK When CAN Finished operation + // from request and comuinication with Master. Master send flag Sleep... + if(system_message.command.do_sleep) + { + // Check All Module Direct Controlled Entered in Sleep and performed + // Local ShutDown periph, IO data ecc... IF ALL OK-> Enter LowPOWER Mode + // ************ SLEEP ALL MODULE OK -> SLEEP SUPERVISOR ************ + // Sleeping or Suspend Module are same. Only normal_mode suspend Sleep Mode + // SLEEP SUPERVISOR -> ALL MODULE IS SLEEPING. Tickless_mode CAN Start + if((param.system_status->tasks[ACCELEROMETER_TASK_ID].state != task_flag::normal) && + (param.system_status->tasks[CAN_TASK_ID].state != task_flag::normal) && + (param.system_status->tasks[ELABORATE_TASK_ID].state != task_flag::normal)) { + // Enter to Sleep Complete (Remove before queue Message TaskSleep) + // Ready for Next Security Startup without Reenter Sleep + // Next Enter with Other QueueMessage from CAN or Other Task... + param.systemMessageQueue->Dequeue(&system_message); + // Enter task sleep (enable global LowPower procedure...) + // Local WatchDog update + TaskWatchDog(SUPERVISOR_TASK_SLEEP_DELAY_MS); + TaskState(state, UNUSED_SUB_POSITION, task_flag::sleepy); + // ... -> Enter LowPower on call Delay ... -> + Delay(Ticks::MsToTicks(SUPERVISOR_TASK_SLEEP_DELAY_MS)); + TaskState(state, UNUSED_SUB_POSITION, task_flag::normal); + } + } + } + } + } + } + else + { + TRACE_ERROR_F(F("SUPERVISOR_STATE_CHECK_OPERATION -> ??? Condizione non gestita!!!\r\n")); + Suspend(); + } + break; + + case SUPERVISOR_STATE_END: + TRACE_VERBOSE_F(F("SUPERVISOR_STATE_END -> SUPERVISOR_STATE_CHECK_OPERATION\r\n")); + state = SUPERVISOR_STATE_CHECK_OPERATION; + break; + } + + // one step switch non blocking + #if (ENABLE_STACK_USAGE) + TaskMonitorStack(); + #endif + + // Local TaskWatchDog update; + TaskWatchDog(SUPERVISOR_TASK_WAIT_DELAY_MS); + + // Standard delay task + DelayUntil(Ticks::MsToTicks(SUPERVISOR_TASK_WAIT_DELAY_MS)); + + } +} + +/// @brief Load configuration from Register +void SupervisorTask::loadConfiguration() +{ + // param.configuration configuration Module + // param.configurationLock Semaphore config Access + // param.registerAccessLock Semaphore register Access + // Verify if config valid + bool register_config_valid = true; + // Param Reading + static uavcan_register_Value_1_0 val = {0}; + uint8_t idx; + + // Reset sensor parameter to default with external initializaztion request + if(param.is_initialization_request) { + param.is_initialization_request = false; + register_config_valid = false; + } + + TRACE_INFO_F(F("SUPERVISOR: Load configuration...\r\n")); + + // Reading RMAP Module identify Param -> (READ) + Check(REWRITE) -> [Readonly] + // uint8_t module_main_version; module main version + // uint8_t module_minor_version; module minor version + // uint8_t module_type; module type + if(register_config_valid) { + // Select type register (uint_8) + uavcan_register_Value_1_0_select_natural8_(&val); + val.natural8.value.count = 3; + // Loading Default + val.natural8.value.elements[0] = MODULE_MAIN_VERSION; + val.natural8.value.elements[1] = MODULE_MINOR_VERSION; + val.natural8.value.elements[2] = MODULE_TYPE; + param.registerAccessLock->Take(); + param.clRegister->read("rmap.module.identify", &val); + param.registerAccessLock->Give(); + if(uavcan_register_Value_1_0_is_natural8_(&val) && (val.natural8.value.count != 3)) { + register_config_valid = false; + } else { + param.configurationLock->Take(); + param.configuration->module_main_version = val.natural8.value.elements[0]; + param.configuration->module_minor_version = val.natural8.value.elements[1]; + param.configuration->module_type = val.natural8.value.elements[2]; + param.configurationLock->Give(); + // Parameter change for update firmare or local modify register? + // Reinit value: Register are readonly purpose utility remote application + if((param.configuration->module_main_version != MODULE_MAIN_VERSION)|| + (param.configuration->module_minor_version != MODULE_MINOR_VERSION)|| + (param.configuration->module_type != MODULE_TYPE)) { + // Rewrite Register... + val.natural8.value.elements[0] = MODULE_MAIN_VERSION; + val.natural8.value.elements[1] = MODULE_MINOR_VERSION; + val.natural8.value.elements[2] = MODULE_TYPE; + // Reload parameter correct + param.configuration->module_main_version = MODULE_MAIN_VERSION; + param.configuration->module_minor_version = MODULE_MINOR_VERSION; + param.configuration->module_type = MODULE_TYPE; + param.registerAccessLock->Take(); + param.clRegister->write("rmap.module.identify", &val); + param.registerAccessLock->Give(); + } + } + } + + // Reading RMAP Module sensor delay acquire -> (READ/WRITE) + // uint32_t sensor_acquisition_delay_ms; + if(register_config_valid) { + // Select type register (uint_32) + uavcan_register_Value_1_0_select_natural32_(&val); + val.natural32.value.count = 1; + // Loading Default + val.natural32.value.elements[0] = SENSORS_ACQUISITION_DELAY_MS; + param.registerAccessLock->Take(); + param.clRegister->read("rmap.module.sensor.acquisition", &val); + param.registerAccessLock->Give(); + if(uavcan_register_Value_1_0_is_natural32_(&val) && (val.natural32.value.count != 1)) { + register_config_valid = false; + } else { + param.configurationLock->Take(); + param.configuration->sensor_acquisition_delay_ms = val.natural32.value.elements[0]; + param.configurationLock->Give(); + } + } + + // Reading multiple Param (ADC Active) + if(register_config_valid) { + // Select type register (uint_8) + uavcan_register_Value_1_0_select_natural8_(&val); + val.natural8.value.count = MAX_ADC_CHANELS; + // Loading Default ( Active first chanel ) + val.natural8.value.elements[0] = 1; + val.natural8.value.elements[1] = 0; + val.natural8.value.elements[2] = 0; + val.natural8.value.elements[3] = 0; + param.registerAccessLock->Take(); + param.clRegister->read("rmap.module.sensor.adc.active", &val); + param.registerAccessLock->Give(); + if(uavcan_register_Value_1_0_is_natural8_(&val) && (val.natural8.value.count != MAX_ADC_CHANELS)) { + register_config_valid = false; + } else { + param.configurationLock->Take(); + for(idx=0; idxsensors[idx].is_active = val.natural8.value.elements[idx]; + param.configurationLock->Give(); + } + } + + // Reading multiple Param (ADC Active) + if(register_config_valid) { + // Select type register (uint_8) + uavcan_register_Value_1_0_select_natural8_(&val); + val.natural8.value.count = MAX_ADC_CHANELS; + // Loading Default ( Active low Voltave 0-3 Volt Input ) + for(idx=0; idxTake(); + param.clRegister->read("rmap.module.sensor.adc.type", &val); + param.registerAccessLock->Give(); + if(uavcan_register_Value_1_0_is_natural8_(&val) && (val.natural8.value.count != MAX_ADC_CHANELS)) { + register_config_valid = false; + } else { + param.configurationLock->Take(); + for(idx=0; idxsensors[idx].adc_type = (Adc_Mode)val.natural8.value.elements[idx]; + param.configurationLock->Give(); + } + } + + // Reading multiple Param (ADC Gain) + if(register_config_valid) { + // Select type register (real_32) + uavcan_register_Value_1_0_select_real32_(&val); + val.real32.value.count = MAX_ADC_CHANELS; + // Loading Default ( Gain is 1 ) + for(idx=0; idxTake(); + param.clRegister->read("rmap.module.sensor.adc.gain", &val); + param.registerAccessLock->Give(); + if(uavcan_register_Value_1_0_is_real32_(&val) && (val.real32.value.count != MAX_ADC_CHANELS)) { + register_config_valid = false; + } else { + param.configurationLock->Take(); + for(idx=0; idxsensors[idx].adc_gain = val.real32.value.elements[idx]; + param.configurationLock->Give(); + } + } + + // Reading multiple Param (ADC Offset) + if(register_config_valid) { + // Select type register (real_32) + uavcan_register_Value_1_0_select_real32_(&val); + val.real32.value.count = MAX_ADC_CHANELS; + // Loading Default ( Offset is 0 ) + for(idx=0; idxTake(); + param.clRegister->read("rmap.module.sensor.adc.offs", &val); + param.registerAccessLock->Give(); + if(uavcan_register_Value_1_0_is_real32_(&val) && (val.real32.value.count != MAX_ADC_CHANELS)) { + register_config_valid = false; + } else { + param.configurationLock->Take(); + for(idx=0; idxsensors[idx].adc_offset = val.real32.value.elements[idx]; + param.configurationLock->Give(); + } + } + + // Reading multiple Param (Analog Min) + if(register_config_valid) { + // Select type register (real_32) + uavcan_register_Value_1_0_select_real32_(&val); + val.real32.value.count = MAX_ADC_CHANELS; + // Loading Default ( Analog Min is LEAF_VOLTAGE_MIN ) + for(idx=0; idxTake(); + param.clRegister->read("rmap.module.sensor.adc.min", &val); + param.registerAccessLock->Give(); + if(uavcan_register_Value_1_0_is_real32_(&val) && (val.real32.value.count != MAX_ADC_CHANELS)) { + register_config_valid = false; + } else { + param.configurationLock->Take(); + for(idx=0; idxsensors[idx].analog_min = val.real32.value.elements[idx]; + param.configurationLock->Give(); + } + } + + // Reading multiple Param (Analog Max) + if(register_config_valid) { + // Select type register (real_32) + uavcan_register_Value_1_0_select_real32_(&val); + val.real32.value.count = MAX_ADC_CHANELS; + // Loading Default ( Analog Max is LEAF_VOLTAGE_MAX ) + for(idx=0; idxTake(); + param.clRegister->read("rmap.module.sensor.adc.max", &val); + param.registerAccessLock->Give(); + if(uavcan_register_Value_1_0_is_real32_(&val) && (val.real32.value.count != MAX_ADC_CHANELS)) { + register_config_valid = false; + } else { + param.configurationLock->Take(); + for(idx=0; idxsensors[idx].analog_max = val.real32.value.elements[idx]; + param.configurationLock->Give(); + } + } + + // INIT CONFIG (Config invalid) + if(!register_config_valid) { + // Reinit Configuration with default classic value + saveConfiguration(CONFIGURATION_DEFAULT); + } + printConfiguration(); +} + +/// @brief Save/Init configuration base Register Class +/// @param is_default true if is need to prepare config default value +void SupervisorTask::saveConfiguration(bool is_default) +{ + // param.configuration configuration Module + // param.configurationLock Semaphore config Access + // param.registerAccessLock Semaphore register Access + static uavcan_register_Value_1_0 val = {0}; // Save configuration into register + uint8_t idx; + + // Load default value to WRITE into config base + if(is_default) { + + param.configurationLock->Take(); + + param.configuration->module_main_version = MODULE_MAIN_VERSION; + param.configuration->module_minor_version = MODULE_MINOR_VERSION; + param.configuration->module_type = MODULE_TYPE; + + // Acquisition time sensor default + param.configuration->sensor_acquisition_delay_ms = SENSORS_ACQUISITION_DELAY_MS; + + for(idx=0; idxsensors[idx].adc_gain = 1; + param.configuration->sensors[idx].adc_offset = 0; + param.configuration->sensors[idx].adc_type = Adc_Mode::Volt; + param.configuration->sensors[idx].analog_min = LEAF_VOLTAGE_MIN; + param.configuration->sensors[idx].analog_max = LEAF_VOLTAGE_MAX; + param.configuration->sensors[idx].is_active = 0; + } + // Enable first chanel + param.configuration->sensors[0].is_active = 1; + + param.configurationLock->Give(); + + } + + // Writing RMAP Module identify Param -> (READ) + // uint8_t module_main_version; module main version + // uint8_t module_minor_version; module minor version + // uint8_t module_type; module type + // Select type register (uint_8) + uavcan_register_Value_1_0_select_natural8_(&val); + val.natural8.value.count = 3; + // Loading Default + param.configurationLock->Take(); + val.natural8.value.elements[0] = param.configuration->module_main_version; + val.natural8.value.elements[1] = param.configuration->module_minor_version; + val.natural8.value.elements[2] = param.configuration->module_type; + param.configurationLock->Give(); + param.registerAccessLock->Take(); + param.clRegister->write("rmap.module.identify", &val); + param.registerAccessLock->Give(); + + // Writing RMAP Module sensor delay acquire -> (READ/WRITE) + // uint32_t sensor_acquisition_delay_ms; + // Select type register (uint_32) + uavcan_register_Value_1_0_select_natural32_(&val); + val.natural32.value.count = 1; + // Loading Default + param.configurationLock->Take(); + val.natural32.value.elements[0] = param.configuration->sensor_acquisition_delay_ms; + param.configurationLock->Give(); + param.registerAccessLock->Take(); + param.clRegister->write("rmap.module.sensor.acquisition", &val); + param.registerAccessLock->Give(); + + uavcan_register_Value_1_0_select_natural8_(&val); + val.natural8.value.count = MAX_ADC_CHANELS; + // Loading Default + param.configurationLock->Take(); + for(idx=0; idxsensors[idx].is_active; + param.configurationLock->Give(); + param.registerAccessLock->Take(); + param.clRegister->write("rmap.module.sensor.adc.active", &val); + param.registerAccessLock->Give(); + + uavcan_register_Value_1_0_select_natural8_(&val); + val.natural8.value.count = MAX_ADC_CHANELS; + // Loading Default + param.configurationLock->Take(); + for(idx=0; idxsensors[idx].adc_type; + param.configurationLock->Give(); + param.registerAccessLock->Take(); + param.clRegister->write("rmap.module.sensor.adc.type", &val); + param.registerAccessLock->Give(); + + // Writing RMAP Module sensor adc GAIN -> (READ/WRITE) + uavcan_register_Value_1_0_select_real32_(&val); + val.real32.value.count = MAX_ADC_CHANELS; + // Loading Default + param.configurationLock->Take(); + for(idx=0; idxsensors[idx].adc_gain; + param.configurationLock->Give(); + param.registerAccessLock->Take(); + param.clRegister->write("rmap.module.sensor.adc.gain", &val); + param.registerAccessLock->Give(); + + // Writing RMAP Module sensor adc OFFSET -> (READ/WRITE) + uavcan_register_Value_1_0_select_real32_(&val); + val.real32.value.count = MAX_ADC_CHANELS; + // Loading Default + param.configurationLock->Take(); + for(idx=0; idxsensors[idx].adc_offset; + param.configurationLock->Give(); + param.registerAccessLock->Take(); + param.clRegister->write("rmap.module.sensor.adc.offs", &val); + param.registerAccessLock->Give(); + + // Writing RMAP Module sensor adc GAIN -> (READ/WRITE) + uavcan_register_Value_1_0_select_real32_(&val); + val.real32.value.count = MAX_ADC_CHANELS; + // Loading Default + param.configurationLock->Take(); + for(idx=0; idxsensors[idx].analog_min; + param.configurationLock->Give(); + param.registerAccessLock->Take(); + param.clRegister->write("rmap.module.sensor.adc.min", &val); + param.registerAccessLock->Give(); + + // Writing RMAP Module sensor adc OFFSET -> (READ/WRITE) + uavcan_register_Value_1_0_select_real32_(&val); + val.real32.value.count = MAX_ADC_CHANELS; + // Loading Default + param.configurationLock->Take(); + for(idx=0; idxsensors[idx].analog_max; + param.configurationLock->Give(); + param.registerAccessLock->Take(); + param.clRegister->write("rmap.module.sensor.adx.max", &val); + param.registerAccessLock->Give(); + + if(is_default) { + TRACE_INFO_F(F("SUPERVISOR: Save configuration...\r\n")); + } else { + TRACE_INFO_F(F("SUPERVISOR: Init configuration and save parameter...\r\n")); + } + printConfiguration(); +} + +/// @brief Print configuratione +void SupervisorTask::printConfiguration() +{ + // param.configuration configuration Module + // param.configurationLock Semaphore config Access + // param.registerAccessLock Semaphore register Access + char stima_name[STIMA_MODULE_NAME_LENGTH]; + + param.configurationLock->Take(); + + getStimaNameByType(stima_name, param.configuration->module_type); + TRACE_INFO_F(F("-> type: %s\r\n"), stima_name); + TRACE_INFO_F(F("-> main version: %u\r\n"), param.configuration->module_main_version); + TRACE_INFO_F(F("-> minor version: %u\r\n"), param.configuration->module_minor_version); + TRACE_INFO_F(F("-> acquisition delay: %u [ms]\r\n"), param.configuration->sensor_acquisition_delay_ms); + + param.configurationLock->Give(); +} diff --git a/platformio/stima_v4/slave-leaf/src/tasks/wdt_task.cpp b/platformio/stima_v4/slave-leaf/src/tasks/wdt_task.cpp new file mode 100644 index 000000000..20a6b1380 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/src/tasks/wdt_task.cpp @@ -0,0 +1,212 @@ +/** + ****************************************************************************** + * @file wdt_task.cpp + * @author Moreno Gasperini + * @brief wdt_task source file (Wdt && Logging Task for Module Slave) + ****************************************************************************** + * @attention + * + *

© Stimav4 is Copyright (C) 2023 ARPAE-SIMC urpsim@arpae.it

+ *

All rights reserved.

+ * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * . + * + ****************************************************************************** +*/ + +#define TRACE_LEVEL WDT_TASK_TRACE_LEVEL +#define LOCAL_TASK_ID WDT_TASK_ID + +#include "tasks/wdt_task.h" + +#if (ENABLE_WDT) + +/// @brief Construct the Wdt Task::WdtTask object +/// @param taskName name of the task +/// @param stackSize size of the stack +/// @param priority priority of the task +/// @param WdtParam parameters for the task +WdtTask::WdtTask(const char *taskName, uint16_t stackSize, uint8_t priority, WdtParam_t wdtParam) : Thread(taskName, stackSize, priority), param(wdtParam) +{ + Start(); +}; + +/// @brief RUN Task +void WdtTask::Run() { + bool firsCheck = true; + uint16_t stackUsage; + char strTask[12] = {0}; + uint8_t last_day_boot_rst; + + // WDT Start to Normal... + param.systemStatusLock->Take(); + param.system_status->tasks[WDT_TASK_ID].state = task_flag::normal; + param.systemStatusLock->Give(); + + TRACE_INFO_F(F("WDT: Starting WDT and Info Stack TASK...")); + + while (true) { + + // Check WDT Ready to reload (reset) + bool resetWdt = true; + + // Reset one time for day (reset and wdt index if event occurs. Send to Master) + if(firsCheck) { + // Init last day at first + last_day_boot_rst = rtc.getDay(); + } else { + // Check day is changed + if(last_day_boot_rst != rtc.getDay()) { + last_day_boot_rst = rtc.getDay(); + // Reset counter if occurs event + if((param.boot_request->tot_reset)||(param.boot_request->wdt_reset)) { + // Reset counter on new or restored firmware + param.boot_request->tot_reset = 0; + param.boot_request->wdt_reset = 0; + // Save info bootloader block + param.eeprom->Write(BOOT_LOADER_STRUCT_ADDR, (uint8_t*) param.boot_request, sizeof(bootloader_t)); + } + } + } + + TRACE_INFO_F(F("%s: "), Thread::GetName().c_str()); + // Trace DateTime with Semaphore + if(param.rtcLock->Take(Ticks::MsToTicks(RTC_WAIT_DELAY_MS))) { + TRACE_INFO_F(F("%02d/%02d/%02d "), rtc.getDay(), rtc.getMonth(), rtc.getYear()); + TRACE_INFO_F(F("%02d:%02d:%02d.%03d\r\n"), rtc.getHours(), rtc.getMinutes(), rtc.getSeconds(), rtc.getSubSeconds()); + param.rtcLock->Give(); + } + + param.systemStatusLock->Take(); + // WDT Always Set + param.system_status->tasks[WDT_TASK_ID].watch_dog = wdt_flag::set; + // Check All Module Setting Flag WDT before reset Global WDT + for(uint8_t id = 0; id < TOTAL_INFO_TASK; id++) + { + // For all Running Task (not suspend) and (real started Running_POS != 0) + // Sleep is noting Suspended. WatchDog (xxx) Longer Time is required !!! + if((param.system_status->tasks[id].state != task_flag::suspended)&& + (param.system_status->tasks[id].running_pos)) { + // Checkink First WDT Timer correct + if (param.system_status->tasks[id].watch_dog == wdt_flag::timer) { + param.system_status->tasks[id].watch_dog_ms -= WDT_TASK_WAIT_DELAY_MS; + if(param.system_status->tasks[id].watch_dog_ms < 0) + { + // Exit Time validity check WDT. Remove Flag and reset TimeUp + param.system_status->tasks[id].watch_dog_ms = 0; + param.system_status->tasks[id].watch_dog = wdt_flag::clear; + } + } + // If single task not performed local signal WDT... + if (param.system_status->tasks[id].watch_dog == wdt_flag::clear) + // Wdt is not applicable (All Working TASK Need to be operative!!!) + resetWdt = false; + } + } + param.systemStatusLock->Give(); + + // Logging Stack (Only ready module task controlled) + #if (ENABLE_STACK_USAGE) + // Update This Task + stackUsage = (uint16_t)uxTaskGetStackHighWaterMark( NULL ); + if((stackUsage) && (stackUsage < param.system_status->tasks[WDT_TASK_ID].stack)) { + param.systemStatusLock->Take(); + param.system_status->tasks[WDT_TASK_ID].stack = stackUsage; + param.systemStatusLock->Give(); + } + TRACE_INFO_F(F("WDT: Stack Free monitor, Heap free: %lu\r\n"), (uint32_t)xPortGetFreeHeapSize()); + for(uint8_t id = 0; id < TOTAL_INFO_TASK; id++) { + if(param.system_status->tasks[id].stack != 0xFFFFu) { + switch(id) { + case ACCELEROMETER_TASK_ID: + strcpy (strTask, "Accelerom. "); break; + case CAN_TASK_ID: + strcpy (strTask, "Can Bus "); break; + case ELABORATE_TASK_ID: + strcpy (strTask, "Elabor.Data "); break; + case SENSOR_TASK_ID: + strcpy (strTask, "Mod.Sensor "); break; + case SUPERVISOR_TASK_ID: + strcpy (strTask, "Supervisor "); break; + case WDT_TASK_ID: + strcpy (strTask, "WDT Info "); break; + } + TRACE_INFO_F(F("%s%s : %d\r\n"), strTask, + (param.system_status->tasks[id].state == task_flag::suspended) ? SUSPEND_STRING : + ((param.system_status->tasks[id].watch_dog == wdt_flag::clear) ? SPACE_STRING : FLAG_STRING), + param.system_status->tasks[id].stack); + } + } + #endif + + // Update TaskWatchDog (if all enabledModule setting local Flag) + // else... Can Flash Saving INFO on Task Not Responding after (XXX mS) + // For check TASK debugging Info + if(resetWdt) + { + #if (ENABLE_WDT) + TRACE_INFO_F(F("WDT: Reset WDT OK\r\n")); + IWatchdog.reload(); + #endif + // Reset WDT Variables for TASK + param.systemStatusLock->Take(); + // Check All Module Setting Flag WDT before reset Global WDT + // WDT Are called from Task into TASK While(1) Generic... + // + for(uint8_t id = 0; id < TOTAL_INFO_TASK; id++) { + // Reset only setted WDT Flags to Reset State (Not ::rest) + if(param.system_status->tasks[id].watch_dog == wdt_flag::set) + param.system_status->tasks[id].watch_dog = wdt_flag::clear; + } + param.systemStatusLock->Give(); + + // Reset WDT Complete ( At first Attempt ALL TASK Working. Clear RollBack request) + // Only at first step check EEProm Structure BootLoader + // All is OK, all task started (no checked position software running_pos) + // No Need to check WDT from All Task (if config parametr error some task) + // Can not perform Complete LOAD and configuration BUT Application is OK + // If All Task Start and enter in WDT Task, The application is Ready to RUN + if(firsCheck) { + firsCheck = false; + // Flag OK Start APP Excuted (Loading structure at boot in main.c) + if (!param.boot_request->app_executed_ok) { + if(param.boot_request->rollback_executed) { + TRACE_INFO_F(F("WDT: Flashing firmware with roll_back executed.")); + } else { + TRACE_INFO_F(F("WDT: Flashing firmware with new version ready, clear flags.")); + } + // Remove request_upload and roll_back (firmware started OK) + param.boot_request->app_executed_ok = true; + param.boot_request->request_upload = false; + param.boot_request->backup_executed = false; + param.boot_request->app_forcing_start = false; + param.boot_request->rollback_executed = false; + param.boot_request->upload_error = 0; + param.boot_request->upload_executed = false; + // Reset counter on new or restored firmware + param.boot_request->tot_reset = 0; + param.boot_request->wdt_reset = 0; + param.eeprom->Write(BOOT_LOADER_STRUCT_ADDR, (uint8_t*) param.boot_request, sizeof(bootloader_t)); + } + } + } + + // Exit WDT + DelayUntil(Ticks::MsToTicks(WDT_TASK_WAIT_DELAY_MS)); + } +} + +#endif diff --git a/platformio/stima_v4/slave-leaf/test/README b/platformio/stima_v4/slave-leaf/test/README new file mode 100644 index 000000000..9b1e87bc6 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html diff --git a/platformio/stima_v4/slave-leaf/user_custom/bootloader.ld b/platformio/stima_v4/slave-leaf/user_custom/bootloader.ld new file mode 100644 index 000000000..aa2d8df29 --- /dev/null +++ b/platformio/stima_v4/slave-leaf/user_custom/bootloader.ld @@ -0,0 +1,176 @@ +/** + ****************************************************************************** + * @file bootloader.ld + * @author Auto-generated by STM32CubeIDE + * @brief Linker script for STM32L452RETx Device from STM32L4 series + * 256Kbytes FLASH + * 160Kbytes RAM + * + * Set heap size, stack size and stack location according + * to application requirements. + * + * Set memory bank area and size if external memory is used + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2020 STMicroelectronics. + *

All rights reserved.

+ * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ****************************************************************************** + */ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +_estack = ORIGIN(RAM) + LENGTH(RAM); /* end of "RAM" Ram type memory */ + +_Min_Heap_Size = 0x200; /* required amount of heap */ +_Min_Stack_Size = 0x400; /* required amount of stack */ + +/* Memories definition */ +MEMORY +{ + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = LD_MAX_DATA_SIZE + /* FLASH (rx) : ORIGIN = 0x800C000 , LENGTH = 208K */ + FLASH (rx) : ORIGIN = 0x800B000 , LENGTH = 0x34FFF +} + +/* Sections */ +SECTIONS +{ + /* The startup code into "FLASH" Rom type memory */ + .isr_vector : + { + . = ALIGN(4); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + } >FLASH + + /* The program code and other data into "FLASH" Rom type memory */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >FLASH + + /* Constant data into "FLASH" Rom type memory */ + .rodata : + { + . = ALIGN(4); + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + . = ALIGN(4); + } >FLASH + + .ARM.extab : { + . = ALIGN(4); + *(.ARM.extab* .gnu.linkonce.armextab.*) + . = ALIGN(4); + } >FLASH + + .ARM : { + . = ALIGN(4); + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + . = ALIGN(4); + } >FLASH + + .preinit_array : + { + . = ALIGN(4); + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + . = ALIGN(4); + } >FLASH + + .init_array : + { + . = ALIGN(4); + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + . = ALIGN(4); + } >FLASH + + .fini_array : + { + . = ALIGN(4); + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(SORT(.fini_array.*))) + KEEP (*(.fini_array*)) + PROVIDE_HIDDEN (__fini_array_end = .); + . = ALIGN(4); + } >FLASH + + /* Used by the startup to initialize data */ + _sidata = LOADADDR(.data); + + /* Initialized data sections into "RAM" Ram type memory */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + + } >RAM AT> FLASH + + /* Uninitialized data section into "RAM" Ram type memory */ + . = ALIGN(4); + .bss : + { + /* This is used by the startup in order to initialize the .bss section */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(.bss*) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM + + /* User_heap_stack section, used to check that there is enough "RAM" Ram type memory left */ + ._user_heap_stack : + { + . = ALIGN(8); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(8); + } >RAM + + /* Remove information from the compiler libraries */ + /DISCARD/ : + { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +}