1、第一次提交，创建hpm6750工程文件

.gitignore

@@ -0,0 +1,52 @@
+# Prerequisites
+*.d
+
+# Object files
+*.o
+*.ko
+*.obj
+*.elf
+
+# Linker output
+*.ilk
+*.map
+*.exp
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Libraries
+*.lib
+*.a
+*.la
+*.lo
+
+# Shared objects (inc. Windows DLLs)
+*.dll
+*.so
+*.so.*
+*.dylib
+
+# Executables
+*.exe
+*.out
+*.app
+*.i*86
+*.x86_64
+*.hex
+
+# Debug files
+*.dSYM/
+*.su
+*.idb
+*.pdb
+
+# Kernel Module Compile Results
+*.mod*
+*.cmd
+.tmp_versions/
+modules.order
+Module.symvers
+Mkfile.old
+dkms.conf

README_en.md

@@ -0,0 +1,62 @@
+# USER TEMPLATE
+
+## OVERVIEW
+
+This project is a user template, and users can copy the folder to their custom working directory for development based on this template.
+
+The user-defined files in this template are:
+
+- User Board
+- user App
+- User Linker
+
+## User Board
+
+In the `user_board` folder, there is a `yaml` file and a `cfg` file, and the names of these two files must be consistent with the folder name, so that start_gui can recognize the folder as a board folder.
+
+Among them, the `yaml` file is a user board level configuration file used to configure SOC names, openocd scripts, etc. The `cfg` file is a board level configuration file for openocd, which specifies parameters for onboard flash, debugger reset-init event handling, debugger gdb-attach event handling, etc.
+
+Usually, users can copy files from the hpm_sdk/boards/hpm_xxxx_evk directory to the user_board folder, and copy the `cfg` file from the evk directory of hpm_sdk/boards/openocd/boards to the user-board folder, and then make modifications based on their own board configuration.
+
+- < Note >: After copying, please modify the file names of `yaml` and `cfg` to match the folder name.
+
+The files in the `user_board` folder in this template were copied from the relevant files of hpm6750evk2. Users can replace or modify them according to their own board configuration
+
+## User App
+
+In the `user_app` folder, users can organize their own applications, such as the main function.
+
+
+When adding .c and .h files, please use `sdk_app_src` to add the .c files and use `sdk_app_inc` to add the path of the .h files in `CMakeList.txt`.
+
+If using `middleware`, `components`, etc., please refer to the `CMakeList.txt` in the sample corresponding to hpm_sdk, and then add relevant instructions in the user's `CMakeList.txt` following the example.
+
+The cmake commands supported in the HPM_SDK can be viewed in `hpm_sdk\docs\index_zh.html`:
+![sdk_cmake_api](doc/sdk_cmake_api.png)
+
+- Tips: After modifying `CMakeList.txt`, the project needs to be regenerated and compiled.
+
+## User Linker
+
+In the `user_app` folder, there are `linker` files for the application, which users can customize according to their own needs.
+
+Usually, users can copy the linker files from the `gcc`/`iar`/`segger` folder in hpm_sdk/soc/xxxx/xxxx/toolchains to user app/linkers and make modifications based on them.
+
+- < Note >: After copying, the linker file names under the `gcc`/`iar`/`segger` files should be consistent, only with different suffixes.
+
+Users can specify the Linker file to be used in the user application's `CMakeList.txt`. If a Linker file is not specified in `CMakeList.txt`, it can also be specified when generating the project using `start_gui` Tool.
+
+If a Linker file is not specified in either of the above cases, the SDK's default linker file will be used, which is located in hpm_sdk/soc/xxxx/xxxx/toolchains.
+
+## START_GUI DEMO
+
+After organizing the above files, you can use start_gui to generate your own project project.
+
+![start_gui_demo](doc/start_gui_demo.png)
+
+- Board Path：Specify the top-level `user_template` folder, start_gui will automatically search for board level files in that folder.
+- Application Path：Specify the top-level `user_template` folder, start_gui will automatically search for applications in that folder.
+- User GCC Liner File：Specify user `gcc` linker files. After generating the project, the gcc toolchain will use the linker in the gcc folder, the iar toolchain will use the linker in the iar folder, and the segger toolchain will use the linker in the segger folder.
+  - < Note >: The `Build Type` of start_gui needs to be correctly specified in conjunction with the linker file. For example, when the linker file uses Flash and there is no SDRAM, the `Build Type` of start_gui needs to be specified as `flash_xip`; When the linker file uses Flash and has SDRAM, the `Build Type` of start_gui needs to be specified as `flash_stram-xip`.
+- After configuration, click `Generate` to generate the project.
+  - < Note >: After modifying `CMakeList.txt`, the project needs to be regenerated and compiled.

README_zh.md

@@ -0,0 +1,64 @@
+# 用户模板
+
+## 概述
+
+本工程是用户模板，用户可将该文件夹复制到用户自定义的工作目录中，基于此模板进行开发。
+
+本模板主要牵涉到的用户自定义的文件有：
+
+- 用户板级文件Board
+- 用户应用程序App
+- 用户链接脚本Linker
+
+
+## 用户板级文件Board
+
+在`user_board`文件夹下，有一个`yaml`文件和一个`cfg`文件，这两个文件的名称**必须**要跟文件夹名称一致，这样start_gui才能识别出该文件夹为板级文件夹。
+
+其中，`yaml`文件是用户板级配置文件，用于配置soc名称、openocd脚本等。`cfg`文件是openocd的板级配置文件，指定板载Flash的参数、调试器reset-init事件处理、调试器gdb-attach事件处理等。
+
+通常，用户可将hpm_sdk/boards/hpm_xxxx_evk目录中文件复制到user_board文件夹下，将hpm_sdk/boards/openocd/boards目录evk对应的`cfg`文件复制到user_board文件夹下，然后在其基础上根据自身板子的配置进行修改。
+
+- <注意>：复制过来后，请修改`yaml`和`cfg`文件名称，与文件夹名称保持一致。
+
+本模板中的`user_board`文件夹的文件是从hpm6750evk2相关文件复制过来的，用户可根据自身板子的配置情况将其替换或修改。
+
+## 用户应用程序App
+
+在`user_app`文件夹下，用户可组织自己的应用程序，例如main函数等。
+
+添加的.c文件和.h文件时，请在`CMakeList.txt`中使用`sdk_app_src`添加.c文件，使用`sdk_app_inc`添加.h文件的路径。
+
+若使用到`middleware`、`components`等，请参考hpm_sdk对应sample中的`CMakeList.txt`，然后在依葫芦画瓢在用户的`CMakeList.txt`中添加相关指令。
+
+HPM_SDK中支持的cmake指令可在hpm_sdk\docs\index_zh.html中查看：
+![sdk_cmake_api](doc/sdk_cmake_api.png)
+
+- Tips：修改`CMakeList.txt`后，需要重新生成工程并编译，才能生效。
+
+## 用户链接脚本Linker
+
+在`user_app`文件夹下，有该应用程序的`linker`文件，用户可根据自身的需求自定义该文件。
+
+通常，用户可将hpm_sdk/soc/xxxx/xxxx/toolchains中的`gcc`/`iar`/`segger`文件夹中的linker文件复制到user_app/linkers中，在其基础上进行修改。
+
+- <注意>：复制过来后，`gcc`/`iar`/`segger`三个文件下的linker文件名称保持一致，仅后缀不一样。
+
+本模板中的linker文件是从hpm6750evk2的flash_xip liner文件复制过来的，用户可根据自身板子的配置情况将其替换或修改。
+
+用户在应用程序的`CMakeList.txt`中可以指定使用的Linker文件，如果在`CMakeList.txt`中不指定Linker文件，也可在start_gui生成工程的时候指定Linker文件。
+
+如果以上两种情况都不指定Linker文件，则会使用SDK默认的linker文件，位于hpm_sdk/soc/xxxx/xxxx/toolchains中。
+
+## START_GUI的使用
+
+上述的文件组织完毕后，可使用start_gui生成自己的project工程。
+
+![start_gui_demo](doc/start_gui_demo.png)
+
+- Board Path：指定顶层的`user_template`文件夹，它将自动搜索该文件夹下的板级文件。
+- Application Path：指定顶层的`user_template`文件夹，它将自动搜索该文件夹下的应用程序。
+- User GCC Liner File：指定用户自定义的gcc linker文件。生成project后，gcc工具链将使用gcc文件夹下的linker，iar工具链将使用iar文件夹下的linker，segger工具链将使用segger文件夹下的linker。
+  - <注意>: start_gui的`Build Type`需要配合linker文件正确指定。例如，当linker文件使用到Flash且没有SDRAM时，start_gui的`Build Type`需要指定为`flash_xip`；当linker文件使用到Flash且有SDRAM时，start_gui的`Build Type`需要指定为`flash_sdram_xip`。
+- 配置完毕后，点击`Generate`，即可生成工程。
+  - <注意>: 修改`CMakeList.txt`后，需要重新生成工程并编译，才能生效。

controller_yy_app/CMakeLists.txt

@@ -0,0 +1,53 @@
+# Copyright (c) 2021 HPMicro
+# SPDX-License-Identifier: BSD-3-Clause
+
+cmake_minimum_required(VERSION 3.13)
+
+set(CUSTOM_GCC_LINKER_FILE ${CMAKE_CURRENT_SOURCE_DIR}/linkers/gcc/user_linker.ld)
+set(CUSTOM_SES_LINKER_FILE ${CMAKE_CURRENT_SOURCE_DIR}/linkers/segger/user_linker.icf)
+set(CUSTOM_IAR_LINKER_FILE ${CMAKE_CURRENT_SOURCE_DIR}/linkers/iar/user_linker.icf)
+
+find_package(hpm-sdk REQUIRED HINTS $ENV{HPM_SDK_BASE})
+
+project(controlware_yy_app)
+# 添加hpm中间件
+set(CONFIG_SDMMC 1)
+set(CONFIG_FATFS 1)
+
+
+add_subdirectory(middleware/)
+# add_subdirectory(controlware/)
+# add_subdirectory(hardware/)
+# add_subdirectory(matrix/)
+# add_subdirectory(payload/)
+# add_subdirectory(remote_controller/)
+# add_subdirectory(software/)
+# add_subdirectory(v8/)
+# add_subdirectory(vklink/)
+# add_subdirectory(user_src/)
+
+sdk_app_inc(controlware/control_inc/)
+sdk_app_inc(hardware/hard_inc/)
+sdk_app_inc(matrix/)
+sdk_app_inc(payload/)
+sdk_app_inc(remote_controller/)
+sdk_app_inc(software/soft_inc/)
+sdk_app_inc(user_src/inc/)
+sdk_app_inc(v8/v8m/)
+sdk_app_inc(v8/v8m_yy/)
+sdk_app_inc(vklink/)
+
+sdk_app_src_glob(vklink/*.c)
+sdk_app_src_glob(v8/v8m/*.c)
+sdk_app_src_glob(v8/v8m_yy/*.c)
+sdk_app_src_glob(user_src/*.c)
+sdk_app_src_glob(software/*.c)
+sdk_app_src_glob(remote_controller/*.c)
+sdk_app_src_glob(payload/*.c)
+sdk_app_src_glob(matrix/*.c)
+sdk_app_src_glob(hardware/*.c)
+sdk_app_src_glob(controlware/*.c)
+
+
+# 创建IDE PRO
+generate_ide_projects()

controller_yy_app/controlware/control_attitude.c

@@ -0,0 +1,314 @@
+#if 0
+#include "control_attitude.h"
+#include "hpm_math.h"
+#include "auto_pilot.h"
+#include "common.h"
+#include "control_throttle.h"
+#include "flight_mode.h"
+#include "helpler_funtions.h"
+#include "lowpass_filter.h"
+#include "my_math.h"
+#include "params.h"
+#include "pilot_navigation.h"
+#include "soft_flash.h"
+#include "soft_imu.h"
+#include "soft_motor_output.h"
+#include "soft_rc_input.h"
+
+
+// 自动模式下机头方向
+static float target_wpt_bearing = 0.0f;
+
+float t_roll_last = 0.0f, t_pitch_last = 0.0f;
+
+/**
+ * @brief 设置自动模式下机头指向
+ * @param yaw_deg 机头指向
+ * @return true 设置成功
+ * @return false 设置失败，机头指向被锁定为指向某点
+ */
+bool set_automode_target_yaw(float yaw_deg)
+{
+    target_wpt_bearing = constrain_float(yaw_deg, 0.0f, 360.0f);
+    return true;
+}
+
+// convert pilot input to lean angles
+// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
+void get_pilot_desired_lean_angles(short roll_in, short pitch_in)
+{
+    float t_roll = 0.0f, t_pitch = 0.0f, t_total = 0.0f;
+    float ratio = 0.0f;
+    float scaler = (float)params_get_value(ParamNum_APMaxTilteAngleDeg) / (float)(500 - RC_DEAD_ZONE);
+
+    if (rc_in[RC_CH3] < 1050 && rc_in[RC_CH3] >= 1000)
+    {
+        roll_in = 1500;
+        pitch_in = 1500;
+    }
+
+    // 将杆量转换为目标姿态角
+    if ((roll_in - 1500) > RC_DEAD_ZONE)
+    {
+        t_roll = (roll_in - 1500 - RC_DEAD_ZONE) * scaler;
+    }
+    else if ((roll_in - 1500) < -RC_DEAD_ZONE)
+    {
+        t_roll = (roll_in - 1500 + RC_DEAD_ZONE) * scaler;
+    }
+    else
+    {
+        t_roll = 0.0f;
+    }
+
+    if ((pitch_in - 1500) > RC_DEAD_ZONE)
+    {
+        t_pitch = (pitch_in - 1500 - RC_DEAD_ZONE) * scaler;
+    }
+    else if ((pitch_in - 1500) < -RC_DEAD_ZONE)
+    {
+        t_pitch = (pitch_in - 1500 + RC_DEAD_ZONE) * scaler;
+    }
+    else
+    {
+        t_pitch = 0.0f;
+    }
+
+    t_total = get_norm(t_roll, t_pitch);
+
+    // do circular limit
+    if (t_total > params_get_value(ParamNum_APMaxTilteAngleDeg))
+    {
+        ratio = params_get_value(ParamNum_APMaxTilteAngleDeg) / t_total;
+
+        t_roll *= ratio;
+        t_pitch *= ratio;
+    }
+
+    pid_m_roll.angle_t = apply(t_roll, t_roll_last, 4.0f, fast_loop_dt);
+    pid_m_pitch.angle_t = apply(t_pitch, t_pitch_last, 4.0f, fast_loop_dt);
+    t_roll_last = pid_m_roll.angle_t;
+    t_pitch_last = pid_m_pitch.angle_t;
+}
+
+// get_pilot_desired_heading - transform pilot's yaw input into a desired yaw
+// rate
+// returns desired yaw rate in centi-degrees per second
+void get_pilot_desired_yaw_angle_fromrc(short yaw_in, float dt)
+{
+    float rc_yaw_rate = 0.0f;
+
+    bool yaw_motion = false;
+    static bool yaw_lock = true;
+
+    float scaler = (float)YAW_RATE_MAX / (float)(500 - RC_DEAD_ZONE);
+
+    if (rc_in[RC_CH3] < 1050 && rc_in[RC_CH3] >= 1000)
+        yaw_in = 1500;
+    //推杆启动慢，默认20启动，导致想小幅度转机头比较难实现
+    if ((yaw_in - 1500) > RC_DEAD_ZONE)
+    {
+        rc_yaw_rate = (yaw_in - 1500 - RC_DEAD_ZONE) * scaler;
+        yaw_motion = true;
+    }
+    else if ((yaw_in - 1500) < -RC_DEAD_ZONE)
+    {
+        rc_yaw_rate = (yaw_in - 1500 + RC_DEAD_ZONE) * scaler;
+        yaw_motion = true;
+    }
+    else
+    {
+        yaw_motion = false;
+        rc_yaw_rate = 0.0f;
+    }
+
+    // angle_p有值是才更新目标角度。避免调试时Kp为零，打杆飞机不动，但是angle_t在更新。
+    if (pid_v_yaw.angle_p > 0)
+    {
+        if (yaw_motion == true)
+        {
+            yaw_lock = false;
+            switch (flight_mode)
+            {
+            case ATTITUDE:
+                // rc_yaw_rate 方向是右打为正，和角度相同
+                pid_m_yaw.angle_t = wrap_360(attitude_head + rc_yaw_rate / pid_v_yaw.angle_p);
+                break;
+            default:
+                pid_m_yaw.angle_t = wrap_360(pid_m_yaw.angle_c + rc_yaw_rate / pid_v_yaw.angle_p);
+                break;
+            }
+        }
+        else
+        {
+            if (yaw_lock == false)
+            {
+                yaw_lock = true;
+                switch (flight_mode)
+                {
+                case ATTITUDE:
+                    // gyro_c 的方向是北偏西正，和角度相反
+                    pid_m_yaw.angle_t = wrap_360(attitude_head - 1.0f * pid_m_yaw.gyro_c / pid_v_yaw.angle_p);
+                    break;
+                default:
+                    pid_m_yaw.angle_t = wrap_360(pid_m_yaw.angle_c - 1.0f * pid_m_yaw.gyro_c / pid_v_yaw.angle_p);
+                    break;
+                }
+            }
+        }
+    }
+
+    attitude_head += (-pid_m_yaw.gyro_c * dt);
+    attitude_head = wrap_360(attitude_head);
+    if (ground_air_status == ON_GROUND)
+    {
+        attitude_head = pid_m_yaw.angle_c;
+        if (abs_int16(yaw_in - 1500) < RC_DEAD_ZONE)
+        {
+            pid_m_yaw.angle_t = pid_m_yaw.angle_c;
+        }
+    }
+}
+
+/*
+姿态控制部分，角度差装换为期望角速度
+*/
+float attitude_pid_ctl_rp(struct pid_method_rpy *method,
+                          struct pid_value_rpy *value)
+{
+    method->angle_e = method->angle_t - method->angle_c;
+
+    // ================ 计算P比例量 ==================
+    method->angle_p_item = method->angle_e * value->angle_p; //当前值为4.5
+
+    method->angle_pid_out = method->angle_p_item;
+
+    method->gyro_t = constrain_float(method->angle_pid_out, -80.0f, +80.0f);
+
+    /*
+    测试快速打杆超调问题，加入目标角速度的单次变化限幅。有明显改善。
+    测试期间查看rate_p<±100左右，变化迅速>，rate_i<±30左右，变化缓慢>，rate_d<±100左右，变化迅速>
+    测试rate_d的低通系数，系数过低时<5HZ>延时过大，导致很容易超调。
+    */
+    method->gyro_t = method->gyro_t_last + constrain_float(method->gyro_t - method->gyro_t_last,
+                        -pid_v_pos.brake_gyro, pid_v_pos.brake_gyro);
+    method->gyro_t_last = method->gyro_t;
+
+    return method->gyro_t;
+}
+
+/**************************实现函数********************************************
+ *函数原型:		float Get_Yaw_Error(float set,float currt)
+ *功　　能:	    计算航向差
+ *******************************************************************************/
+float get_yaw_error(float target, float currt)
+{
+    float temp;
+    temp = target - currt;
+    if (temp < 0)
+        temp += 360.0f;
+
+    if ((temp >= 0.0f) && (temp <= 180.0f))
+        return temp;
+    else
+        return (temp - 360.0f);
+}
+/*
+姿态控制部分，角度差装换为期望角速度
+*/
+float attitude_pid_ctl_yaw(struct pid_method_rpy *method,
+                           struct pid_value_rpy *value)
+{
+
+    if (flight_mode == ATTITUDE)
+    {
+        // 如果电机输出饱和，触发了航向限制，则赋值一下angle_t
+        if (MotorOutput_GetYawRestrictionStatus())
+            pid_m_yaw.angle_t = attitude_head;
+
+        method->angle_e = get_yaw_error(method->angle_t, attitude_head);
+
+        if (pid_v_yaw.angle_p > 0 &&
+            fabsf(method->angle_e) > YAW_RATE_MAX / pid_v_yaw.angle_p)
+        {
+            method->angle_e = method->angle_e / fabsf(method->angle_e) *
+                              YAW_RATE_MAX / pid_v_yaw.angle_p;
+
+            method->angle_t = wrap_360(attitude_head + method->angle_e);
+        }
+    }
+    else
+    {
+        // 如果电机输出饱和，触发了航向限制，则赋值一下angle_t
+        if (MotorOutput_GetYawRestrictionStatus())
+            pid_m_yaw.angle_t = pid_m_yaw.angle_c;
+
+        method->angle_e = get_yaw_error(method->angle_t, method->angle_c);
+
+        if (pid_v_yaw.angle_p > 0 &&
+            fabsf(method->angle_e) > YAW_RATE_MAX / pid_v_yaw.angle_p)
+        {
+            method->angle_e = method->angle_e / fabsf(method->angle_e) *
+                              YAW_RATE_MAX / pid_v_yaw.angle_p;
+
+            method->angle_t = wrap_360(method->angle_c + method->angle_e);
+        }
+    }
+
+    method->angle_p_item =
+        method->angle_e * pid_v_yaw.angle_p * -1.0f; //当前值为4.5
+
+    method->gyro_t = method->angle_p_item;
+
+    return method->gyro_t;
+}
+
+/**
+ * @brief 在不同的模式平滑计算目标杆量
+ * 由于SD卡存在延迟现象，此函数应
+ */
+void get_target_yaw_by_flight_mode(unsigned char flight_mode, float dt)
+{
+    if (pilot_mode == PILOT_NORMAL)
+    {
+        switch (flight_mode)
+        {
+        case ATTITUDE:
+            get_pilot_desired_yaw_angle_fromrc(rc_in[RC_YAW], dt);
+            set_automode_target_yaw(pid_m_yaw.angle_t);
+            break;
+        case GCS_VEHICLE_LAUNCH:
+            get_smooth_target_yaw(target_wpt_bearing, dt);
+            break;
+        default:
+            get_smooth_target_yaw(target_wpt_bearing, dt);
+            break;
+        }
+    }
+}
+
+/*********************************************************************
+参  数  ： argTargetYaw - 航线的方向
+返回值  ： 传给航向内环的目标航向
+功  能  ： 通过航线的方向计算出传递给内环的目标航向
+*********************************************************************/
+void get_smooth_target_yaw(float argTargetYaw, float dt)
+{
+    float yawError = wrap_180(argTargetYaw - pid_m_yaw.angle_t);
+
+    if (yawError > parinf._par_maxyawrate * dt)
+    {
+        pid_m_yaw.angle_t += (parinf._par_maxyawrate * dt);
+    }
+    else if (yawError < -1.0f * parinf._par_maxyawrate * dt)
+    {
+        pid_m_yaw.angle_t += (-1.0f * parinf._par_maxyawrate * dt);
+    }
+    else
+    {
+        pid_m_yaw.angle_t = argTargetYaw;
+    }
+
+    pid_m_yaw.angle_t = wrap_360(pid_m_yaw.angle_t);
+}
+#endif

controller_yy_app/controlware/control_inc/control_attitude.h

@@ -0,0 +1,25 @@
+
+#ifndef __CONTROL_ATTITUDE_H
+#define __CONTROL_ATTITUDE_H
+
+#include "control_rate.h"
+
+extern float t_roll_last, t_pitch_last;
+
+#define YAW_RATE_MAX 60 // 航向最大100deg/s
+
+float attitude_pid_ctl_rp(struct pid_method_rpy *method, struct pid_value_rpy *value);
+float attitude_pid_ctl_yaw(struct pid_method_rpy *method, struct pid_value_rpy *value);
+
+void get_pilot_desired_lean_angles(short roll_in, short pitch_in);
+void get_pilot_desired_yaw_angle_fromrc(short yaw_in, float dt);
+
+float get_yaw_error(float target, float currt);
+
+void get_target_yaw_by_flight_mode(unsigned char, float);
+
+void get_smooth_target_yaw(float argTargetYaw, float dt);
+
+bool set_automode_target_yaw(float yaw_deg);
+
+#endif

controller_yy_app/controlware/control_inc/control_rate.h

@@ -0,0 +1,133 @@
+
+#ifndef __CONTROL_RATE_H
+#define __CONTROL_RATE_H
+
+#include "flt_butter.h"
+#include "stdbool.h"
+#include <stdint.h>
+
+struct pid_value_rpy
+{
+    float angle_p;
+    float gyro_p;
+    float gyro_i;
+    float gyro_d;
+    unsigned char gyro_imax;
+};
+extern struct pid_value_rpy pid_v_roll;
+extern struct pid_value_rpy pid_v_pitch;
+extern struct pid_value_rpy pid_v_yaw;
+
+struct pid_value_ap
+{
+    float dist_p;
+    float speed_p;
+    float acc_p;
+    float acc_i;
+    float acc_d;
+    float brake_gyro;
+};
+extern struct pid_value_ap pid_v_alt;
+extern struct pid_value_ap pid_v_pos;
+
+struct pid_method_rpy
+{
+    float angle_c; // 当前角度值
+    float angle_t; // 目标角度值
+    float angle_e; // 角度差
+
+    float angle_p_item; // angle----P值
+
+    float angle_i_item;             // angle----I值
+    unsigned int angle_i_time_last; // angle----上次积分的时间戳
+
+    float angle_pid_out; // 当前PID的输出
+
+    float gyro_c;      // 当前角速度值
+    float gyro_c_last; //
+    float gyro_t;      // 目标角速度值
+    float gyro_t_last; // 上一次的目标角速度值
+    float gyro_e;      // 角速度差
+    float gyro_e_last;
+
+    bool gyro_filter_ready; // 滤波参数清零
+
+    LpfButter2 gyro_dirv_filter;
+
+    float gyro_p_item; // gyro----P值
+
+    float gyro_i_item; // gyro----I值
+
+    float gyro_d_item;      // gyro----D值
+    float gyro_d_item_last; // gyro----上一次D值
+    float gyro_d_filter;    // D值得滤波系数
+
+    float pid_out; // 当前PID的输出
+
+    float rotate_angle;       // 需要旋转的角度
+    int8_t enable_cw_rotate;  // 使能顺时针旋转
+    int8_t enable_ccw_rotate; // 使能逆时针旋转
+    bool enable_tyaw;
+};
+extern struct pid_method_rpy pid_m_roll;
+extern struct pid_method_rpy pid_m_pitch;
+extern struct pid_method_rpy pid_m_yaw;
+
+struct pid_method_ap
+{
+    int loc_c; // 当前位置
+    int loc_t; // 目标位置
+    int loc_e; // 位置差
+
+    float loc_p_item; // loction----P值
+
+    float vel_c;           // 当前速度值
+    float vel_t;           // 目标速度值
+    float vel_t_last;      // 上一次的目标速度值
+    float vel_e;           // 速度差
+    float vel_e_last;      // 上一次的速度差
+    bool vel_filter_ready; // 滤波参数清零
+
+    float vel_p_item;             // vel----P值
+    float vel_i_item;             // vel----I值
+    unsigned int vel_i_time_last; // vel----上次积分的时间戳
+    float vel_d_item;             // vel----D值
+    float vel_d_item_last;        // vel----D值上一次的值
+    float vel_dv_item;            // vel----D值
+    float vel_dv_item_last;       // vel----D值上一次的值
+
+    float accel_ff_item; // 目标速度微分出来的加速度前馈
+    LpfButter2 accel_ff_butter_filt;
+
+    float acc_c;           // 当前加速度值
+    float acc_c_last;      // 上一次加速度值
+    float acc_t;           // 目标加速度值
+    float acc_e;           // 加速度差
+    float acc_e_last;      // 上一次的加速度的差
+    bool acc_filter_reset; // 滤波参数清零
+
+    float acc_p_item; // acc----P值
+
+    float acc_i_item;             // acc----I值
+    unsigned int acc_i_time_last; // acc----上次积分的时间戳
+    float acc_d_item;             // acc----D值
+    bool thr_hold;
+
+    float pid_out_last; // 上次的PID输出
+    float pid_out;      // 当前PID的输出
+};
+extern struct pid_method_ap pid_m_alt;
+extern struct pid_method_ap pid_m_posx;
+extern struct pid_method_ap pid_m_posy;
+extern float attitude_head;
+
+float rate_pid_ctl_rp(struct pid_method_rpy *method,
+                      struct pid_value_rpy *value, float dt);
+float rate_pid_ctl_yaw(struct pid_method_rpy *method,
+                       struct pid_value_rpy *value, float dt);
+float rate_pid_ctl_thr(void);
+
+void clear_rate_i_item(struct pid_method_rpy *method);
+void init_attitude_head(void);
+
+#endif

controller_yy_app/controlware/control_inc/control_throttle.h

@@ -0,0 +1,179 @@
+
+#ifndef __CONTROL_THROTTLE_H
+#define __CONTROL_THROTTLE_H
+
+#include <stdbool.h>
+#include <stdint.h>
+#include "control_rate.h"
+
+#define HOVER_THR 1300
+// 解锁限制条状枚举类
+enum
+{
+    // 允许解锁
+    UNLOCK_ALLOW = 0,
+    // 不允许解锁，IMU 通讯错包
+    UNLOCK_DMAERR = 1,
+    // 不允许解锁，速度超 0.4m/s
+    UNLOCK_VELERR = 2,
+    // 不允许解锁，遥控器拨杆位置错误
+    UNLOCK_RCERR = 3,
+    // 不允许解锁，未收到IMU数据
+    UNLOCK_NOIMU = 4,
+    // 不允许解锁，GPS状态标志错误
+    UNLOCK_GPSERR = 5,
+    // 不允许解锁，陀螺加速度计状态标志错误, 或加速度超 0.8m/s^2
+    UNLOCK_ACCERR = 6,
+    // 不允许解锁，未INS定位
+    UNLOCK_NOINSGPS = 7,
+    // 不允许解锁，无RTK
+    UNLOCK_NORTK = 8,
+    // 不允许解锁，磁状态标志错误
+    UNLOCK_MAGERROR = 9,
+    // 不允许解锁，陀螺超10deg/s
+    UNLOCK_GYROERROR = 10,
+    // 不允许解锁，温度超过80度
+    UNLOCK_OVER_TEMPRATURE = 11,
+    // 不允许解锁，imu版本不匹配
+    UNLOCK_IMU_VERSION_ERR = 12,
+    // 不允许解锁，在禁飞区内
+    UNLOCK_IN_NO_FLY_ZONE = 13,
+    // 不允许解锁, 电压低
+    UNLOCK_VOLT_LOW = 14,
+    // 不允许解锁, 智能电池 BMS 电量低
+    UNLOCK_BMS_LOW_CAPACITY = 15,
+    // 不允许解锁, 智能电池 BMS 故障
+    UNLOCK_BMS_FAULT = 16,
+    // 不允许解锁, 智能电池 BMS 通讯异常
+    UNLOCK_BMS_LINK_LOST = 17,
+    // 不允许解锁, 有断桨故障
+    UNLOCK_SERVO_FAILSAFE = 18,
+    // 不允许解锁, 姿态倾斜超 30 度
+    UNLOCK_LEAN_ANGLE = 19,
+    // 不允许解锁, SD 卡故障
+    UNLOCK_SD_FAIL = 20,
+};
+
+// 油门上锁原因枚举类
+enum
+{
+    //初始状态
+    DEFLOCK = 0,
+    //遥控器主动上锁
+    RCLOCK = 1,
+    //遥控器自动上锁
+    RCAUTOLOCK = 2,
+    //摇杆主动上锁
+    ROCKLOCK = 3,
+    //摇杆自动上锁
+    ROCKAUTOLOCK = 4,
+    //摇杆弹起上锁
+    ROCKUPLOCK = 5,
+    //返航上锁
+    RTHLOCK = 6,
+    //角度上锁
+    ANGLELOCK = 7,
+    //电台丢失上锁
+    LINKLOSTLOCK = 8,
+    //自动起飞超时上锁
+    LAUNCHOTLOCK = 9,
+    //丢失IMU上锁
+    NOIMULOCK = 10,
+    //一键上锁
+    ONKEYLOCK = 11,
+    //降落上锁
+    LANDLOCK = 12,
+    //丢星姿态降落上锁
+    GPSFAILLOCK = 13,
+    //遥控器错误上锁
+    RCBADLOCK = 14,
+    // 车载降落上锁
+    VEHICLELAND_LOCK = 15,
+    // 地面站强制上锁
+    GCSFORCELOCK = 16,
+    // 开伞上锁
+    PARACHUTELOCK = 17,
+};
+
+// 解锁原因枚举类
+enum
+{
+    // 初始状态
+    DEFUNLOCK = 0,
+    // 遥控器解锁
+    RCUNLOCK = 1,
+    // 摇杆解锁
+    ROCKUNLOCK = 2,
+    // 自动起飞解锁
+    LAUNCHUNLOCK = 3,
+    // 车载起飞解锁
+    VEHICLELAUNCH_UNLOCK = 4,
+};
+
+// 油门锁状态枚举类
+enum
+{
+    //上锁状态
+    LOCKED = 1,
+    //解锁状态
+    UNLOCKED = 2,
+    //起飞状态-给IMU发送指令使用
+    TAKEOFF = 3
+};
+
+// 离地状态枚举类
+enum
+{
+    //在地上
+    ON_GROUND = 1,
+    //在空中
+    IN_AIR = 2,
+};
+
+// 油门控制状态
+typedef enum
+{
+    THROTTLE_OFF = 0,
+    THROTTLE_ONGROUND_IDLE = 1,
+    THROTTLE_INAIR_RUNNING = 2
+} ThrCtrolStatusType;
+extern ThrCtrolStatusType throttle_pidctrl_status;
+
+// 定高状态标志
+enum
+{
+    ALT_HOLD = 1,
+    NO_ALT_HOLD = 2
+};
+extern char althold_state;
+
+extern bool unlock_fail;
+
+extern char thr_lock_flag;
+extern char thr_unlock_flag;
+
+extern char thr_lock_status;
+extern char ground_air_status;
+
+void pilot_unlock_decide(void);
+void pilot_takeoff_decide(void);
+void pilot_lock_decide(void);
+
+void get_pilot_desired_climb_rate_fromrc(short thr_in);
+
+void Calculate_Target_Az(float dt);
+
+void rate_pid_acctothrottle_alt(float dt);
+
+extern float base_thr, temp_basethr;
+
+void set_thr_lock_status(uint8_t lock_status, uint8_t reason);
+
+int judge_pilot_gsunlock_allow(void);
+int judge_pilot_rcunlock_allow(void);
+
+void throttle_enable_unlock_tune(void);
+bool throttle_get_enable_unlock_tune(void);
+
+void clear_poshold_i_item(struct pid_method_ap *method);
+#endif

controller_yy_app/controlware/control_inc/mode_attitude.h

@@ -0,0 +1,32 @@
+
+#ifndef __MODE_ATTITUDE_H
+#define __MODE_ATTITUDE_H
+
+#include "flt_butter.h"
+#include "lowpass_filter.h"
+#include "stdbool.h"
+
+struct controller_xy
+{
+    float pc[2];            /* 当前位置  m */
+    float vc[2];            /* 当前速度  m/s */
+    float ac[2];            /* 当前加速度 m/s^2 */
+
+    float pt[2];             /* 目标位置  m */
+    float at[2];             /* 目标加速度 m/s^2 */
+    float vt[2];             /* 目标速度  m/s */
+    float ve_integ[2];       /* 速度误差积分 */
+    LpfButter2 vt_ff_flt[2]; /* 目标速度微分前馈滤波器 */
+
+    float pos_integ[2];     /* 位置误差积分 */
+
+    bool reset; /* 控制器重置 */
+};
+extern struct controller_xy _cxy;
+extern float dist_comp[2];
+
+bool attitude_init(void);
+void attitude_run(void);
+void attitude_exit(void);
+
+#endif

controller_yy_app/controlware/control_inc/mode_gcs_tax_launch_run.h

@@ -0,0 +1,28 @@
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+enum
+{
+    __STATE_CHECK_ACC_OVERLOAD = 0, /* 检测加速度过载 */
+    __STATE_SELF_STABILITY = 1,     /* 起飞自稳 */
+    __STATE_LAUNCH_GO = 2,          /* 起飞 */
+};
+
+enum
+{
+   __WAVE_FORWARD = 1,
+   __WAVE_REVERSE = 0,
+};
+
+extern uint8_t __mode_state;  /* 模式运行阶段 */
+extern uint32_t __time_stamp; /* 定时计数 */
+extern float record_target_alt;
+extern uint16_t angle_plan_run_cnt;
+bool gcs_tax_launch_init(void);
+
+void gcs_tax_launch_run(float dt);
+
+void gcs_tax_launch_exit(void);
+

controller_yy_app/controlware/control_inc/mode_vehicle_land.h

@@ -0,0 +1,9 @@
+#ifndef _MODE_VEHICLE_LAND_H_
+#define _MODE_VEHICLE_LAND_H_
+
+#include <stdbool.h>
+
+
+
+
+#endif

controller_yy_app/controlware/control_inc/mode_vehicle_launch.h

@@ -0,0 +1,9 @@
+#ifndef _MODE_VEHICLE_LAUNCH_H_
+#define _MODE_VEHICLE_LAUNCH_H_
+
+#include <stdbool.h>
+
+bool vehicle_launch_init(void);
+
+
+#endif

controller_yy_app/controlware/control_rate.c

@@ -0,0 +1,224 @@
+#include "control_throttle.h"
+#include "flight_mode.h"
+#include "helpler_funtions.h"
+#include "lowpass_filter.h"
+#include "my_math.h"
+#include "params.h"
+#include "soft_gs.h"
+#include "soft_motor_output.h"
+#include "soft_rc_input.h"
+#include "soft_time.h"
+#include "soft_imu.h"
+#include "control_rate.h"
+#include <math.h>
+
+// malloc测试，定义结构体指针的话必须要malloc申请空间，
+// 否则只会分配4字节的指针空间，不会自动分配结构体内存空间，导致无法计算以及赋值
+// 所以决定定义变量时都定义为变量，不定义指针，
+struct pid_method_rpy pid_m_roll;
+struct pid_method_rpy pid_m_pitch;
+struct pid_method_rpy pid_m_yaw;
+struct pid_method_ap pid_m_alt;
+struct pid_method_ap pid_m_posx;
+struct pid_method_ap pid_m_posy;
+
+struct pid_value_rpy pid_v_roll;
+struct pid_value_rpy pid_v_pitch;
+struct pid_value_rpy pid_v_yaw;
+struct pid_value_ap pid_v_alt;
+struct pid_value_ap pid_v_pos;
+
+// 陀螺积分的虚拟机头方向
+float attitude_head = 0.0f;
+
+/*
+内环的速率控制函数
+*/
+float rate_pid_ctl_rp(struct pid_method_rpy *method, struct pid_value_rpy *value, float dt)
+{
+    // 计算目标角速度与当前角度的差
+    method->gyro_e = method->gyro_t - method->gyro_c;
+
+    if (method->gyro_filter_ready != true)
+    {
+        method->gyro_filter_ready = true;
+        butter2_filter_init(&method->gyro_dirv_filter, 400, 25);
+        // butter2_filter_init(&method->gyro_dirv_filter, 400, 20);
+        method->gyro_d_item = 0.f;
+    }
+    else
+    {
+        float dirv = 0.f;
+        if (dt > 0.f)
+        {
+            dirv = value->gyro_d * (method->gyro_e - method->gyro_e_last) / dt;
+            dirv = constrain_float(dirv, -120, 120);
+            method->gyro_e_last = method->gyro_e;
+        }
+
+        method->gyro_d_item = butter2_filter_apply(&method->gyro_dirv_filter, dirv);
+    }
+
+    if (ground_air_status == ON_GROUND)
+    {
+        method->gyro_d_item = 0.f;
+        butter2_filter_reset(&method->gyro_dirv_filter, 0);
+    }
+
+    // 计算角速度的P值
+    method->gyro_p_item = constrain_float(method->gyro_e, -250, 250) * value->gyro_p;
+
+    method->gyro_i_item += method->gyro_e * dt * value->gyro_i * 10.0f;
+    // 角速度的I值得限幅
+    method->gyro_i_item = constrain_float(method->gyro_i_item, -value->gyro_imax, value->gyro_imax);
+
+   
+    method->pid_out = method->gyro_p_item + method->gyro_i_item + method->gyro_d_item;
+
+    if ((control_mode == CONTROL_REMOTE) &&
+        (rc_signal_health == RC_SIGNAL_HEALTH) && (rc_in[RC_CH1] < 1050) &&
+        
+        (rc_in[RC_CH4] > 1950) && (rc_in[RC_CH3] < 1050) &&
+        (rc_in[RC_CH2] > 1950))
+        method->pid_out = 0.0f;
+
+    return method->pid_out;
+}
+
+/*
+内环的速率控制函数
+*/
+float rate_pid_ctl_yaw(struct pid_method_rpy *method,
+                       struct pid_value_rpy *value, float dt)
+{
+    // 如果电机输出有饱和，则目标航向角速度指数递减
+    if (MotorOutput_GetYawRestrictionStatus())
+    {
+        pid_m_yaw.gyro_t = 0.5f * pid_m_yaw.gyro_c;
+    }
+
+    // 计算目标角速度与当前角度的差
+    method->gyro_e = method->gyro_t - method->gyro_c;
+
+    if (method->gyro_filter_ready != true)
+    {
+        method->gyro_filter_ready = true;
+        butter2_filter_init(&method->gyro_dirv_filter, 400, 30);
+        method->gyro_d_item = 0.f;
+    }
+    else
+    {
+        float dirv = 0.f;
+        if (dt > 0.f)
+        {
+            dirv = value->gyro_d * (method->gyro_e - method->gyro_e_last) / dt;
+            dirv = constrain_float(dirv, -100, 100);
+            method->gyro_e_last = method->gyro_e;
+        }
+
+        method->gyro_d_item = butter2_filter_apply(&method->gyro_dirv_filter, dirv);
+    }
+
+    if (ground_air_status == ON_GROUND)
+    {
+        method->gyro_d_item = 0.f;
+        butter2_filter_reset(&method->gyro_dirv_filter, 0);
+    }
+
+    // 角速度差限幅
+    method->gyro_e = constrain_float(method->gyro_e, -200.0f, 200.0f);
+
+    // 计算角速度的P值
+    method->gyro_p_item = method->gyro_e * value->gyro_p;
+
+    // 计算角速度的I值
+    method->gyro_i_item += method->gyro_e * dt * value->gyro_i * 10.0f;
+    // 角速度的I值得限幅
+
+    if (MotorOutput_GetYawRestrictionStatus())
+    {
+        method->gyro_i_item = constrain_float( method->gyro_i_item, -2.0f * value->gyro_imax, 2.0f * value->gyro_imax);
+    }
+    else
+    {
+        method->gyro_i_item = constrain_float(method->gyro_i_item, -value->gyro_imax, value->gyro_imax);
+    }
+
+    method->pid_out =
+        method->gyro_p_item + method->gyro_i_item + method->gyro_d_item;
+    method->pid_out = constrain_float(method->pid_out, -300.0f, +300.0f);
+
+    if ((control_mode == CONTROL_REMOTE) &&
+        (rc_signal_health == RC_SIGNAL_HEALTH) && (rc_in[RC_CH1] < 1050) &&
+        (rc_in[RC_CH4] > 1950) && (rc_in[RC_CH3] < 1050) &&
+        (rc_in[RC_CH2] > 1950))
+        method->pid_out = 0.0f;
+
+    return method->pid_out;
+}
+extern float thr_max; // 前面上升阶段为了减小电流 限制油门最大为 1400 
+/*
+内环获取油门量
+*/
+float rate_pid_ctl_thr(void)
+{
+    float throttle_ctrl_value = 1000.0f;
+
+    /* 根据油门控制状态标志，选择油门输出是 灭车、控制量、怠速 */
+    switch (throttle_pidctrl_status)
+    {
+    case THROTTLE_INAIR_RUNNING:
+        if ((rc_in[RC_CH8] < 1500) && (comp_rc_status == COMP_NORMAL) &&
+            (rc_signal_health == RC_SIGNAL_HEALTH) &&
+            (control_mode == CONTROL_REMOTE))
+        {
+            throttle_ctrl_value = rc_in[RC_CH3];
+            throttle_ctrl_value = constrain_float(throttle_ctrl_value, 1000.0f, 2000.0f);
+            base_thr = throttle_ctrl_value;
+            pid_m_alt.acc_i_item = 0;
+        }
+        else
+        {
+            if (base_thr < HOVER_THR) // 电机怠速是默认等级2 怠速油门10 基础油门宏默认是应该给到1300 如果后续需要响应高度需要变换 
+                base_thr += 1.0f;
+
+            throttle_ctrl_value = base_thr + pid_m_alt.pid_out;
+            throttle_ctrl_value = constrain_float(throttle_ctrl_value, 1000.0f, thr_max);
+        }
+        break;
+
+    case THROTTLE_ONGROUND_IDLE:
+        if ((rc_in[RC_CH8] < 1500) && (comp_rc_status == COMP_NORMAL) &&
+            (rc_signal_health == RC_SIGNAL_HEALTH) &&
+            (control_mode == CONTROL_REMOTE))
+        {
+            throttle_ctrl_value = rc_in[RC_CH3];
+            base_thr = throttle_ctrl_value;
+            pid_m_alt.acc_i_item = 0;
+        }
+        else
+        {
+            throttle_ctrl_value = conf_par.idle_speed + 25.0f;
+        }
+        break;
+
+    case THROTTLE_OFF:
+    default:
+        break;
+    }
+    return throttle_ctrl_value;
+}
+
+/*
+清零积分项
+*/
+void clear_rate_i_item(struct pid_method_rpy *method)
+{
+    method->gyro_i_item = 0.0f;
+}
+
+void init_attitude_head(void)
+{
+    attitude_head = pid_m_yaw.angle_c;
+    pid_m_yaw.angle_t = pid_m_yaw.angle_c;
+}

controller_yy_app/controlware/control_throttle.c

@@ -0,0 +1,612 @@
+#include "control_throttle.h"
+#include "auto_pilot.h"
+#include "common.h"
+#include "control_attitude.h"
+#include "control_rate.h"
+#include "flight_mode.h"
+#include "hard_imu_uart3.h"
+#include "helpler_funtions.h"
+#include "lowpass_filter.h"
+#include "my_crc.h"
+#include "my_math.h"
+#include "params.h"
+#include "pilot_navigation.h"
+#include "soft_flash.h"
+#include "soft_gps.h"
+#include "soft_gs.h"
+#include "soft_imu.h"
+#include "soft_motor_output.h"
+#include "soft_port_uart4.h"
+#include "soft_rc_input.h"
+#include "soft_sdcard.h"
+#include "soft_time.h"
+#include "soft_voltage.h"
+#include "ver_config.h"
+// #include "control_poshold.h"
+#include <math.h>
+
+/* 油门控制输出状态 */
+ThrCtrolStatusType throttle_pidctrl_status = THROTTLE_OFF;
+
+/* 是否允许调试解锁, 若允许则会禁用部分解锁条件限制 */
+bool _enable_unlock_tune = false;
+
+void throttle_enable_unlock_tune(void) { _enable_unlock_tune = true; }
+
+bool throttle_get_enable_unlock_tune(void)
+{
+    return _enable_unlock_tune;
+}
+
+/**
+ * @brief 是否允许地面站指令解锁
+ *
+ * @return int
+ */
+int judge_pilot_gsunlock_allow(void)
+{
+    if (thr_lock_status == UNLOCKED)
+        return UNLOCK_ALLOW;
+
+    // imu 数据错误
+    if (imu_link_status != COMP_NORMAL)
+    {
+        return UNLOCK_NOIMU;
+    }
+
+    // imu 通讯错误包
+    if (dma_iserror == true)
+    {
+
+        return UNLOCK_DMAERR;
+    }
+
+    // 无解算定位
+    if (ins.insgps_ok != INSGPS)
+    {
+        return UNLOCK_NOINSGPS;
+    }
+
+    // GPS 状态标志位异常不允许解锁
+    if ((raw_gps._gps_status & GPS_STATUS1_ERROR_MASK) ||
+        (raw_gps._gps_status2 & GPS_STATUS2_ERROR_MASK))
+    {
+        return UNLOCK_GPSERR;
+    }
+
+    // IMU 磁状态异常
+    if (raw_gps._mag_status & MAG_STATUS_ERROR_MASK)
+    {
+        return UNLOCK_MAGERROR;
+    }
+
+    // 动力异常
+    if (Motor_GetFailsafeNum() > 0 && _enable_unlock_tune == false)
+    {
+        return UNLOCK_SERVO_FAILSAFE;
+    }
+
+    // IMU 陀螺加速度计异常
+    if (raw_gps._acc_gyro_status & ACCGYRO_STATUS_ERROR_MASK)
+    {
+        return UNLOCK_ACCERR;
+    }
+
+    // 速度超范围不允许解锁
+    if (thr_lock_status == LOCKED && ins.insgps_ok == INSGPS &&
+        (fabsf(pid_m_posx.vel_c) > 40.0f || fabsf(pid_m_posy.vel_c) > 40.0f ||
+         fabsf(pid_m_alt.vel_c) > 40.0f))
+    {
+        return UNLOCK_VELERR;
+    }
+
+    // 加计超范围不允许解锁
+    if (thr_lock_status == LOCKED &&
+        (fabsf(pid_m_posx.acc_c) > 80.0f || fabsf(pid_m_posy.acc_c) > 80.0f ||
+         fabsf(pid_m_alt.acc_c) > 80.0f))
+    {
+        return UNLOCK_ACCERR;
+    }
+
+    // 陀螺超范围不允许解锁
+    if (thr_lock_status == LOCKED &&
+        (fabsf(pid_m_roll.gyro_c) > 10.0f ||
+         fabsf(pid_m_pitch.gyro_c) > 10.0f || fabsf(pid_m_yaw.gyro_c) > 10.0f))
+    {
+        return UNLOCK_GYROERROR;
+    }
+
+    // 姿态倾斜超范围不允许解锁
+    if (thr_lock_status == LOCKED && (fabsf(pid_m_roll.angle_c) > 30.0f ||
+                                      fabsf(pid_m_pitch.angle_c) > 30.0f))
+    {
+        return UNLOCK_LEAN_ANGLE;
+    }
+
+    // 温度超范围不允许解锁
+    if (thr_lock_status == LOCKED && ins.temprature > 80)
+    {
+        return UNLOCK_OVER_TEMPRATURE;
+    }
+
+    // 电压低不允许解锁
+    if (battary_volt_islow)
+    {
+        return UNLOCK_VOLT_LOW;
+    }
+
+    // 智能电池故障不允许解锁
+    const VoltageBms *bms = Voltage_GetBmsInfo();
+    if (bms->link_status == COMP_NORMAL)
+    {
+        if (bms->electricity_percentage < params_get_value(ParamNum_BmsLowCapcityPercentage))
+        {
+            return UNLOCK_BMS_LOW_CAPACITY;
+        }
+
+        if (bms->fault_status >= BMS_FAULT_SERIOUS)
+        {
+            return UNLOCK_BMS_FAULT;
+        }
+    }
+    else if (bms->link_status == COMP_LOST)
+    {
+        return UNLOCK_BMS_LINK_LOST;
+    }
+
+    // sd 卡故障不允许解锁
+    if (sdcard_initok() == 0)
+    {
+        return UNLOCK_SD_FAIL;
+    }
+
+    return UNLOCK_ALLOW;
+}
+
+/**
+ * @brief 是否允许遥控器解锁
+ *
+ * @return int
+ */
+int judge_pilot_rcunlock_allow(void)
+{
+    if (thr_lock_status == UNLOCKED)
+        return UNLOCK_ALLOW;
+    // 遥控器位置不对
+    if ((rc_in[RC_CH6] < MAX_S || rc_in[RC_CH6] > MAX_E ||
+         rc_in[RC_CH7] < MAX_S || rc_in[RC_CH7] > MAX_E) &&
+        thr_lock_status == LOCKED && comp_rc_status == COMP_NORMAL)
+    {
+        return UNLOCK_RCERR;
+    }
+
+    // imu 数据错误
+    if (imu_link_status != COMP_NORMAL)
+    {
+        return UNLOCK_NOIMU;
+    }
+
+    // imu 通讯错误包
+    if (dma_iserror == true)
+    {
+        return UNLOCK_DMAERR;
+    }
+
+    // GPS 状态标志位异常不允许解锁
+    if (((raw_gps._gps_status & GPS_STATUS1_ERROR_MASK) ||
+         (raw_gps._gps_status2 & GPS_STATUS2_ERROR_MASK)) &&
+        _enable_unlock_tune == false)
+    {
+        return UNLOCK_GPSERR;
+    }
+
+    // IMU 磁状态异常
+    if ((raw_gps._mag_status & MAG_STATUS_ERROR_MASK) &&
+        _enable_unlock_tune == false)
+    {
+        return UNLOCK_MAGERROR;
+    }
+
+    if (Motor_GetFailsafeNum() > 0 && _enable_unlock_tune == false)
+    {
+        return UNLOCK_SERVO_FAILSAFE;
+    }
+
+    // IMU 陀螺加速度计异常
+    if (raw_gps._acc_gyro_status & ACCGYRO_STATUS_ERROR_MASK)
+    {
+        return UNLOCK_ACCERR;
+    }
+
+    // 速度超范围不允许解锁
+    if (thr_lock_status == LOCKED && ins.insgps_ok == INSGPS &&
+        (fabsf(pid_m_posx.vel_c) > 40.0f || fabsf(pid_m_posy.vel_c) > 40.0f ||
+         fabsf(pid_m_alt.vel_c) > 40.0f) &&
+        _enable_unlock_tune == false)
+    {
+        return UNLOCK_VELERR;
+    }
+
+    // 加计超范围不允许解锁
+    if (thr_lock_status == LOCKED &&
+        (fabsf(pid_m_posx.acc_c) > 80.0f || fabsf(pid_m_posy.acc_c) > 80.0f ||
+         fabsf(pid_m_alt.acc_c) > 80.0f) &&
+        _enable_unlock_tune == false)
+    {
+        return UNLOCK_ACCERR;
+    }
+
+    // 陀螺超范围不允许解锁
+    if (thr_lock_status == LOCKED &&
+        (fabsf(pid_m_roll.gyro_c) > 10.0f ||
+         fabsf(pid_m_pitch.gyro_c) > 10.0f ||
+         fabsf(pid_m_yaw.gyro_c) > 10.0f) &&
+        _enable_unlock_tune == false)
+    {
+        return UNLOCK_GYROERROR;
+    }
+
+    // 姿态倾斜超范围不允许解锁
+    if (thr_lock_status == LOCKED && (fabsf(pid_m_roll.angle_c) > 30.0f ||
+                                      fabsf(pid_m_pitch.angle_c) > 30.0f))
+    {
+        return UNLOCK_LEAN_ANGLE;
+    }
+
+    // 温度超范围不允许解锁
+    if (thr_lock_status == LOCKED && ins.temprature > 80)
+    {
+        return UNLOCK_OVER_TEMPRATURE;
+    }
+
+    // 电压低不允许解锁
+    if (battary_volt_islow && _enable_unlock_tune == false)
+    {
+        return UNLOCK_VOLT_LOW;
+    }
+
+    const VoltageBms *bms = Voltage_GetBmsInfo();
+    if (bms->link_status == COMP_NORMAL)
+    {
+        if (bms->electricity_percentage < params_get_value(ParamNum_BmsLowCapcityPercentage) && _enable_unlock_tune == false)
+        {
+            return UNLOCK_BMS_LOW_CAPACITY;
+        }
+
+        if (bms->fault_status >= BMS_FAULT_SERIOUS && _enable_unlock_tune == false)
+        {
+            return UNLOCK_BMS_FAULT;
+        }
+    }
+    else if (bms->link_status == COMP_LOST && _enable_unlock_tune == false)
+    {
+        return UNLOCK_BMS_LINK_LOST;
+    }
+
+    // sd卡故障不允许解锁
+    if (sdcard_initok() == 0 && _enable_unlock_tune == false)
+    {
+        return UNLOCK_SD_FAIL;
+    }
+
+    return UNLOCK_ALLOW;
+}
+
+#define PILOT_RCUNLOCK_PERIOD 200000
+unsigned int pilot_rcunlock_time = 0, pilot_rcunlock_delay = 0;
+
+// 上锁方式的标志位
+char thr_lock_flag = DEFLOCK;
+// 解锁方式的标志位
+char thr_unlock_flag = DEFUNLOCK;
+
+// 上锁状态位
+char thr_lock_status = LOCKED;
+// 起飞状态位
+char ground_air_status = ON_GROUND;
+
+void pilot_unlock_decide(void)
+{
+    int rcunlock_allow_flag = judge_pilot_rcunlock_allow();
+
+    // 遥控器模式下
+    if (control_mode == CONTROL_REMOTE && rc_signal_health == RC_SIGNAL_HEALTH)
+    { // 遥控器解锁
+        if (thr_lock_status == LOCKED && rcunlock_allow_flag == UNLOCK_ALLOW &&
+            (rc_in[RC_CH1] > 1950) && (rc_in[RC_CH2] > 1950) &&
+            (rc_in[RC_CH3] < 1050) && (rc_in[RC_CH4] < 1050))
+        {
+            pilot_rcunlock_delay += micros() - pilot_rcunlock_time;
+            pilot_rcunlock_time = micros();
+
+            if (pilot_rcunlock_delay > PILOT_RCUNLOCK_PERIOD)
+            {
+                pilot_rcunlock_delay = 0;
+
+                set_thr_lock_status(UNLOCKED, RCUNLOCK);
+
+                // 解锁成功
+                if (thr_lock_status == UNLOCKED)
+                {
+                    pid_m_yaw.angle_t = pid_m_yaw.angle_c;
+
+                    // 每次解锁清气压,清零气压由控制端来完成，因为给IMU发命令会导致DMA错误
+                    unclock_clear_altitude();
+
+                    // 初始化定点参数
+                    clear_poshold_i_item(&pid_m_posx);
+                    clear_poshold_i_item(&pid_m_posy);
+                    clear_poshold_i_item(&pid_m_alt);
+                }
+                else
+                {
+                }
+            }
+        }
+        else
+        {
+            pilot_rcunlock_delay = 0;
+            pilot_rcunlock_time = micros();
+        }
+    }
+    else if (control_mode == CONTROL_GCS)
+    {
+    }
+}
+
+/*
+飞行器起飞识别
+*/
+
+// 基础油门
+float base_thr = 1000; // HOVER_THR; 
+void pilot_takeoff_decide(void)
+{
+    // 遥控器 姿态模式/定点模式 下起飞判断
+    if (control_mode == CONTROL_REMOTE &&
+        (flight_mode == ATTITUDE || flight_mode == GPS_POSHOLD) &&
+        thr_lock_status == UNLOCKED && ground_air_status == ON_GROUND)
+    {
+        if (rc_in[RC_CH3] <= (1200 + RC_DEAD_ZONE))
+        {
+            record_position(&pid_m_posx.loc_t, &pid_m_posy.loc_t,
+                            pid_m_posx.loc_c, pid_m_posy.loc_c);
+            clear_poshold_i_item(&pid_m_posx);
+            clear_poshold_i_item(&pid_m_posy);
+            clear_poshold_i_item(&pid_m_alt);
+            // 解锁后没有推油门的落锁设置在解锁判断里面
+        }
+        else if (rc_in[RC_CH3] > (1200 + RC_DEAD_ZONE))
+        {
+            ground_air_status = IN_AIR;
+            throttle_pidctrl_status = THROTTLE_INAIR_RUNNING;
+        }
+    }
+}
+
+/**
+ * @brief 上锁识别 TODO:遥控器上锁识别放到手动飞行模式中去
+ *
+ */
+void pilot_lock_decide(void)
+{
+    static uint32_t remote_rclock_time = 0, remote_rclock_delay = 0;
+
+    // 如果是在遥控器模式下
+    if (control_mode == CONTROL_REMOTE && rc_signal_health == RC_SIGNAL_HEALTH)
+    {
+        // 如果已经解锁
+        if (thr_lock_status == UNLOCKED)
+        {
+            if (flight_mode != GPS_RTH)
+            {
+                // 检测遥控器主动操作上锁
+                if ((rc_in[RC_CH1] < 1050) && (rc_in[RC_CH4] > 1950) &&
+                    (rc_in[RC_CH3] < 1050) && (rc_in[RC_CH2] > 1950))
+                {
+                    remote_rclock_delay += micros() - remote_rclock_time;
+                    remote_rclock_time = micros();
+
+                    if (remote_rclock_delay > 0.2f * 1000000)
+                    {
+                        remote_rclock_delay = 0;
+
+                        set_thr_lock_status(LOCKED, RCLOCK);
+                    }
+                }
+                else
+                {
+                    remote_rclock_delay = 0;
+                    remote_rclock_time = micros();
+                }
+            }
+        }
+        // 如果已经上锁
+        else
+        {
+            // 清零计时变量
+            remote_rclock_delay = 0;
+            remote_rclock_time = micros();
+        }
+    }
+}
+
+/*
+通过遥控器的值获取期望爬升速度
+*/
+char althold_state = NO_ALT_HOLD;
+
+// 有个±5的余量，避免垂直速度方向有偏差导致小速度时无法正确执行。15太大了。
+void get_pilot_desired_climb_rate_fromrc(short thr_in)
+{
+    float desired_rate = 0.0f; // cm/s
+    static unsigned int stop_time = 0;
+
+    // ensure a reasonable throttle value
+    thr_in = constrain_int16(thr_in, 1000, 2000);
+
+    if (rc_in[RC_CH8] < 1500)
+    {
+        althold_state = NO_ALT_HOLD;
+        pid_m_alt.loc_t = pid_m_alt.loc_c;
+        pid_m_alt.vel_t = 0;
+        return;
+    }
+
+    // check throttle is above, below or in the deadband
+    if (thr_in < 1500 - RC_DEAD_ZONE)
+    {
+        // below the deadband
+        desired_rate = -10 + ((parinf._par_maxdownspeed * 10) * (thr_in - (1500 - RC_DEAD_ZONE)) / 450.0f);
+
+        althold_state = NO_ALT_HOLD;
+        pid_m_alt.vel_t = desired_rate;
+        stop_time = 0;
+    }
+    else if (thr_in > 1500 + RC_DEAD_ZONE)
+    {
+        // above the deadband
+        desired_rate = 20 + ((parinf._par_maxupspeed * 10) * (thr_in - (1500 + RC_DEAD_ZONE)) / 450.0f);
+
+        althold_state = NO_ALT_HOLD;
+        pid_m_alt.vel_t = desired_rate;
+        stop_time = 0;
+    }
+    else
+    {
+        if (althold_state != ALT_HOLD)
+        {
+            pid_m_alt.vel_t = 0.0f;
+
+            if (fabsf(pid_m_alt.vel_c) > 30.0f)
+                stop_time = micros();
+
+            if ((unsigned int)(micros() - stop_time > 1500000)) // 1.5s定高
+            {
+                althold_state = ALT_HOLD;
+                pid_m_alt.loc_t = pid_m_alt.loc_c;
+            }
+        }
+    }
+}
+
+/**************************实现函数********************************************
+ *函数原型:		void Acc_AltHold_Control()
+ *功　　能:	    高度控制
+ *******************************************************************************/
+
+void Calculate_Target_Az(float dt)
+{
+    // 如果是定高状态，则将高度差计算在内，否则置0
+    if (althold_state == ALT_HOLD)
+    {
+        pid_m_alt.vel_t = cal_tar_vel_z(pid_m_alt.loc_t, pid_m_alt.loc_c, 400, 400);
+
+        // 计算垂直速度差
+        pid_m_alt.vel_e = pid_m_alt.vel_t - pid_m_alt.vel_c;
+        // 速度的差需要放开，这样从升降速切换到定高时能快速定住
+        pid_m_alt.vel_e = constrain_float(pid_m_alt.vel_e, -500.0f, 500.0f);
+    }
+    else if (althold_state == NO_ALT_HOLD)
+    {
+        // 计算垂直速度差
+        pid_m_alt.vel_e = pid_m_alt.vel_t - pid_m_alt.vel_c;
+        // 速度的差需要放开，这样从升降速切换到定高时能快速定住
+        pid_m_alt.vel_e = constrain_float(pid_m_alt.vel_e, -500.0f, 500.0f);
+    }
+
+    pid_m_alt.vel_p_item = pid_m_alt.vel_e * pid_v_alt.speed_p;
+    pid_m_alt.acc_t = pid_m_alt.vel_p_item;
+}
+
+/*
+高度通道加速度pi到油门
+*/
+void rate_pid_acctothrottle_alt(float dt)
+{
+    pid_m_alt.acc_t = constrain_float(pid_m_alt.acc_t, -400.0f, 400.0f);
+    pid_m_alt.acc_e = pid_m_alt.acc_t - pid_m_alt.acc_c;
+    pid_m_alt.acc_e = constrain_float(pid_m_alt.acc_e, -500.0f, 500.0f);
+
+    //----------P--------------------
+    // 加速度差转换为电机输出量 ,系数为0.4
+    pid_m_alt.acc_p_item = pid_m_alt.acc_e * pid_v_alt.acc_p;
+
+    //------------------I-----------
+    // 加速度差的积分，系数为1.5，1m/s^2的加速度差1s积分1500
+    pid_m_alt.acc_i_item += dt * pid_v_alt.acc_i * constrain_float(pid_m_alt.acc_e, -100.0f, +100.0f) * 10.0f;
+
+    pid_m_alt.acc_i_item = constrain_float(pid_m_alt.acc_i_item, 1000 - conf_par.idle_speed, 1650 - base_thr); // -100 300
+
+    // 在地上的时候积分不累加
+    if (ground_air_status != IN_AIR)
+    {
+        pid_m_alt.acc_i_item = 0.0f;
+    }
+
+    // 油门输出量
+    pid_m_alt.pid_out = pid_m_alt.acc_p_item + pid_m_alt.acc_i_item;
+
+    // 去掉Z轴声音会变大
+    pid_m_alt.pid_out = pid_m_alt.pid_out_last + constrain_float((pid_m_alt.pid_out - pid_m_alt.pid_out_last), -10.0f, 10.0f);
+    pid_m_alt.pid_out_last = pid_m_alt.pid_out;
+
+    pid_m_alt.pid_out = constrain_float(pid_m_alt.pid_out, -200.0f, 600.0f);
+    // pid_m_alt.pid_out = constrain_float(pid_m_alt.pid_out, 0.0f, 600.0f); // 抗冲击必须有
+    // 如果监测到可能在地面，则油门控制 pid 量清空
+    if (throttle_pidctrl_status == THROTTLE_OFF ||
+        throttle_pidctrl_status == THROTTLE_ONGROUND_IDLE)
+    {
+        pid_m_alt.pid_out = 0.0f;
+        pid_m_alt.acc_i_item = 0.0f;
+    }
+
+    if (pid_m_alt.thr_hold == true)
+    {
+        pid_m_alt.acc_i_item = 100;
+        pid_m_alt.pid_out_last = 100;
+        pid_m_alt.pid_out = 100;
+       
+    }
+}
+
+bool unlock_fail = false;
+
+void set_thr_lock_status(uint8_t lock_status, uint8_t reason)
+{
+    if (lock_status == LOCKED)
+    {
+        thr_lock_status = LOCKED;
+        ground_air_status = ON_GROUND;
+
+        throttle_pidctrl_status = THROTTLE_OFF;
+
+        notify_imu_lock = true;
+        thr_lock_flag = reason;
+
+        wp_have_cycle_times = 0;
+    }
+    else if (lock_status == UNLOCKED)
+    {
+        notify_imu_unlock = true;
+        {
+            thr_lock_status = UNLOCKED;
+            ground_air_status = ON_GROUND;
+
+            thr_unlock_flag = reason;
+            throttle_pidctrl_status = THROTTLE_ONGROUND_IDLE;
+            unlock_fail = false;
+        }
+
+        if (thr_lock_status == LOCKED)
+        {
+            unlock_fail = true;
+        }
+    }
+}
+
+void clear_poshold_i_item(struct pid_method_ap *method)
+{
+    method->acc_i_item = 0.0f;
+    method->vel_i_item = 0.0f;
+}

controller_yy_app/controlware/mode_attitude.c

@@ -0,0 +1,506 @@
+#include "mode_attitude.h"
+#include "auto_pilot.h"
+#include "control_attitude.h"
+#include "control_rate.h"
+#include "control_throttle.h"
+#include "euler.h"
+#include "flight_mode.h"
+#include "helpler_funtions.h"
+#include "math.h"
+#include "matrix.h"
+#include "params.h"
+#include "soft_imu.h"
+#include "soft_port_uart4.h"
+#include "soft_rc_input.h"
+#include "my_math.h"
+#include "soft_motor_output.h"
+#include "mode_gcs_tax_launch_run.h"
+
+struct controller_xy _cxy;
+
+
+bool attitude_init(void)
+{
+    init_attitude_head();
+
+    butter2_filter_init(&_cxy.vt_ff_flt[0], 200, 10);
+    butter2_filter_init(&_cxy.vt_ff_flt[1], 200, 10);
+
+    return true;
+}
+
+float dist_comp[2] = {0.0f};
+void control_xy(struct controller_xy *cxy, float dt)
+{
+    const float P_KP = pid_v_pos.dist_p;
+    const float V_KP = pid_v_pos.speed_p;
+    const float V_KI = pid_v_pos.acc_i;
+    const float V_KD = pid_v_pos.acc_p;
+
+    cxy->pc[0] = pid_m_posx.loc_c * 0.01f;
+    cxy->pc[1] = pid_m_posy.loc_c * 0.01f;
+
+    cxy->vc[0] = pid_m_posx.vel_c * 0.01f;
+    cxy->vc[1] = pid_m_posy.vel_c * 0.01f;
+
+    cxy->ac[0] = pid_m_posx.acc_c * 0.01f;
+    cxy->ac[1] = pid_m_posy.acc_c * 0.01f;
+
+    /* 按需重置控制器 */
+    if (cxy->reset)
+    {
+        cxy->reset = false;
+
+        cxy->pt[0] = cxy->pc[0];
+        cxy->pt[1] = cxy->pc[1];
+
+        cxy->vt[0] = cxy->vc[0];
+        cxy->vt[1] = cxy->vc[1];
+
+        cxy->at[0] = cxy->ac[0];
+        cxy->at[1] = cxy->ac[1];
+
+        cxy->ve_integ[0] = cxy->ve_integ[1] = 0;
+        cxy->pos_integ[0] = cxy->pos_integ[1] = 0;
+
+        butter2_filter_reset(&cxy->vt_ff_flt[0], 0);
+        butter2_filter_reset(&cxy->vt_ff_flt[1], 0);
+    }
+
+    /*将无人机调整到互定位的桶的中心*/
+    for (int i = 0; i < 2; i++)
+    {
+        if (cxy->pt[i] > 0.1f)
+        {
+            cxy->pt[i] -= 0.002f;
+        }
+        else if (cxy->pt[i] < -0.1f)
+        {
+            cxy->pt[i] += 0.002f;
+        }
+    }
+
+    float vt[2] = {0}, ve[2] = {0};
+    static float target_pos_shift[2] = {0};
+
+    for (int i = 0; i < 2; ++i)
+    {
+
+        // /*如果位置积分持续存在，则需要将位置向相反的方向移动，否则会丢失互定位*/
+        // if(fabsf(cxy->pos_integ[i]) > 0.2f) // 0.2~1.0m 的误差区间，是消除稳态误差的关键阶段 —— 积分项缓慢累加，精准 “拉回” 目标点，同时避免震荡
+        // {
+        //     target_pos_shift[i] += (cxy->pos_integ[i] * 1.0f - target_pos_shift[i]) * 0.001f;
+        // }
+        // else // 若位置误差 <0.2m，说明已接近目标点，积分项继续累加可能导致 “积分震荡”（来回穿越目标点
+        // {
+           target_pos_shift[i] = 0.0f;
+        //}
+
+        vt[i] = (cxy->pt[i] + target_pos_shift[i] - cxy->pc[i]) * P_KP;
+
+        // if(fabsf(cxy->pt[i] + target_pos_shift[i] - cxy->pc[i]) < 3.0f 
+        //     &&fabsf(cxy->pt[i] + target_pos_shift[i] - cxy->pc[i]) > 1.0f )
+        //     cxy->pos_integ[i] += (cxy->pt[i] + target_pos_shift[i] - cxy->pc[i]) * dt * V_KI;
+
+        // cxy->pos_integ[i] = constrain_float(cxy->pos_integ[i], -2.0f, 2.0f);    
+        // 若位置误差 > 1.0m，说明载体离目标点还很远，此时优先用比例项产生大速度指令快速逼近，积分项累加反而可能导致超调
+
+    }
+    #if 0
+    for (int i = 0; i < 2; ++i)
+    {
+        /*如果位置积分持续存在，则需要将位置向相反的方向移动，否则会丢失互定位*/
+        if(fabsf(cxy->pos_integ[i]) > 0.2f)
+        {
+            // 优化1：加快目标位置偏移响应速度（系数从0.001→0.01），快速抵消积分饱和影响
+            target_pos_shift[i] += (cxy->pos_integ[i] * 1.0f - target_pos_shift[i]) * 0.01f;
+        }
+        else
+        {
+        target_pos_shift[i] = 0.0f;
+        }
+
+        // 速度指令 = 修正后的位置误差 × 位置环比例系数（P_KP）
+        vt[i] = (cxy->pt[i] + target_pos_shift[i] - cxy->pc[i]) * P_KP;
+
+        // 优化2：缩小积分触发区间（从1~3m→0.2~1.0m），仅小误差时积分，避免大误差饱和
+        if(fabsf(cxy->pt[i] + target_pos_shift[i] - cxy->pc[i]) < 1.0f 
+            && fabsf(cxy->pt[i] + target_pos_shift[i] - cxy->pc[i]) > 0.2f )
+        {
+            // 积分累积：误差×时间步长×积分系数
+            cxy->pos_integ[i] += (cxy->pt[i] + target_pos_shift[i] - cxy->pc[i]) * dt * V_KI;
+        }
+
+        // 优化3：减小积分饱和值（从±2.0f→±0.5f），避免加速度额外偏移过大
+        cxy->pos_integ[i] = constrain_float(cxy->pos_integ[i], -0.5f, 0.5f);
+        
+        // 优化4：添加积分衰减逻辑，无误差时快速清零，避免积分残留
+        cxy->pos_integ[i] *= 0.98f;
+    }
+    #endif
+    dist_comp[0] = target_pos_shift[0];
+    dist_comp[1] = target_pos_shift[1];
+
+    Vector_ConstrainNorm(vt, 2, 2.0f);
+
+    /* 限制目标速度增量 */
+    const float ACC_MAX = GRAVITY_MSS * tanf(DEG_TO_RAD * 35.0f);
+    float delt_vt[2] = {0};
+    for (int i = 0; i < 2; ++i)
+    {
+        delt_vt[i] = vt[i] - cxy->vt[i];
+    }
+    Vector_ConstrainNorm(delt_vt, 2, ACC_MAX * dt);
+    for (int i = 0; i < 2; ++i)
+    {
+        vt[i] = cxy->vt[i] + delt_vt[i];
+    }
+
+    for (int i = 0; i < 2; ++i)
+    {
+        ve[i] = vt[i] - (1.0f * cxy->vc[i]);
+    }
+    for (int i = 0; i < 2; ++i)
+    {
+        float ve_integ = cxy->ve_integ[i] + dt * ve[i] * V_KI;
+        cxy->ve_integ[i] = constrain_float(ve_integ, -10.0f, 10.0f);
+    }
+
+    float vt_ff[2] = {delt_vt[0] * V_KD / dt, delt_vt[1] * V_KD / dt};
+    Vector_ConstrainNorm(vt_ff, 2, 2.0f);
+    for (int i = 0; i < 2; ++i)
+    {
+        vt_ff[i] = butter2_filter_apply(&cxy->vt_ff_flt[i], vt_ff[i]);
+    }
+
+    float at[2] = {0};
+    for (int i = 0; i < 2; ++i)
+    {
+        at[i] = V_KP * ve[i] + cxy->ve_integ[i] + vt_ff[i] - V_KD * 1.5f * cxy->ac[i] + cxy->pos_integ[i];
+    }
+    Vector_ConstrainNorm(at, 2, 10.0f);
+    float delt_at[2] = {0};
+    for (int i = 0; i < 2; ++i)
+    {
+        delt_at[i] = at[i] - cxy->at[i];
+    }
+    Vector_ConstrainNorm(delt_at, 2, 35.0f * dt);
+    for (int i = 0; i < 2; ++i)
+    {
+        at[i] = cxy->at[i] + delt_at[i];
+    }
+
+    for (int i = 0; i < 2; ++i)
+    {
+        cxy->vt[i] = vt[i];
+        cxy->at[i] = at[i];
+    }
+}
+
+
+
+#define _ATT_CXY_0_RATIO (-0.8f) // 比例暂时以1除以积分周期（s）为准
+#define _ATT_CXY_1_RATIO (-0.8f)
+#define _ATT_HWD_INT_PERIOD (5.0f) // 以5s为积分周期
+
+#define _ATT_ROLL_THRESHOLD  (5.0f) // ROLL角度阈值（度）
+#define _ATT_PITCH_THRESHOLD (5.0f) // PITCH角度阈值（度）
+#define _ATT_ANGLE_PREIOD    (3.0f) // 角度变化阈值时间周期（秒）
+#define _ATT_GO_BACK_TIME_PERIOD  (5.0f) // 打反向运动时常
+#define _ATT_GO_BACK_EULER_ANGLE (15.0f) // 打反向欧拉角阈值（度）
+typedef enum
+{
+    HDW_STATE_ACC_INT = 0,       // 加速度积分
+    HDW_STATE_BACK_EULER,        // 打反向欧拉角
+    HDW_STATE_RECOVER            // 恢复阶段
+} hdw_state_t;
+
+typedef struct
+{
+    hdw_state_t state;
+    uint32_t state_enter_t;
+
+    // 加速度积分
+    bool acc_int_active;
+    uint32_t acc_int_start_t;
+    float acc_int_x;
+    float acc_int_y;
+
+    // 姿态监控
+    bool angle_monitor_triggered;
+    uint32_t angle_over_start_t;
+    bool roll_triggered;
+    bool pitch_triggered;
+
+    // 反向欧拉
+    bool euler_dir_set;
+    int8_t roll_dir;   // -1 / 0 / +1
+    int8_t pitch_dir;
+
+} hdw_ctx_t;
+
+static hdw_ctx_t hdw_ctx = {
+    .state = HDW_STATE_ACC_INT,
+    .state_enter_t = 0,
+
+    .acc_int_active = false,
+    .acc_int_start_t = 0,
+    .acc_int_x = 0.0f,
+    .acc_int_y = 0.0f,
+
+    .angle_monitor_triggered = false,
+    .angle_over_start_t = 0,
+    .roll_triggered = false,
+    .pitch_triggered = false,
+
+    .euler_dir_set = false,
+    .roll_dir = 0,
+    .pitch_dir = 0,
+};
+static inline void no_hdw_reset(hdw_ctx_t *ctx)
+{
+    ctx->state = HDW_STATE_ACC_INT;
+    ctx->state_enter_t = 0;
+
+    ctx->acc_int_active = false;
+    ctx->acc_int_start_t = 0;
+    ctx->acc_int_x = 0.0f;
+    ctx->acc_int_y = 0.0f;
+
+    ctx->angle_monitor_triggered = false;
+    ctx->angle_over_start_t = 0;
+}
+static inline void no_hdw_enter_state(hdw_ctx_t *ctx, hdw_state_t new_state)
+{
+    ctx->state = new_state;
+    ctx->state_enter_t = micros();
+
+    // 状态切换通用清理
+    ctx->angle_monitor_triggered = false;
+    ctx->acc_int_active = false;
+   
+    
+}
+// 无互定位下状态机
+static void hdw_run_no_insgps(hdw_ctx_t *ctx, float dt)
+{
+    float ax_cmd = 0.0f;
+    float ay_cmd = 0.0f;
+
+    switch (ctx->state)
+    {
+    /*================ 1. 加速度积分 =================*/
+    case HDW_STATE_ACC_INT:
+    {
+
+        if (!ctx->acc_int_active)
+        {
+            ctx->acc_int_x = 0;
+            ctx->acc_int_y = 0;
+            ctx->acc_int_start_t = micros();
+            ctx->acc_int_active = true;
+        }
+
+        ctx->acc_int_x += pid_m_posx.acc_c * dt * 0.01f;
+        ctx->acc_int_y += pid_m_posy.acc_c * dt * 0.01f;
+        // 水平方向无加速度时应该是姿态角为0度 
+        // 姿态异常检测 
+        // float att_abs = fmaxf(fabsf(pid_m_roll.angle_c),
+        //                     fabsf(pid_m_pitch.angle_c));
+
+        // if (att_abs > 8.0f)
+        // {
+        //     // 姿态开始不可信，慢慢衰减积分
+        //     ctx->acc_int_x *= 0.98f;
+        //     ctx->acc_int_y *= 0.98f;
+        // }
+
+        ax_cmd = constrain_float(ctx->acc_int_x * _ATT_CXY_0_RATIO, -3.0f, 3.0f);
+        ay_cmd = constrain_float(ctx->acc_int_y * _ATT_CXY_1_RATIO, -3.0f, 3.0f);
+
+        // 姿态异常检测
+        if ((fabsf(pid_m_roll.angle_t - pid_m_roll.angle_c) > _ATT_ROLL_THRESHOLD ||
+            fabsf(pid_m_pitch.angle_t - pid_m_pitch.angle_c) > _ATT_PITCH_THRESHOLD) )
+        {
+            if(ctx->angle_monitor_triggered == false)
+            {
+                ctx->angle_monitor_triggered = true;
+                ctx->angle_over_start_t = micros();
+            }
+            
+            if (micros() - ctx->angle_over_start_t > _ATT_ANGLE_PREIOD * 1e6)
+            {
+                bool roll_over  = fabsf(pid_m_roll.angle_t  - pid_m_roll.angle_c)  > _ATT_ROLL_THRESHOLD;
+                bool pitch_over = fabsf(pid_m_pitch.angle_t - pid_m_pitch.angle_c) > _ATT_PITCH_THRESHOLD;
+
+                ctx->roll_triggered  = roll_over;
+                ctx->pitch_triggered = pitch_over;
+                ctx->euler_dir_set = false;
+                angle_plan_run_cnt++;
+                no_hdw_enter_state(ctx, HDW_STATE_BACK_EULER);
+            }
+        }else
+        {
+            ctx->angle_monitor_triggered = false;
+        }
+
+        // 周期性重置积分
+        if (micros() - ctx->acc_int_start_t > _ATT_HWD_INT_PERIOD * 1e6)
+        {
+            ctx->acc_int_x = 0;
+            ctx->acc_int_y = 0;
+            // float leak = dt / _HWD_INT_PERIOD;
+            // ctx->acc_int_x *= (1.0f - leak);
+            // ctx->acc_int_y *= (1.0f - leak);
+
+            ctx->acc_int_start_t = micros();
+        }
+        break;
+    }
+
+    /*================ 2. 打反向欧拉 =================*/
+    case HDW_STATE_BACK_EULER:
+    {
+        if (!ctx->euler_dir_set)
+        {
+            ctx->roll_dir  = (pid_m_roll.angle_c > 0) ? 1 : -1;
+            ctx->pitch_dir = (pid_m_pitch.angle_c > 0) ? 1 : -1;
+            ctx->euler_dir_set = true;
+        }
+
+        if (micros() - ctx->state_enter_t > _ATT_GO_BACK_TIME_PERIOD * 1e6)
+        {
+            no_hdw_enter_state(ctx, HDW_STATE_ACC_INT);
+        }
+        break;
+    }
+
+    /*================ 3. 恢复 =================*/
+    case HDW_STATE_RECOVER:
+    {
+        if (micros() - ctx->state_enter_t > 1.0f * 1e6) // 以0姿态角持续1秒
+        {
+            no_hdw_enter_state(ctx, HDW_STATE_ACC_INT);
+        }
+        break;
+    }
+
+    default:
+        no_hdw_enter_state(ctx, HDW_STATE_ACC_INT);
+        break;
+    }
+
+    /* 统一输出期望加速度 */
+    _cxy.at[0] = ax_cmd; // 只有在加速度积分下有用 反向欧拉直接控制欧拉角覆盖期望加速度
+    _cxy.at[1] = ay_cmd;
+}
+// 设置反向欧拉角
+static void no_hdw_apply_back_euler(hdw_ctx_t *ctx)
+{
+    if (ctx->state != HDW_STATE_BACK_EULER)
+        return;
+
+    if (ctx->roll_triggered)
+        pid_m_roll.angle_t = -ctx->roll_dir * _ATT_GO_BACK_EULER_ANGLE;
+    else
+        pid_m_roll.angle_t = 0;
+
+    if (ctx->pitch_triggered)
+        pid_m_pitch.angle_t = -ctx->pitch_dir * _ATT_GO_BACK_EULER_ANGLE;
+    else
+        pid_m_pitch.angle_t = 0;
+}
+
+void attitude_run(void)
+{
+    hdw_ctx_t *ctx = &hdw_ctx;
+
+    if (comp_rc_status == COMP_NORMAL && rc_signal_health == RC_SIGNAL_HEALTH)
+    {
+        if (ground_air_status == IN_AIR)
+        {
+            if(rc_in[RC_CH8] > 1500)
+            {                
+                // if(ins.insgps_ok == INSGPS)
+                // {
+                //     control_xy(&_cxy, fast_loop_dt);
+                //     no_hdw_reset(ctx); // 复位无互定位下的控制器
+                // }
+                // else              
+                // {
+                //    _cxy.reset = true; // 重置互定位控制器
+                    hdw_run_no_insgps(ctx, fast_loop_dt); // 无互定位状态机 
+                //}
+                
+
+                float dcm_z[3] = {_cxy.at[0], _cxy.at[1], GRAVITY_MSS};
+                Vector_Normalize(dcm_z, 3);
+
+                float dcm_y[3] = {0, 1, 0};
+                dcm_y[2] = -(dcm_z[0] * dcm_y[0] + dcm_z[1] * dcm_y[1]) / dcm_z[2];
+                Vector_Normalize(dcm_y, 3);
+
+                float dcm_x[3] = {0, 0, 0};
+                Vector_CrossProduct_3D(dcm_y, dcm_z, dcm_x);
+
+                float dcm[3][3];
+                for (int i = 0; i < 3; ++i)
+                {
+                    dcm[i][0] = dcm_x[i];
+                    dcm[i][1] = dcm_y[i];
+                    dcm[i][2] = dcm_z[i];
+                }
+                euler_angle_t euler = {0};
+                Euler_ByDcm(dcm, &euler);
+
+                pid_m_roll.angle_t = apply(euler.roll * RAD_TO_DEG, t_roll_last, 15.0f, fast_loop_dt);
+                pid_m_pitch.angle_t = apply(euler.pitch * RAD_TO_DEG, t_pitch_last, 15.0f, fast_loop_dt);
+
+                no_hdw_apply_back_euler(ctx); // 打反向欧拉角时 覆盖期望欧拉
+                t_roll_last = pid_m_roll.angle_t;
+                t_pitch_last = pid_m_pitch.angle_t;
+            }
+            else
+            {
+                _cxy.reset = true;
+                get_pilot_desired_lean_angles(rc_in[RC_ROLL], rc_in[RC_PITCH]);
+            }           
+        }
+        else
+        {
+            _cxy.reset = true;
+            get_pilot_desired_lean_angles(rc_in[RC_ROLL], rc_in[RC_PITCH]);
+
+            /* 清除内环积分 */
+            clear_rate_i_item(&pid_m_roll);
+            clear_rate_i_item(&pid_m_pitch);
+            clear_rate_i_item(&pid_m_yaw);
+        }
+        get_pilot_desired_climb_rate_fromrc(rc_in[RC_THR]);
+    }
+    else
+    {
+        _cxy.reset = true;
+        get_pilot_desired_lean_angles(1500, 1500);
+        get_pilot_desired_yaw_angle_fromrc(1500, fast_loop_dt);
+        get_pilot_desired_climb_rate_fromrc(1500);
+    }
+
+    Calculate_Target_Az(fast_loop_dt);
+}
+
+void attitude_exit(void)
+{
+    pid_m_posx.vel_i_item = 0.0f;
+    pid_m_posy.vel_i_item = 0.0f;
+
+    pid_m_posx.acc_filter_reset = true;
+    pid_m_posy.acc_filter_reset = true;
+
+    /* 在有落地上锁检测的模式都需要以下代码，以免进入触地怠速后切出到别的模式无法恢复油门控制
+     */
+    if (ground_air_status == IN_AIR && throttle_pidctrl_status == THROTTLE_ONGROUND_IDLE)
+    {
+        throttle_pidctrl_status = THROTTLE_INAIR_RUNNING;
+    }
+}

controller_yy_app/controlware/mode_gcs_tax_launch_run.c

@@ -0,0 +1,486 @@
+#include "mode_gcs_tax_launch_run.h"
+#include "auto_pilot.h"
+#include "control_attitude.h"
+#include "control_rate.h"
+#include "control_throttle.h"
+#include "euler.h"
+#include "helpler_funtions.h"
+#include "lowpass_filter.h"
+#include "matrix.h"
+#include "mode_attitude.h"
+#include "my_math.h"
+#include "params.h"
+#include "pilot_navigation.h"
+#include "soft_can_yy.h"
+#include "soft_imu.h"
+#include "soft_time.h"
+#include "soft_voltage.h"
+#include "flight_mode.h"
+#include "soft_motor_output.h"
+#include <math.h>
+#include <stdint.h>
+
+
+uint8_t __mode_state = 0;  /* 模式运行阶段 */
+uint32_t __time_stamp = 0; /* 定时计数 */
+
+struct yy_hdw_pos
+{
+    uint8_t pos_f; /* m */
+    uint8_t pos_r;
+    uint8_t pos_d;
+};
+
+/**
+ * @brief 模式初始化
+ *
+ * @return true-初始化成功
+ */
+bool gcs_tax_launch_init(void)
+{
+    __mode_state = __STATE_CHECK_ACC_OVERLOAD;
+    __time_stamp = micros();
+
+    butter2_filter_init(&_cxy.vt_ff_flt[0], 200, 10);
+    butter2_filter_init(&_cxy.vt_ff_flt[1], 200, 10);
+
+    return true;
+}
+
+/**
+ * @brief 检测过载
+ *
+ * @param dt 运行周期
+ */
+static void _check_acc_overload_run(float dt)
+{
+    const float acc_overload_thr = 1.0f * 9.8f;
+    static uint32_t acc_over_load_count = 0;
+
+    float acc_norm = 0.01f * sqrtf(raw_imu._x_acc * raw_imu._x_acc +
+                                   raw_imu._y_acc * raw_imu._y_acc +
+                                   raw_imu._z_acc * raw_imu._z_acc);
+    if (acc_norm > acc_overload_thr)
+    {
+        if (acc_over_load_count++ >= 2)
+        {
+            // __mode_state = __STATE_SELF_STABILITY;
+            // __time_stamp = micros();
+        }
+    }
+}
+
+/**
+ * @brief 起飞自稳定
+ *
+ * @param dt 运行周期
+ */
+static void _self_stability_run(float dt)
+{
+    if (thr_lock_status == LOCKED)
+    {
+        if (micros() - __time_stamp > 0.3f * 1e6) /* 延迟 0.3s 后解锁 */
+        {
+            __mode_state = __STATE_LAUNCH_GO;
+            pid_m_alt.thr_hold = true;
+
+            set_thr_lock_status(UNLOCKED, LAUNCHUNLOCK);
+            set_automode_target_yaw(pid_m_yaw.angle_c);
+
+            althold_state = NO_ALT_HOLD;
+
+            /* 清除定点积分 */
+            clear_poshold_i_item(&pid_m_posx);
+            clear_poshold_i_item(&pid_m_posy);
+            clear_poshold_i_item(&pid_m_alt);
+
+            /* 清除内环积分 */
+            clear_rate_i_item(&pid_m_roll);
+            clear_rate_i_item(&pid_m_pitch);
+            clear_rate_i_item(&pid_m_yaw);
+
+            ground_air_status = IN_AIR;
+            throttle_pidctrl_status = THROTTLE_INAIR_RUNNING;
+            pid_m_alt.vel_t = pid_m_alt.vel_c;
+            pid_m_roll.angle_t = 0;
+            pid_m_pitch.angle_t = 0;
+        }
+    }
+}
+
+extern void control_xy(struct controller_xy *cxy, float dt);
+float record_target_alt = 50 * 100; // cm
+float thr_max = 1400.0f; // 限制油门最大为
+uint16_t angle_plan_run_cnt = 0;
+#define _CXY_0_RATIO (-0.8f) // X轴加速度积分结果的比例系数（经验值，需根据实测调整）
+#define _CXY_1_RATIO (-0.8f) // Y轴加速度积分结果的比例系数
+#define _HWD_INT_PERIOD (5.0f) // 以5s为积分周期
+
+#define _ROLL_THRESHOLD  (5.0f) // ROLL角度阈值（度）
+#define _PITCH_THRESHOLD (5.0f) // PITCH角度阈值（度）
+#define _ANGLE_PERIOD    (3.0f) // 角度变化阈值时间周期（秒）
+#define _GO_BACK_TIME_PERIOD      (5.0f) // 打反向运动时常
+#define _GO_BACK_EULER_ANGLE (15.0f) // 打反向欧拉角阈值（度）
+
+#define START_UP_STATE1_DURATION (2 * 1e6) // 弹起阶段稳定时长
+#define YAW_DELAY_AFTER_LAUNCH (6.0f * 1e6) // 起飞后航向控制延迟
+#define START_UP_STATE2_THR_RAMP_DURATION (1 * 1e6) 
+// 启动阶段1给1450油门 3秒后进入策略
+#define START_UP_STATE0_THR_MAX (1400.0f) // 弹射阶段0 最大油门
+#define START_UP_STATE1_THR_MAX (1430.0f) // 弹起阶段1 最大油门
+#define START_UP_STATE2_THR_MAX (1480.0f) // 弹起阶段2 最大油门
+typedef enum
+{
+    HDW_STATE_POSITION = 0, // 互定位位置控制
+    HDW_STATE_ACC_INT ,       // 加速度积分
+    HDW_STATE_BACK_EULER,        // 打反向欧拉角
+    HDW_STATE_RECOVER            // 恢复阶段
+} hdw_state_t;
+
+typedef struct
+{
+    hdw_state_t state;
+    uint32_t state_enter_t;
+
+    // 加速度积分
+    bool acc_int_active;
+    uint32_t acc_int_start_t;
+    float acc_int_x;
+    float acc_int_y;
+
+    // 姿态监控
+    bool angle_monitor_triggered;
+    uint32_t angle_over_start_t;
+    bool roll_triggered;
+    bool pitch_triggered;
+
+    // 反向欧拉
+    bool euler_dir_set;
+    int8_t roll_dir;   // -1 / 0 / +1
+    int8_t pitch_dir;
+
+} hdw_ctx_t;
+
+
+
+static hdw_ctx_t hdw_ctx = {
+    .state = HDW_STATE_ACC_INT,
+    .state_enter_t = 0,
+
+    .acc_int_active = false,
+    .acc_int_start_t = 0,
+    .acc_int_x = 0.0f,
+    .acc_int_y = 0.0f,
+
+    .angle_monitor_triggered = false,
+    .angle_over_start_t = 0,
+    .roll_triggered = false,
+    .pitch_triggered = false,
+
+    .euler_dir_set = false,
+    .roll_dir = 0,
+    .pitch_dir = 0,
+};
+
+static inline void no_hdw_reset(hdw_ctx_t *ctx)
+{
+    ctx->state = HDW_STATE_ACC_INT;
+    ctx->state_enter_t = 0;
+
+    ctx->acc_int_active = false;
+    ctx->acc_int_start_t = 0;
+    ctx->acc_int_x = 0.0f;
+    ctx->acc_int_y = 0.0f;
+
+    ctx->angle_monitor_triggered = false;
+    ctx->angle_over_start_t = 0;
+}
+static inline void no_hdw_enter_state(hdw_ctx_t *ctx, hdw_state_t new_state)
+{
+    ctx->state = new_state;
+    ctx->state_enter_t = micros();
+
+    // 状态切换通用清理
+    ctx->angle_monitor_triggered = false;
+    ctx->acc_int_active = false;
+   
+    
+}
+// 无互定位下状态机
+static void hdw_run_no_insgps(hdw_ctx_t *ctx, float dt)
+{
+    float ax_cmd = 0.0f;
+    float ay_cmd = 0.0f;
+
+    switch (ctx->state)
+    {
+    case HDW_STATE_POSITION:break;
+    /*================ 1. 加速度积分 =================*/
+    case HDW_STATE_ACC_INT:
+    {
+
+        if (!ctx->acc_int_active)
+        {
+            ctx->acc_int_x = 0;
+            ctx->acc_int_y = 0;
+            ctx->acc_int_start_t = micros();
+            ctx->acc_int_active = true;
+        }
+
+        ctx->acc_int_x += pid_m_posx.acc_c * dt * 0.01f;
+        ctx->acc_int_y += pid_m_posy.acc_c * dt * 0.01f;
+       
+        ax_cmd = constrain_float(ctx->acc_int_x * _CXY_0_RATIO, -3.0f, 3.0f);
+        ay_cmd = constrain_float(ctx->acc_int_y * _CXY_1_RATIO, -3.0f, 3.0f);
+
+        // 姿态异常检测
+        // if ((fabsf(pid_m_roll.angle_t - pid_m_roll.angle_c) > _ROLL_THRESHOLD ||
+        //     fabsf(pid_m_pitch.angle_t - pid_m_pitch.angle_c) > _PITCH_THRESHOLD) )
+        // {
+        //     if(ctx->angle_monitor_triggered == false)
+        //     {
+        //         ctx->angle_monitor_triggered = true;
+        //         ctx->angle_over_start_t = micros();
+        //     }
+            
+        //     if (micros() - ctx->angle_over_start_t > _ANGLE_PERIOD * 1e6)
+        //     {
+        //         bool roll_over  = fabsf(pid_m_roll.angle_t  - pid_m_roll.angle_c)  > _ROLL_THRESHOLD;
+        //         bool pitch_over = fabsf(pid_m_pitch.angle_t - pid_m_pitch.angle_c) > _PITCH_THRESHOLD;
+
+        //         ctx->roll_triggered  = roll_over;
+        //         ctx->pitch_triggered = pitch_over;
+        //         ctx->euler_dir_set = false;
+        //         angle_plan_run_cnt++;
+        //         no_hdw_enter_state(ctx, HDW_STATE_BACK_EULER);
+        //     }
+        // }else
+        // {
+        //     ctx->angle_monitor_triggered = false;
+        // }
+
+        // 周期性重置积分
+        if (micros() - ctx->acc_int_start_t > _HWD_INT_PERIOD * 1e6)
+        {
+            ctx->acc_int_x = 0;
+            ctx->acc_int_y = 0;
+
+            ctx->acc_int_start_t = micros();
+        }
+        break;
+    }
+
+    /*================ 2. 打反向欧拉 =================*/
+    case HDW_STATE_BACK_EULER:
+    {
+        ax_cmd = 0.0f; 
+        ay_cmd = 0.0f;
+        
+        if (!ctx->euler_dir_set)
+        {
+            ctx->roll_dir  = (pid_m_roll.angle_c > 0) ? 1 : -1;
+            ctx->pitch_dir = (pid_m_pitch.angle_c > 0) ? 1 : -1;
+            ctx->euler_dir_set = true;
+        }
+
+        if (micros() - ctx->state_enter_t > _GO_BACK_TIME_PERIOD * 1e6)
+        {
+            no_hdw_enter_state(ctx, HDW_STATE_ACC_INT);
+        }
+        break;
+    }
+
+    /*================ 3. 恢复 =================*/
+    case HDW_STATE_RECOVER:
+    {
+        if (micros() - ctx->state_enter_t > 1.0f * 1e6) // 以0姿态角持续1秒
+        {
+            no_hdw_enter_state(ctx, HDW_STATE_ACC_INT);
+        }
+        break;
+    }
+
+    default:
+        no_hdw_enter_state(ctx, HDW_STATE_ACC_INT);
+        break;
+    }
+
+    /* 统一输出期望加速度 */
+    _cxy.at[0] = ax_cmd; // 只有在加速度积分下有用 反向欧拉直接控制欧拉角覆盖期望加速度
+    _cxy.at[1] = ay_cmd;
+}
+
+// 设置反向欧拉角
+static void no_hdw_apply_back_euler(hdw_ctx_t *ctx)
+{
+    if (ctx->state != HDW_STATE_BACK_EULER)
+        return;
+
+    if (ctx->roll_triggered)
+        pid_m_roll.angle_t = -ctx->roll_dir * _GO_BACK_EULER_ANGLE;
+    else
+        pid_m_roll.angle_t = 0;
+
+    if (ctx->pitch_triggered)
+        pid_m_pitch.angle_t = -ctx->pitch_dir * _GO_BACK_EULER_ANGLE;
+    else
+        pid_m_pitch.angle_t = 0;
+}
+
+
+void __tax_launch_go_run(float dt)
+{
+    const struct yy_can_rx_msg_data *yycan_data = yy_can_rx_data_get();
+    hdw_ctx_t *hdw_ctx_p = &hdw_ctx;
+    // start_up_ctx_t *start_up_ctx_p = &start_up_ctx;
+    /**************************************/
+    if (pid_m_alt.thr_hold == true)
+    {
+        thr_max =  START_UP_STATE0_THR_MAX;
+        if (micros() - __time_stamp > 2.0f * 1e6)
+        {
+            pid_m_alt.thr_hold = false;
+        }
+
+        althold_state = NO_ALT_HOLD;
+
+        t_roll_last = pid_m_roll.angle_t = 0;
+        t_pitch_last = pid_m_pitch.angle_t = 0;
+
+        _cxy.reset = true;
+
+        /*弹射后2秒钟，记录当时的高度，作为自动控制时候的目标高度*/
+        // record_target_alt = pid_m_alt.loc_c;
+        record_target_alt = 50 * 100; // 50m高度
+        no_hdw_enter_state(hdw_ctx_p, HDW_STATE_ACC_INT);     
+    }
+    else
+    {
+        // alt_reached = true;
+        thr_max = START_UP_STATE2_THR_MAX; // 油门限制1480
+        hdw_run_no_insgps(hdw_ctx_p, dt); // 无互定位状态机
+          
+        // 大地系下的期望加速度 _cxy.at[0], _cxy.at[1]
+        float dcm_z[3] = {_cxy.at[0], _cxy.at[1], GRAVITY_MSS};
+        Vector_Normalize(dcm_z, 3);
+
+        float dcm_y[3] = {0, 1, 0};
+        dcm_y[2] = -(dcm_z[0] * dcm_y[0] + dcm_z[1] * dcm_y[1]) / dcm_z[2];
+        Vector_Normalize(dcm_y, 3);
+
+        float dcm_x[3] = {0, 0, 0};
+        Vector_CrossProduct_3D(dcm_y, dcm_z, dcm_x);
+
+        float dcm[3][3];
+        for (int i = 0; i < 3; ++i)
+        {
+            dcm[i][0] = dcm_x[i];
+            dcm[i][1] = dcm_y[i];
+            dcm[i][2] = dcm_z[i];
+        }
+        euler_angle_t euler = {0};
+        Euler_ByDcm(dcm, &euler);
+        pid_m_roll.angle_t = apply(euler.roll * RAD_TO_DEG, t_roll_last, 15.0f, dt);
+        pid_m_pitch.angle_t = apply(euler.pitch * RAD_TO_DEG, t_pitch_last, 15.0f, dt);
+
+        // no_hdw_apply_back_euler(hdw_ctx_p); // 打反向欧拉角时 覆盖期望欧拉
+        t_roll_last = pid_m_roll.angle_t;
+        t_pitch_last = pid_m_pitch.angle_t;
+
+        althold_state = NO_ALT_HOLD;
+
+        /********************优先响应高度****************/
+       
+        // if (!isnan(yycan_data->txgl_alt_sp) && yycan_data->txgl_alt_sp > 0 && yycan_data->txgl_alt_sp <= 100)
+        // {
+        //     record_target_alt = yycan_data->txgl_alt_sp * 100.0f; 
+        //     if(debug_mode == false)
+        //     {
+        //         if (fabsf(yycan_data->txgl_alt_sp * 100 - pid_m_alt.loc_c) < 30.0f)
+        //         {
+        //             updata_cmd_status(ALT_EXECUTE, true);
+        //             updata_cmd_status(ALT_SETTING, false);
+        //             alt_reached = true;
+        //         }
+        //     }
+        //     else
+        //     {
+        //         if(micros() - recv_alt_time > 3 * 1e6 && alt_reached == false)
+        //         {
+        //             updata_cmd_status(ALT_EXECUTE, true);
+        //             updata_cmd_status(ALT_SETTING, false);
+        //             alt_reached = true;
+        //         }
+        //     }
+        // }
+    
+             
+        //pid_m_alt.loc_t = record_target_alt;
+        //float perror_u = (record_target_alt - pid_m_alt.loc_c);
+        // pid_m_alt.vel_t = constrain_float(perror_u * 0.8f, -100.0f, 100.0f); // 期望速度倍做了限制 -100~100cm/s
+        // pid_m_alt.vel_t = constrain_float(perror_u * 0.8f, -50.0f, 50.0f); // 期望速度倍做了限制 -50~50cm/s
+        pid_m_alt.vel_t = 100.0f;  // 期望速度80cm/s 以0.8m/s的速度撞击没有那么大的冲击, 加长高油门时间 减小减速油门时间 2：1
+        /********************高度达到后，相应航向****************/
+        if(((micros()-__time_stamp) > 6.0f * 1e6))
+        {
+            alt_reached = true;
+        }
+        if (alt_reached) // 差不多就是4秒达到最高点
+        {
+            // 航向到位，更新flag
+            if (pid_m_yaw.enable_tyaw)
+            {            
+                set_automode_target_yaw(yycan_data->txgl_yaw_sp);
+
+                if (fabsf(wrap_180(pid_m_yaw.angle_c - yycan_data->txgl_yaw_sp)) < 5.0f)
+                {
+                    updata_cmd_status(YAW_EXECUTE, true);
+                    updata_cmd_status(YAW_SETTING, false);
+
+                    pid_m_yaw.enable_tyaw = false;
+
+                    yaw_reached = true;
+                }              
+            }
+           
+        }
+        else
+        {
+            t_roll_last  = pid_m_roll.angle_t = 0;
+            t_pitch_last = pid_m_pitch.angle_t = 0;
+            hdw_ctx_p->acc_int_x = 0; // 上升到最高点之前不进行加速度积分策略
+            hdw_ctx_p->acc_int_y = 0;
+        }
+
+        /*所有动作暂停指令 实际台子无该指令*/
+        if ((yycan_data->txgl_drone_cmd & CTL_STOP_CMD_MASK) == CTL_STOP_ACTION)
+        {
+            set_automode_target_yaw(wrap_360(pid_m_yaw.angle_c)); // 航向锁定
+            pid_m_alt.vel_t = 0; // 垂速为0
+        }
+    }
+    Calculate_Target_Az(fast_loop_dt);
+}
+
+void gcs_tax_launch_run(float dt)
+{
+    switch (__mode_state)
+    {
+    case __STATE_CHECK_ACC_OVERLOAD:
+        _check_acc_overload_run(dt);
+        break;
+    case __STATE_SELF_STABILITY:
+        _self_stability_run(dt);
+        break;
+    case __STATE_LAUNCH_GO:
+        __tax_launch_go_run(dt);
+        break;
+    default:
+        break;
+    }
+}
+
+void gcs_tax_launch_exit(void)
+{
+    pid_m_alt.thr_hold = false;
+}

controller_yy_app/hardware/hard_can.c

@@ -0,0 +1,236 @@
+#include <stdio.h>
+#include <assert.h>
+#include "board.h"
+#include "hpm_can_drv.h"
+#include "hard_can.h"
+#include "test.h"
+// PD20 PD25 CAN2 调试使用
+#define CAN2         HPM_CAN2
+#define CAN2_IRQ     IRQn_CAN2
+#define CAN2_CLK_NAME clock_can2
+#define BAUD_1000k   (1000000)
+#define BAUD_500k    (500000)
+
+
+void can_gpio_cofig(void)
+{
+    HPM_IOC->PAD[IOC_PAD_PD20].FUNC_CTL = IOC_PD20_FUNC_CTL_CAN2_TXD; 
+
+    HPM_IOC->PAD[IOC_PAD_PD25].FUNC_CTL = IOC_PD25_FUNC_CTL_CAN2_RXD;
+}
+
+/**
+ * @brief CAN1 硬件初始化  具体使用哪个CAN口配置 
+ * @param baudrate 波特率，如 500000 (500Kbps) 或 1000000 (1Mbps)
+ */
+void CAN1_Mode_Init(uint32_t baudrate)
+{
+    can_config_t can_config;
+    can_filter_config_t can_filter;
+    hpm_stat_t status;
+    /* 配置IO 获取时钟*/
+    can_gpio_cofig(); // 配置IO
+    clock_set_source_divider(CAN2_CLK_NAME, clk_src_pll1_clk1, 5); //80M
+    clock_add_to_group(CAN2_CLK_NAME, 0);
+    uint32_t freq = clock_get_frequency(CAN2_CLK_NAME);
+    /* 配置过滤器 - 接收所有消息 */
+    can_filter.enable = true;
+    can_filter.index = 0;
+    can_filter.id_mode = can_filter_id_mode_both_frames; // 接收标准与扩展帧
+    can_filter.code = 0; // 期望接收的报文ID的基础值
+    can_filter.mask = 0x1FFFFFFF; // 关键点：掩码全部设为  1：忽略
+
+    /* 默认配置并修改 */
+    can_get_default_config(&can_config);
+    can_config.baudrate = baudrate;
+    can_config.mode = can_mode_normal;
+    
+    /* 使能中断 */
+    // 接收事件 主发送缓冲器发送完成 次发送缓冲器发送完成 错误事件
+    // 仲裁丢失错误 被动错误状态 总线错误
+    can_config.irq_txrx_enable_mask = CAN_EVENT_RECEIVE | 
+                                      CAN_EVENT_TX_PRIMARY_BUF | 
+                                      CAN_EVENT_TX_SECONDARY_BUF | 
+                                      CAN_EVENT_ERROR;
+    can_config.irq_error_enable_mask = CAN_ERROR_ARBITRATION_LOST_INT_ENABLE | 
+                                       CAN_ERROR_PASSIVE_INT_ENABLE | 
+                                       CAN_ERROR_BUS_ERROR_INT_ENABLE;
+
+    
+    /* 关联滤波器 */
+    can_config.filter_list = &can_filter; // 最多可以给一个can配置16个滤波器 每个都有16路 牛逼
+    can_config.filter_list_num = 1;
+    
+    /* 初始化 CAN */
+    status = can_init(CAN2, &can_config, freq);
+    if (status != status_success) {
+        printf("CAN initialization failed, error code: %d\n", status);
+        return;
+    }
+    
+    /* 使能中断 */
+    intc_m_enable_irq_with_priority(CAN2_IRQ, 1);
+    printf("CAN0 initialized successfully at %d bps\n", baudrate);
+}
+
+#ifdef CAN2_TEST
+#include "hpm_sysctl_drv.h"
+static volatile  bool has_new_rcv_msg;
+static volatile  bool has_sent_out;
+static volatile  bool has_error;
+static volatile  can_receive_buf_t s_can_rx_buf;
+static volatile  uint8_t error_flags;
+
+SDK_DECLARE_EXT_ISR_M(CAN2_IRQ, can2_isr)
+void can2_isr(void)
+{
+    uint8_t flags = can_get_tx_rx_flags(CAN2);
+    if ((flags & CAN_EVENT_RECEIVE) != 0) {
+        can_read_received_message(CAN2, (can_receive_buf_t *)&s_can_rx_buf);
+        has_new_rcv_msg = true;
+    }
+    if ((flags & (CAN_EVENT_TX_PRIMARY_BUF | CAN_EVENT_TX_SECONDARY_BUF))) {
+        has_sent_out = true;
+    }
+    if ((flags & CAN_EVENT_ERROR) != 0) {
+        has_error = true;
+    }
+    can_clear_tx_rx_flags(CAN2, flags);
+
+    error_flags = can_get_error_interrupt_flags(CAN2);
+    if (error_flags != 0) {
+        has_error = true;
+    }
+    can_clear_error_interrupt_flags(CAN2, error_flags);
+}
+
+static uint8_t can_get_data_bytes_from_dlc(uint32_t dlc)
+{
+    uint32_t data_bytes = 0;
+
+    dlc &= 0xFU;
+    if (dlc <= 8U) {
+        data_bytes = dlc;
+    }
+    else {
+        switch (dlc) {
+        case can_payload_size_12:
+            data_bytes = 12U;
+            break;
+        case can_payload_size_16:
+            data_bytes = 16U;
+            break;
+        case can_payload_size_20:
+            data_bytes = 20U;
+            break;
+        case can_payload_size_24:
+            data_bytes = 24U;
+            break;
+        case can_payload_size_32:
+            data_bytes = 32U;
+            break;
+        case can_payload_size_48:
+            data_bytes = 48U;
+            break;
+        case can_payload_size_64:
+            data_bytes = 64U;
+            break;
+        default:
+            /* Code should never touch here */
+            break;
+        }
+    }
+
+    return data_bytes;
+}
+
+static void show_received_can_message(const can_receive_buf_t *rx_msg)
+{
+    uint32_t msg_len = can_get_data_bytes_from_dlc(rx_msg->dlc);
+    printf("CAN message info:\nID=%08x\nContent=:\n", rx_msg->id);
+    uint32_t remaining_size = msg_len;
+    uint32_t print_size;
+
+    for (uint32_t i = 0; i < msg_len; i += 16) {
+        print_size = MIN(remaining_size, 16);
+        for (uint32_t j = 0; j < print_size; j++) {
+            printf("%02x ", rx_msg->data[i + j]);
+        }
+        printf("\n");
+        remaining_size -= print_size;
+    }
+}
+
+static void can_echo_test_responder(void)
+{
+    
+    hpm_stat_t status = 1;
+    printf("CAN echo test: Responder is waiting for echo message...\n");
+    while (!has_new_rcv_msg) {
+    }
+    has_new_rcv_msg = false;
+    show_received_can_message((const can_receive_buf_t *)&s_can_rx_buf);
+
+    can_transmit_buf_t tx_buf;
+    memset(&tx_buf, 0, sizeof(tx_buf));
+    tx_buf.dlc = s_can_rx_buf.dlc;
+    tx_buf.id = 0x321;
+    uint32_t msg_len = can_get_data_bytes_from_dlc(s_can_rx_buf.dlc);
+    memcpy(&tx_buf.data, (uint8_t *)&s_can_rx_buf.data, msg_len);
+    status = can_send_message_blocking(CAN2, &tx_buf);
+    if (status != status_success) {
+        printf("CAN sent message failed, error_code:%d\n", status);
+        return;
+    }
+    printf("Sent echo message back\n");
+}
+
+static const char *get_can_error_kind_str(uint8_t error_kind)
+{
+    const char *error_info_str = NULL;
+    switch (error_kind) {
+    case CAN_KIND_OF_ERROR_NO_ERROR:
+        error_info_str = "No error";
+        break;
+    case CAN_KIND_OF_ERROR_BIT_ERROR:
+        error_info_str = "Bit error";
+        break;
+    case CAN_KIND_OF_ERROR_FORM_ERROR:
+        error_info_str = "Form error";
+        break;
+    case CAN_KIND_OF_ERROR_STUFF_ERROR:
+        error_info_str = "Stuff error";
+        break;
+    case CAN_KIND_OF_ERROR_ACK_ERROR:
+        error_info_str = "ACK error";
+        break;
+    case CAN_KIND_OF_ERROR_CRC_ERROR:
+        error_info_str = "CRC error";
+        break;
+    case CAN_KIND_OF_ERROR_OTHER_ERROR:
+        error_info_str = "Other errors";
+        break;
+    default:
+        error_info_str = "Uknown error";
+        break;
+    }
+
+    return error_info_str;
+}
+/* can2 demo test 2026/03/14 ok 扩展canid*/
+/*
+CAN message info:
+ID=00000001
+Content=:
+e2 80 00 00 00 fe fe 00 
+Sent echo message back
+*/
+void can2_test(void)
+{
+  CAN1_Mode_Init(BAUD_1000k);
+  board_delay_ms(3000);
+  can_echo_test_responder();
+
+}
+
+#endif

controller_yy_app/hardware/hard_flash_at45db.c

@@ -0,0 +1,352 @@
+#include "hard_flash_at45db.h"
+#include "board.h"
+#include "hpm_spi_drv.h"
+#include "hpm_clock_drv.h"
+#include "hpm_gpio_drv.h"
+// 后续具体的IO宏都放到board头文件里
+#ifndef GD25Q16_FLASH
+
+/* 使用 SPI2 作为示例，你可以根据需要修改 */
+#define AT45DB_SPI_BASE          HPM_SPI2
+
+#define AT45DB_CS_GPIO_PTR       HPM_GPIO0    /* GPIO 控制器基地址 */
+#define AT45DB_CS_PORT           1      /* GPIOB = 1 */
+#define AT45DB_CS_PIN            12     /* PIN12 */
+/******************SPI时钟频率*******************/
+#define AT45DB_CLK_FRE           21000000
+/* CS 控制宏 */
+#define AT45DB_CS_SELECT()       gpio_write_pin(AT45DB_CS_GPIO_PTR, AT45DB_CS_PORT, AT45DB_CS_PIN, 0)
+#define AT45DB_CS_DESELECT()     gpio_write_pin(AT45DB_CS_GPIO_PTR, AT45DB_CS_PORT, AT45DB_CS_PIN, 1)
+
+/* 静态变量 */
+static uint32_t spi_clk_freq;
+
+/**
+ * @brief SPI 读写一个字节
+ */
+static uint8_t spi_readwrite_byte(uint8_t byte)
+{
+    uint8_t rx_byte = 0;
+    spi_control_config_t control_config;
+    
+    /* 配置传输控制 */
+    spi_master_get_default_control_config(&control_config);
+    control_config.common_config.trans_mode = spi_trans_write_read_together;
+    control_config.master_config.cmd_enable = false;
+    control_config.master_config.addr_enable = false;
+    
+    /* 执行传输 */
+    spi_transfer(AT45DB_SPI_BASE, 
+                 &control_config,
+                 NULL, NULL,
+                 &byte, 1, 
+                 &rx_byte, 1);
+    
+    return rx_byte;
+}
+
+/**
+ * @brief 初始化 AT45DB 的 SPI 接口
+ */
+void at45db_init(void)
+{
+    spi_timing_config_t timing_config = {0};
+    spi_format_config_t format_config = {0};
+    
+    /* 初始化 SPI 时钟和引脚 */
+    spi_clk_freq = board_init_spi_clock(AT45DB_SPI_BASE);
+    board_init_spi_pins(AT45DB_SPI_BASE);
+    
+    /*  配置 CS 引脚 */
+    AT45DB_CS_DESELECT();  /* CS 默认高电平 */
+    
+    /* 3. 配置 SPI 时序（主模式）*/
+    spi_master_get_default_timing_config(&timing_config);
+    timing_config.master_config.clk_src_freq_in_hz = spi_clk_freq;
+    timing_config.master_config.sclk_freq_in_hz = AT45DB_CLK_FRE; 
+    spi_master_timing_init(AT45DB_SPI_BASE, &timing_config);
+    
+    /* 4. 配置 SPI 格式 */
+    spi_master_get_default_format_config(&format_config);
+    format_config.common_config.data_len_in_bits = 8;      /* 8位数据 */
+    format_config.common_config.mode = spi_master_mode;    /* 主机模式 */
+    format_config.common_config.cpol = spi_sclk_low_idle;  /* CPOL=0：空闲时低电平 */
+    format_config.common_config.cpha = spi_sclk_sampling_odd_clk_edges; /* CPHA=0：第一个边沿采样 */
+    spi_format_init(AT45DB_SPI_BASE, &format_config);
+}
+
+/**
+ * @brief 查询 AT45DB 是否准备好
+ * @return 1: 准备好, 0: 忙
+ */
+uint8_t at45db_is_busy(void)
+{
+    uint8_t status_reg;
+    
+    AT45DB_CS_SELECT();
+    spi_readwrite_byte(AT45DB_READ_STATE_REGISTER);
+    status_reg = spi_readwrite_byte(0x00);
+    AT45DB_CS_DESELECT();
+    
+    return (status_reg & 0x80) ? 1 : 0;
+}
+
+/**
+ * @brief 检查 AT45DB 是否存在
+ * @return 1: 存在, 0: 不存在
+ */
+uint8_t at45db_check(void)
+{
+    uint8_t buf[3];
+    
+    AT45DB_CS_SELECT();
+    spi_readwrite_byte(0x9F);  /* 读ID命令 */
+    buf[0] = spi_readwrite_byte(0);
+    buf[1] = spi_readwrite_byte(0);
+    buf[2] = spi_readwrite_byte(0);
+    AT45DB_CS_DESELECT();
+    
+    /* AT45DB161 的 ID 是 0x1F, 0x26, 0x00 */
+    if ((buf[0] == 0x1F) && (buf[1] == 0x26) && (buf[2] == 0x00)) {
+        return 1;
+    }
+    return 0;
+}
+
+/**
+ * @brief 读一页数据
+ * @param page 页地址 (0-4096)
+ * @param pdata 数据缓冲区
+ */
+void at45db_read_page(uint16_t page, uint8_t *pdata)
+{
+    int i;
+    
+    if (page > 4096) return;
+    
+    /* 等待芯片就绪 */
+    while (at45db_is_busy() == 0);
+    
+    AT45DB_CS_SELECT();
+    spi_readwrite_byte(AT45DB_MM_PAGE_READ);
+    spi_readwrite_byte((int8_t)(page >> 6));   /* A20 - A9 */
+    spi_readwrite_byte((int8_t)(page << 2));
+    spi_readwrite_byte(0x00);                   /* 3字节地址 */
+    
+    /* 4个 dummy 字节 */
+    spi_readwrite_byte(0x00);
+    spi_readwrite_byte(0x00);
+    spi_readwrite_byte(0x00);
+    spi_readwrite_byte(0x00);
+    
+    /* 读取数据 */
+    for (i = 0; i < 512; i++) {
+        *pdata++ = spi_readwrite_byte(0x00);
+    }
+    AT45DB_CS_DESELECT();
+}
+
+/**
+ * @brief 写一页数据
+ * @param page 页地址 (0-4095)
+ * @param data 要写入的数据
+ */
+void at45db_write_page(uint16_t page, uint8_t *data)
+{
+    uint16_t i;
+    
+    if (page > 4095) return;
+    
+    /* 等待芯片就绪 */
+    while (at45db_is_busy() == 0);
+    
+    /* 步骤1：将数据写入 buffer2 */
+    AT45DB_CS_SELECT();
+    spi_readwrite_byte(AT45DB_BUFFER_2_WRITE);
+    spi_readwrite_byte(0x00);
+    spi_readwrite_byte(0x00);
+    spi_readwrite_byte(0x00);
+    
+    for (i = 0; i < 512; i++) {
+        spi_readwrite_byte(data[i]);
+    }
+    AT45DB_CS_DESELECT();
+    
+    /* 等待写入完成 */
+    while (at45db_is_busy() == 0);
+    
+    /* 步骤2：将 buffer2 的内容编程到主存储器 */
+    AT45DB_CS_SELECT();
+    spi_readwrite_byte(AT45DB_B2_TO_MM_PAGE_PROG_WITH_ERASE);
+    spi_readwrite_byte((uint8_t)(page >> 6));
+    spi_readwrite_byte((uint8_t)(page << 2));
+    spi_readwrite_byte(0x00);
+    AT45DB_CS_DESELECT();
+}
+
+/**
+ * @brief 在一页内写入部分数据
+ * @param page 页地址
+ * @param data 数据
+ * @param len 长度
+ */
+void at45db_write_bytes_on_one_page(uint16_t page, uint8_t *data, uint16_t len)
+{
+    uint16_t i;
+    
+    if (page > 4095 || len > 512) return;
+    
+    /* 等待芯片就绪 */
+    while (at45db_is_busy() == 0);
+    
+    /* 将数据写入 buffer2 */
+    AT45DB_CS_SELECT();
+    spi_readwrite_byte(AT45DB_BUFFER_2_WRITE);
+    spi_readwrite_byte(0x00);
+    spi_readwrite_byte(0x00);
+    spi_readwrite_byte(0x00);
+    
+    for (i = 0; i < len; i++) {
+        spi_readwrite_byte(data[i]);
+    }
+    AT45DB_CS_DESELECT();
+    
+    /* 等待写入完成 */
+    while (at45db_is_busy() == 0);
+    
+    /* 编程到主存储器 */
+    AT45DB_CS_SELECT();
+    spi_readwrite_byte(AT45DB_B2_TO_MM_PAGE_PROG_WITH_ERASE);
+    spi_readwrite_byte((uint8_t)(page >> 6));
+    spi_readwrite_byte((uint8_t)(page << 2));
+    spi_readwrite_byte(0x00);
+    AT45DB_CS_DESELECT();
+}
+
+/**
+ * @brief 从任意地址读取多个字节
+ * @param addr 起始地址 (0-4096*512)
+ * @param pdata 数据缓冲区
+ * @param len 要读取的字节数
+ */
+void at45db_read_bytes(uint32_t addr, uint8_t *pdata, uint16_t len)
+{
+    int i, j, k;
+    uint8_t temp[512];
+    uint16_t page, offset;
+    
+    if (addr > 4096 * 512) return;
+    
+    page = addr / 512;
+    offset = addr % 512;
+    
+    at45db_read_page(page++, temp);
+    for (i = offset; (len != 0) && (i < 512); len--, i++) {
+        *pdata++ = temp[i];
+    }
+    
+    if ((i == 512) && (len != 0)) {
+        if (len > 512) {
+            for (k = 0; k < len / 512; k++) {
+                at45db_read_page(page++, temp);
+                for (j = 0; j < 512; j++) {
+                    *pdata++ = temp[j];
+                }
+            }
+            len -= 512 * k;
+        }
+        if (len != 0) {
+            at45db_read_page(page, temp);
+            for (j = 0; j < len; j++) {
+                *pdata++ = temp[j];
+            }
+        }
+    }
+}
+
+/**
+ * @brief 从任意地址写入多个字节
+ * @param addr 起始地址 (0-4096*512)
+ * @param pdata 要写入的数据
+ * @param len 要写入的字节数
+ */
+void at45db_write_bytes(uint32_t addr, uint8_t *pdata, uint16_t len)
+{
+    uint8_t temp[512];
+    uint16_t page, offset;
+    uint16_t i, j, k;
+    
+    if (addr > 4096 * 512) return;
+    
+    page = addr / 512;
+    offset = addr % 512;
+    
+    at45db_read_page(page, temp);
+    for (i = offset; (len != 0) && (i < 512); len--, i++) {
+        temp[i] = *pdata++;
+    }
+    at45db_write_page(page++, temp);
+    
+    if ((i == 512) && (len != 0)) {
+        if (len > 512) {
+            for (k = 0; k < len / 512; k++) {
+                for (j = 0; j < 512; j++) {
+                    temp[j] = *pdata++;
+                }
+                at45db_write_page(page++, temp);
+            }
+            len -= 512 * k;
+        }
+        
+        if (len != 0) {
+            at45db_read_page(page, temp);
+            for (j = 0; j < len; j++) {
+                temp[j] = *pdata++;
+            }
+            at45db_write_page(page, temp);
+        }
+    }
+}
+
+/**
+ * @brief 写入浮点数
+ */
+void at45db_write_float(uint32_t addr, float value)
+{
+    f_bytes data;
+    data.value = value;
+    at45db_write_bytes(addr, &data.byte[0], 4);
+}
+
+/**
+ * @brief 读取浮点数
+ */
+float at45db_read_float(uint32_t addr)
+{
+    f_bytes data;
+    data.value = 0;
+    at45db_read_bytes(addr, &data.byte[0], 4);
+    return data.value;
+}
+
+/**
+ * @brief 写入16位整数
+ */
+void at45db_write_int16(uint32_t addr, int16_t value)
+{
+    i_bytes data;
+    data.value = value;
+    at45db_write_bytes(addr, &data.byte[0], 2);
+}
+
+/**
+ * @brief 读取16位整数
+ */
+int16_t at45db_read_int16(uint32_t addr)
+{
+    i_bytes data;
+    data.value = 0;
+    at45db_read_bytes(addr, &data.byte[0], 2);
+    return data.value;
+}
+#endif

controller_yy_app/hardware/hard_flash_gd25q16.c

@@ -0,0 +1,374 @@
+#include "hard_flash_gd25q16.h"
+#include "board.h"
+#include "hpm_spi_drv.h"
+#include "hpm_clock_drv.h"
+#include "hpm_gpio_drv.h"
+
+#ifdef GD25Q16_FLASH
+
+/* 使用 SPI2 作为示例，你可以根据需要修改 */
+#define AT45DB_SPI_BASE          HPM_SPI2
+/******************SPI时钟频率*******************/
+#define AT45DB_CLK_FRE           21000000
+/* GPIO 参数定义 - 根据实际硬件修改 */
+#define AT45DB_CS_GPIO_PTR       HPM_GPIO0    /* GPIO 控制器基地址 */
+#define AT45DB_CS_PORT           1            /* GPIOB = 1 */
+#define AT45DB_CS_PIN            12           /* PIN12 */
+/* CS 控制宏 - 完全保留原名 */
+#define AT45db161_SPI_SELECT()   gpio_write_pin(AT45DB_CS_GPIO_PTR, AT45DB_CS_PORT, AT45DB_CS_PIN, 0)
+#define AT45db161_SPI_DESELECT() gpio_write_pin(AT45DB_CS_GPIO_PTR, AT45DB_CS_PORT, AT45DB_CS_PIN, 1)
+void gd25q16_cs_high()
+{
+  AT45db161_SPI_DESELECT();
+}
+
+void gd25q16_cs_low()
+{
+  AT45db161_SPI_SELECT();
+}
+
+/* 静态变量 */
+static uint32_t spi_clk_freq;
+
+/**
+ * @brief SPI 读写一个字节 - 保留原名
+ */
+uint8_t SPI2_ReadWrite_Byte(uint8_t byte)
+{
+    uint8_t rx_byte = 0;
+    spi_control_config_t control_config;
+    
+    /* 配置传输控制 */
+    spi_master_get_default_control_config(&control_config);
+    control_config.common_config.trans_mode = spi_trans_write_read_together;
+    control_config.master_config.cmd_enable = false;
+    control_config.master_config.addr_enable = false;
+    
+    /* 执行传输 */
+    spi_transfer(AT45DB_SPI_BASE, 
+                 &control_config,
+                 NULL, NULL,
+                 &byte, 1, 
+                 &rx_byte, 1);
+    
+    return rx_byte;
+}
+
+/**
+ * @brief 写使能 - 保留为静态函数，不暴露给外部
+ */
+static void GD25Q16_WriteEnable(void)
+{
+    AT45db161_SPI_SELECT();
+    SPI2_ReadWrite_Byte(GD25Q16_WRITE_ENABLE);
+    AT45db161_SPI_DESELECT();
+}
+
+/**
+ * @brief 扇区擦除 - 保留原名
+ */
+static void GD25Q16_Erase_Sector(uint32_t addr)
+{
+    while(AT45DB_IS_BUSY() == 0);
+    GD25Q16_WriteEnable();
+    while(AT45DB_IS_BUSY() == 0);
+    AT45db161_SPI_SELECT();
+    SPI2_ReadWrite_Byte(GD25Q16SECTOR_ERASE);
+    SPI2_ReadWrite_Byte((uint8_t)(addr >> 16));
+    SPI2_ReadWrite_Byte((uint8_t)(addr >> 8));
+    SPI2_ReadWrite_Byte((uint8_t)(addr));
+    AT45db161_SPI_DESELECT();
+    while(AT45DB_IS_BUSY() == 0);
+}
+
+/**
+ * @brief 初始化 SPI 接口 - 保留原名
+ */
+void AT45db161_SPI_Configuration(void)
+{
+    spi_timing_config_t timing_config = {0};
+    spi_format_config_t format_config = {0};
+    
+    /* 1. 初始化 SPI 时钟和引脚 */
+    spi_clk_freq = board_init_spi_clock(AT45DB_SPI_BASE);
+    board_init_spi_pins(AT45DB_SPI_BASE);
+    
+    /* 2. 配置 CS 引脚为输出 */
+    gpio_set_pin_output(AT45DB_CS_GPIO_PTR, AT45DB_CS_PORT, AT45DB_CS_PIN);
+    AT45db161_SPI_DESELECT();  /* CS 默认高电平 */
+    
+    /* 3. 配置 SPI 时序（主模式）*/
+    spi_master_get_default_timing_config(&timing_config);
+    timing_config.master_config.clk_src_freq_in_hz = spi_clk_freq;
+    timing_config.master_config.sclk_freq_in_hz = AT45DB_CLK_FRE;
+    spi_master_timing_init(AT45DB_SPI_BASE, &timing_config);
+    
+    /* 4. 配置 SPI 格式 */
+    spi_master_get_default_format_config(&format_config);
+    format_config.common_config.data_len_in_bits = 8;      /* 8位数据 */
+    format_config.common_config.mode = spi_master_mode;    /* 主机模式 */
+    format_config.common_config.cpol = spi_sclk_low_idle;  /* CPOL=0 */
+    format_config.common_config.cpha = spi_sclk_sampling_odd_clk_edges; /* CPHA=0 */
+    spi_format_init(AT45DB_SPI_BASE, &format_config);
+}
+
+/**
+ * @brief 初始化 flash 接口 - 保留原名
+ */
+void flash_at45db_init(void)
+{
+    AT45db161_SPI_Configuration();
+}
+
+/**
+ * @brief 查询是否准备好 - 保留原名
+ */
+uint8_t AT45DB_IS_BUSY(void)
+{
+    uint8_t Status_Register;
+
+    AT45db161_SPI_SELECT();
+    SPI2_ReadWrite_Byte(GD25Q16_READ_STATE_REGISTER);
+    Status_Register = SPI2_ReadWrite_Byte(0x00);
+    AT45db161_SPI_DESELECT();
+    
+    /* GD25Q16: bit0=0 表示空闲，bit0=1 表示忙 */
+    /* 原代码返回1表示准备好，所以需要取反 */
+    if ((Status_Register & 0x01) != 0x01)
+        return 1; /* 准备好 */
+    return 0;     /* 忙 */
+}
+
+/**
+ * @brief 检查芯片是否存在 - 保留原名
+ */
+uint8_t AT45DB_Check(void)
+{
+    uint8_t buf[3];
+    
+    AT45db161_SPI_SELECT();
+    SPI2_ReadWrite_Byte(0x9F); /* 读JEDEC ID */
+    buf[0] = SPI2_ReadWrite_Byte(0);
+    buf[1] = SPI2_ReadWrite_Byte(0);
+    buf[2] = SPI2_ReadWrite_Byte(0);
+    AT45db161_SPI_DESELECT();
+    
+    /* GD25Q16 的 ID 是 0xC8, 0x40, 0x15 */
+    if ((buf[0] == 0xC8) && (buf[1] == 0x40) && (buf[2] == 0x15))
+        return 1;
+    return 0;
+}
+
+/**
+ * @brief 读一页数据 - 保留原名
+ */
+void AT45DB_ReadPage(uint16_t Page_Add, uint8_t *pdata)
+{
+    int i;
+    if (Page_Add > 8192)
+        return;
+    
+    while (AT45DB_IS_BUSY() == 0);
+    
+    uint32_t addr = Page_Add * 256;
+    
+    AT45db161_SPI_SELECT();
+    SPI2_ReadWrite_Byte(GD25Q16_MM_PAGE_READ);
+    SPI2_ReadWrite_Byte((int8_t)(addr >> 16));
+    SPI2_ReadWrite_Byte((int8_t)(addr >> 8));
+    SPI2_ReadWrite_Byte((int8_t)(addr));
+
+    for (i = 0; i < 256; i++) {
+        *pdata++ = SPI2_ReadWrite_Byte(0x00);
+    }
+    AT45db161_SPI_DESELECT();
+}
+
+/**
+ * @brief 写一页数据 - 保留原名
+ */
+void AT45DB_WritePage(uint16_t page, uint8_t *Data)
+{
+    uint16_t i;
+    uint32_t addr = page * 256;
+    
+    if (page >= 8192)
+        return;
+    
+    while (AT45DB_IS_BUSY() == 0);
+    
+    GD25Q16_WriteEnable();
+    while (AT45DB_IS_BUSY() == 0);
+    
+    AT45db161_SPI_SELECT();
+    SPI2_ReadWrite_Byte(GD25Q16_WRITE);
+    SPI2_ReadWrite_Byte((uint8_t)(addr >> 16));
+    SPI2_ReadWrite_Byte((uint8_t)(addr >> 8));
+    SPI2_ReadWrite_Byte((uint8_t)(addr));
+    
+    for (i = 0; i < 256; i++) {
+        SPI2_ReadWrite_Byte(Data[i]);
+    }
+    AT45db161_SPI_DESELECT();
+    
+    while (AT45DB_IS_BUSY() == 0);
+}
+
+/**
+ * @brief 在一页内写入部分数据 - 保留原名
+ */
+void AT45DB_WriteBytes_OnOnePage(uint16_t page, uint8_t *Data, uint16_t len)
+{
+    uint16_t i;
+    
+    if (page > 4095 || len > 256) return;
+    
+    while (AT45DB_IS_BUSY() == 0);
+
+    AT45db161_SPI_SELECT();
+    SPI2_ReadWrite_Byte(AT45DB_BUFFER_2_WRITE);
+    SPI2_ReadWrite_Byte(0x00);
+    SPI2_ReadWrite_Byte(0x00);
+    SPI2_ReadWrite_Byte(0x00);
+    
+    for (i = 0; i < len; i++) {
+        SPI2_ReadWrite_Byte(Data[i]);
+    }
+    AT45db161_SPI_DESELECT();
+
+    while (AT45DB_IS_BUSY() == 0);
+    
+    if (page < 4096) {
+        AT45db161_SPI_SELECT();
+        SPI2_ReadWrite_Byte(GD25Q16_MM_PAGE_PROG_WITH_ERASE);
+        SPI2_ReadWrite_Byte((uint8_t)(page >> 6));
+        SPI2_ReadWrite_Byte((uint8_t)(page << 2));
+        SPI2_ReadWrite_Byte(0x00);
+        AT45db161_SPI_DESELECT();
+    }
+}
+
+/**
+ * @brief 从任意地址读取多个字节 - 保留原名
+ */
+void AT45DB_Read_Bytes(uint32_t add, uint8_t *pdata, uint16_t len)
+{
+    int i, j, k;
+    uint8_t temp[256];
+    uint16_t page, offset;
+    
+    if (add > 8192 * 256) return;
+    
+    page = add / 256;
+    offset = add % 256;
+
+    AT45DB_ReadPage(page++, temp);
+    for (i = offset; (len != 0) && (i < 256); len--, i++) {
+        *pdata++ = temp[i];
+    }
+
+    if ((i == 256) && (len != 0)) {
+        if (len > 256) {
+            for (k = 0; k < len / 256; k++) {
+                AT45DB_ReadPage(page++, temp);
+                for (j = 0; j < 256; j++) {
+                    *pdata++ = temp[j];
+                }
+            }
+            len -= 256 * k;
+        }
+        if (len != 0) {
+            AT45DB_ReadPage(page, temp);
+            for (j = 0; j < len; j++) {
+                *pdata++ = temp[j];
+            }
+        }
+    }
+}
+
+/**
+ * @brief 从任意地址写入多个字节 - 保留原名
+ */
+void AT45DB_Write_Bytes(uint32_t add, uint8_t *pdata, uint16_t len)
+{
+    uint8_t temp[256];
+    uint16_t page, offset;
+    uint16_t i, j, k;
+    
+    if (add > 8192 * 256) return;
+    
+    page = add / 256;
+    offset = add % 256;
+
+    AT45DB_ReadPage(page, temp);
+    for (i = offset; (len != 0) && (i < 256); len--, i++) {
+        temp[i] = *pdata++;
+    }
+    
+    GD25Q16_Erase_Sector(add);
+    AT45DB_WritePage(page++, temp);
+
+    if ((i == 256) && (len != 0)) {
+        if (len > 256) {
+            for (k = 0; k < len / 256; k++) {
+                for (j = 0; j < 256; j++) {
+                    temp[j] = *pdata++;
+                }
+                AT45DB_WritePage(page++, temp);
+            }
+            len -= 256 * k;
+        }
+
+        if (len != 0) {
+            AT45DB_ReadPage(page, temp);
+            for (j = 0; j < len; j++) {
+                temp[j] = *pdata++;
+            }
+            AT45DB_WritePage(page, temp);
+        }
+    }
+}
+
+/**
+ * @brief 写入浮点数 - 保留原名
+ */
+void AT45DB_Write_float(uint32_t add, float wvalue)
+{
+    f_bytes data;
+    data.value = wvalue;
+    AT45DB_Write_Bytes(add, &data.byte[0], 4);
+}
+
+/**
+ * @brief 读取浮点数 - 保留原名
+ */
+float AT45DB_Read_float(uint32_t add)
+{
+    f_bytes data;
+    data.value = 0;
+    AT45DB_Read_Bytes(add, &data.byte[0], 4);
+    return data.value;
+}
+
+/**
+ * @brief 写入16位整数 - 保留原名
+ */
+void AT45DB_Write_int16(uint32_t add, int16_t wvalue)
+{
+    i_bytes data;
+    data.value = wvalue;
+    AT45DB_Write_Bytes(add, &data.byte[0], 2);
+}
+
+/**
+ * @brief 读取16位整数 - 保留原名
+ */
+int16_t AT45DB_Read_int16(uint32_t add)
+{
+    i_bytes data;
+    data.value = 0;
+    AT45DB_Read_Bytes(add, &data.byte[0], 2);
+    return data.value;
+}
+
+#endif /* GD25Q16_FLASH */

controller_yy_app/hardware/hard_hdma_int.c

@@ -0,0 +1,24 @@
+#include "hard_hdma_int.h"
+#include "hpm_dma_drv.h"
+#include "board.h"
+
+volatile bool uart2_tx_dma_done;
+volatile bool uart3_tx_dma_done;
+/*--------------------------------------------------------------------------*/
+/* DMA 发送完成中断服务程序                                                */
+/*--------------------------------------------------------------------------*/
+SDK_DECLARE_EXT_ISR_M(IRQn_HDMA, dma_isr)
+void dma_isr(void)
+{   
+    volatile hpm_stat_t stat_tx_chn;
+    stat_tx_chn = dma_check_transfer_status(SBUS_UART2_DMA_CONTROLLER, SBUS_UART2_TX_DMA_CH);
+    if (stat_tx_chn & DMA_CHANNEL_STATUS_TC) { 
+        uart2_tx_dma_done = true;
+    }
+
+    stat_tx_chn = dma_check_transfer_status(IMU_UART3_DMA_CONTROLLER, IMU_UART3_TX_DMA_CH);
+    if (stat_tx_chn & DMA_CHANNEL_STATUS_TC) {
+        uart3_tx_dma_done = true;
+
+    }
+}

controller_yy_app/hardware/hard_imu_uart3.c

@@ -0,0 +1,296 @@
+#include "board.h"
+#include "hard_imu_uart3.h"
+#include "hpm_dma_drv.h"
+#include "hpm_dmamux_drv.h"
+// #include "hard_uart_rx_dma_soft_idle.h" // 软件触发 模拟空闲中断 需要连接触发物理接口和rx 且需要2路定时器通道 
+#include "hpm_uart_drv.h"
+#include "hard_hdma_int.h"
+#include "test.h"
+/* uart0 给控制台打印串口使用了 尽量避开
+  uart should configure pin function before opening clock 
+  1、IO 2、时钟 3、串口 
+  4、手册：源数据 burst 数量，burst 传输的字节数等于（SrcBurstSize * SrcWidth） 这个用法要看手册 单次允许DMA调度传输的最大字节数
+  5、AHB外设只能用HDMA??? 31-0 TRANSIZE 源数据的数据量，总传输数据量为（TranSize * SrcWidth）
+
+*/
+/* hpm6750没有串口空闲中断 无法使用串口空闲加DMA进行接收 所以接收方面使用软件超时
+  增加阻塞的读中断次数代替DMA传输 由于增加了FIFO，串口也不会像之前那样频繁中断 假如 thr 12 就是接收12个字节进入一次接收中断 
+  #define UART_SOC_FIFO_SIZE       (16U)
+  设置的触发中单的长度要小于FIFO长度 不然会导致丢数据 3/4 12字节 以628M主频 512字节 中断43次来说 115200 波特率 要 44.4ms（传输） + 0.014ms（中断） ≈ 44.4ms。
+  如果换成460800​ 波特率，处理 512字节​ 数据包的总时间约为 11.1ms。如果每次都是512字节 这对于内环400hz 不成立
+  后面看了imu接收的姿态信息是 大概70-80字节 数据包是70字节，在460800波特率下，传输时间约为 1.52ms 也就是不到2ms的姿态包 
+
+  
+*/
+/* ========== 硬件配置 - 根据实际硬件修改 ========== */
+#define IMU_UART3                   HPM_UART3           /* 使用 UART3 */
+#define IMU_UART3_CLK_NAME          clock_uart3         /* UART3 时钟 */
+#define IMU_UART3_IRQ               IRQn_UART3          /* UART3 中断号*/
+#define IMU_UART3_IRQ_RANK          2
+#define IMU_UART3_BAUD              460800
+
+/*===================软空闲中断 DMA接收 额外配置============================*/
+
+//#define IMU_UART3_RX_TRGM                     HPM_TRGM2
+//#define IMU_UART3_RX_TRGM_PIN                 IOC_PAD_PD19
+//#define IMU_UART3_RX_TRGM_INPUT_SRC           HPM_TRGM2_INPUT_SRC_TRGM2_P9 /* Corresponding to Pin setting */
+//#define IMU_UART3_RX_TRGM_OUTPUT_GPTMR_IN     HPM_TRGM2_OUTPUT_SRC_GPTMR4_IN2 /* Corresponding to GPTMR inst */
+//#define IMU_UART3_RX_TRGM_OUTPUT_GPTMR_SYNCI  HPM_TRGM2_OUTPUT_SRC_GPTMR4_SYNCI /* Corresponding to GPTMR inst */
+
+//#define IMU_UART3_RX_GPTMR                    HPM_GPTMR4
+//#define IMU_UART3_RX_GPTMR_CLK_NAME           clock_gptmr4
+//#define IMU_UART3_RX_GPTMR_CMP_CHANNEL        IRQn_GPTMR4
+//#define IMU_UART3_RX_GPTMR_CAPTURE_CHANNEL    0
+//#define IMU_UART3_RX_GPTMR_IRQ                2
+
+/* ========== 缓冲区定义 ========== */
+/* DMA 缓冲区必须放在非缓存区域 */
+ATTR_PLACE_AT_NONCACHEABLE uint8_t USART3_DMA_Recieve_Buf[UART3_RECIEVE_LENDTH] = {0};
+// ATTR_PLACE_AT_NONCACHEABLE uint8_t dma_buf[UART3_RECIEVE_LENDTH] = {0};
+
+ATTR_PLACE_AT_NONCACHEABLE uint8_t USART3_Tx_Buf[UART3_TX_MAX_LENGTH] = {0};
+
+/* ========== 静态变量 ========== */
+static volatile uint32_t length;
+static volatile bool rec_succ = false;
+
+static volatile bool uart3_rx_fifo_timeout;
+static volatile uint16_t rx3_interrupt_count;
+/* ========== 静态函数声明 ========== */
+static  void  Initial_UART3(unsigned int bps);
+static void DMA_UART3Config(void);
+static void UART3NVIC_Configuration(void);
+
+/* ========== 函数实现 ========== */
+
+/**
+ * @brief IMU UART3 初始化 - 保留原名
+ */
+void imu_uart3_init(unsigned int bps)
+{
+    Initial_UART3(bps);
+    DMA_UART3Config();
+    UART3NVIC_Configuration();
+}
+
+
+static void uart3_pin_config(void)
+{
+  HPM_IOC->PAD[IOC_PAD_PB24].FUNC_CTL = IOC_PB24_FUNC_CTL_UART3_RXD;
+  HPM_IOC->PAD[IOC_PAD_PB25].FUNC_CTL = IOC_PB25_FUNC_CTL_UART3_TXD;
+}
+/**
+ * @brief 初始化 UART3 接口 - 保留原名
+ */
+static void Initial_UART3(unsigned int bps)
+{
+    uart_config_t uart_config = {0};
+ 
+    /* 配置IO 使能 UART3 时钟 */
+
+    uart3_pin_config();
+  
+    clock_set_source_divider(IMU_UART3_CLK_NAME, clk_src_osc24m, 1);
+    clock_add_to_group(IMU_UART3_CLK_NAME, 0);
+   
+    /* 获取默认 UART 配置 */
+    uart_default_config(IMU_UART3, &uart_config);
+    
+    /* 修改配置 */
+    uart_config.baudrate = bps;
+    uart_config.word_length = word_length_8_bits;   /* 8 位数据 */
+    uart_config.num_of_stop_bits = stop_bits_1;     /* 1 位停止位 */
+    uart_config.parity = parity_none;               /* 无校验 */
+    uart_config.fifo_enable = true;                       /* 使能 FIFO */
+    uart_config.dma_enable = true;                         /* 使能 DMA */
+    uart_config.tx_fifo_level = uart_tx_fifo_trg_not_full; // 1个字节起搬 DMA->FIFO->UART输出
+    uart_config.rx_fifo_level = uart_rx_fifo_trg_gt_three_quarters; // 16*3/4 = 12字节FIFO 对于512字节的缓冲区需要 42到43次中断 
+    uart_config.src_freq_in_hz = clock_get_frequency(IMU_UART3_CLK_NAME);
+
+    /* 初始化 UART */
+    hpm_stat_t stat = uart_init(IMU_UART3, &uart_config);
+    if (stat != status_success) {
+        /* uart failed to be initialized */
+        printf("failed to initialize uart\n");
+        while(1);
+    }
+    
+
+  
+}
+
+
+/**
+ * @brief 串口3 DMA 初始化配置
+ */
+void DMA_UART3Config(void)
+{
+    dma_handshake_config_t handshake_config;
+     dma_channel_config_t channel_config;
+    /* 使能 DMA 时钟 */
+    // HDMA 时钟来源于系统总线时钟（AHB）
+    
+    /* ========== 配置 TX DMA 通道 ========== */
+    /* 初始化 DMA 通道 */
+    dma_default_channel_config(IMU_UART3_DMA_CONTROLLER, &channel_config);
+    
+    /* 配置 DMAMUX：将 UART3 TX 请求连接到 TX DMA 通道 */
+    dmamux_config(IMU_UART3_DMAMUX_CONTROLLER, IMU_UART3_TX_DMAMUX_CH, IMU_UART3_TX_DMA_REQ, true);
+    
+    /* 配置 TX 握手参数 */
+    dma_default_handshake_config(IMU_UART3_DMA_CONTROLLER, &handshake_config);
+    handshake_config.ch_index = IMU_UART3_TX_DMA_CH;
+    handshake_config.dst = (uint32_t)&IMU_UART3->THR;        /* 目标：发送寄存器 */
+    handshake_config.dst_fixed = true;                           /* 目标地址固定 */
+    handshake_config.src = core_local_mem_to_sys_address(0, (uint32_t)USART3_Tx_Buf); /* 源：内存 */ // 建议4字节对齐
+    handshake_config.src_fixed = false;                          /* 源地址递增 */
+    handshake_config.data_width = DMA_TRANSFER_WIDTH_BYTE;       /* 字节传输 */
+    handshake_config.size_in_byte = UART3_TX_MAX_LENGTH;         /* 传输大小 */
+    
+   hpm_stat_t stat = dma_setup_handshake(IMU_UART3_DMA_CONTROLLER, &handshake_config, false); // 不触发单次发送搬运
+   if(stat != status_success)
+   {
+      printf("uart3 dma tx config error\r\n");
+   }
+    
+}
+
+/**
+ * @brief 初始化 USART3 中断优先级 - 保留原名
+ */
+void UART3NVIC_Configuration(void)
+{
+/*
+    uart_intr_rx_data_avail_or_timeout  // 接收缓冲区非空 或 超时（FIFO）
+    uart_intr_tx_slot_avail  // 发送缓冲区空
+    uart_intr_rx_line_stat  // 接收线状态
+    uart_intr_modem_stat // 模式状态（流控等）
+*/
+    uart_enable_irq(IMU_UART3, uart_intr_rx_data_avail_or_timeout); //  接收中断
+    intc_m_enable_irq_with_priority(IMU_UART3_IRQ, IMU_UART3_IRQ_RANK);
+    intc_m_enable_irq_with_priority(IMU_UART3_DMA_IRQ, IMU_UART3_DMA_IRQ_RANK);
+}
+
+/**
+ * @brief UART3 发送 1 字节数据  
+ */
+void UART3_Put_Char(unsigned char data)
+{
+    /* 等待发送寄存器空 */
+    while (!uart_check_status(IMU_UART3, uart_stat_transmitter_empty));
+    
+    /* 发送数据 */
+    uart_send_byte(IMU_UART3, data);
+    
+    /* 等待发送完成 */
+    while (!uart_check_status(IMU_UART3, uart_stat_transmitter_empty));
+}
+
+/**
+ * @brief 开启 USART3 DMA 发送 - 保留原名
+ */
+void open_usart3_dma_tx(unsigned short count)
+{
+    dma_handshake_config_t handshake_config;
+    
+    /* 等待上次发送完成 */
+    // 软件dma传输完成标志 每次调用open_usart3_dma_tx会重新置位硬件TC
+    uint32_t timeout = 1000000;
+    while (usart3_tx_isbusy() && timeout--) {
+        __asm("nop");
+    }
+    if (timeout == 0) {
+        // 超时处理，复位DMA
+        dma_abort_channel(IMU_UART3_DMA_CONTROLLER, IMU_UART3_TX_DMA_CH);
+        uart3_tx_dma_done = true;  // 强制完成
+    }  
+    /* 重新配置 TX 传输大小 */
+    dma_default_handshake_config(IMU_UART3_DMA_CONTROLLER, &handshake_config);
+    handshake_config.ch_index = IMU_UART3_TX_DMA_CH;
+    handshake_config.dst = (uint32_t)&IMU_UART3->THR;
+    handshake_config.dst_fixed = true;
+    handshake_config.src = core_local_mem_to_sys_address(0, (uint32_t)USART3_Tx_Buf);
+    handshake_config.src_fixed = false;
+    handshake_config.data_width = DMA_TRANSFER_WIDTH_BYTE;
+    handshake_config.size_in_byte = count;
+    
+    dma_setup_handshake(IMU_UART3_DMA_CONTROLLER, &handshake_config, true); // true表示触发一次
+    
+   
+}
+
+/**
+ * @brief 获取串口发送是否忙碌
+ */
+bool usart3_tx_isbusy(void)
+{
+     return !uart3_tx_dma_done;  // 如果没完成，就是忙碌
+}
+
+/* ========== 中断服务函数 ========== */
+extern void recv_imu_data_hookfunction(const uint8_t *pbuffer,
+                                       uint32_t rx_length);
+
+
+
+SDK_DECLARE_EXT_ISR_M(IMU_UART3_IRQ, uart3_isr)
+void uart3_isr(void)
+{
+   uint8_t count = 0;
+   uint8_t irq_id = uart_get_irq_id(IMU_UART3);
+    if (irq_id == uart_intr_id_rx_data_avail) {
+        rx3_interrupt_count++;
+        while (uart_check_status(IMU_UART3, uart_stat_data_ready)) {
+            USART3_DMA_Recieve_Buf[length++] = uart_read_byte(IMU_UART3);
+            count++;
+             /*in order to ensure rx fifo there are remaining bytes*/
+            if (count > 12) {
+                break;
+            }
+        }
+    }
+    if (irq_id == uart_intr_id_rx_timeout) { // 接收超时 FIFO超时4个字符后代表一次接收完毕
+        // 接收完毕
+        rx3_interrupt_count++;
+        while (uart_check_status(IMU_UART3, uart_stat_data_ready)) {
+            USART3_DMA_Recieve_Buf[length++] = uart_read_byte(IMU_UART3);
+        }
+        // recv_imu_data_hookfunction((uint8_t *)USART3_DMA_Recieve_Buf, length);
+        memset(USART3_Tx_Buf,0,sizeof(USART3_Tx_Buf));
+        memcpy(USART3_Tx_Buf, USART3_DMA_Recieve_Buf,length);
+        rec_succ = true;
+        length = 0;
+        // uart_rx_fifo_timeout = true;
+    }
+
+}
+
+
+/* uart dma rx_irq  tx_dma demo 2026/03/14 pass*/
+#ifdef UART3_TEST
+
+void imu_uart3_test(void)
+{
+  // char c = 0;
+  imu_uart3_init(IMU_UART3_BAUD);
+
+  while(1)
+  {
+      // c++;
+      board_delay_ms(500);
+      if(rec_succ)
+      {
+        rec_succ = false;
+       
+        // printf("    putchar:%s \r\n",USART3_Tx_Buf);
+        open_usart3_dma_tx(sizeof(USART3_Tx_Buf));
+        // memset(USART3_Tx_Buf,0,sizeof(USART3_Tx_Buf));
+      }
+      
+      // UART3_Put_Char(c); // pass
+      // printf("    putchar:%c\r\n",c);
+  }
+
+}
+#endif
+

controller_yy_app/hardware/hard_inc/hard_can.h

@@ -0,0 +1,10 @@
+#ifndef HARD_CAN_H
+#define HARD_CAN_H
+
+#include <stdint.h>
+
+void CAN1_Mode_Init(uint32_t baudrate);
+
+void can2_test(void);
+
+#endif // ! HARD_CAN_H

controller_yy_app/hardware/hard_inc/hard_flash_at45db.h

@@ -0,0 +1,66 @@
+#ifndef __HARD_FLASH_AT45DB_H
+#define __HARD_FLASH_AT45DB_H
+
+#include "ver_config.h"
+
+#ifndef GD25Q16_FLASH
+
+#include "stdint.h"
+
+#define AT45DB_BUFFER_1_WRITE 0x84       /* 写入第一缓冲区 */
+#define AT45DB_BUFFER_2_WRITE 0x87       /* 写入第二缓冲区 */
+#define AT45DB_BUFFER_1_WRITE_FLASH 0x82 /* 写入第一缓冲区 */
+#define AT45DB_BUFFER_2_WRITE_FLASH 0x85 /* 写入第二缓冲区 */
+
+#define AT45DB_BUFFER_1_READ 0xD4 /* 读取第一缓冲区 */
+#define AT45DB_BUFFER_2_READ 0xD6 /* 读取第二缓冲区 */
+#define AT45DB_B1_TO_MM_PAGE_PROG_WITH_ERASE                                   \
+    0x83 /* 将第一缓冲区的数据写入主存储器（擦除模式）*/
+#define AT45DB_B2_TO_MM_PAGE_PROG_WITH_ERASE                                   \
+    0x86 /* 将第二缓冲区的数据写入主存储器（擦除模式）*/
+#define AT45DB_MM_PAGE_TO_B1_XFER                                              \
+    0x53 /* 将主存储器的指定页数据加载到第一缓冲区    */
+#define AT45DB_MM_PAGE_TO_B2_XFER                                              \
+    0x55 /* 将主存储器的指定页数据加载到第二缓冲区    */
+#define AT45DB_PAGE_ERASE 0x81   /* 页删除（每页512/528字节） */
+#define AT45DB_SECTOR_ERASE 0x7C /* 扇区擦除（每扇区128K字节）*/
+#define AT45DB_READ_STATE_REGISTER 0xD7 /* 读取状态寄存器 */
+#define AT45DB_BLACK_ERASE 0x50         /* 块删除(每块4KByte)*/
+#define AT45DB_MM_PAGE_READ 0xD2        /* 直接读主存储器的内存页*/
+#define AT45DB_Continuous_Read 0x0B
+
+#define FLASH_PAGE_TOTAL 4096 // 0..4095
+#define FLASH_PAGE_SIZE 512
+#define FLASH_SIZE_KBYTE 2112
+
+//浮点 联合体
+typedef union {
+    float value;
+    unsigned char byte[4];
+} f_bytes;
+
+//整数 联合体
+typedef union {
+    short value;
+    unsigned char byte[2];
+} i_bytes;
+
+void flash_at45db_init(void);
+
+uint8_t AT45DB_Check(void);
+uint8_t AT45DB_IS_BUSY(void);
+
+void AT45DB_Read_Bytes(uint32_t add, uint8_t *pdata, uint16_t len);
+void AT45DB_Write_Bytes(uint32_t add, uint8_t *pdata, uint16_t len);
+
+int16_t AT45DB_Read_int16(uint32_t add);
+void AT45DB_Write_int16(uint32_t add, int16_t wvalue);
+
+void AT45DB_ReadPage(uint16_t Page_Add, uint8_t *pdata);
+void AT45DB_WritePage(uint16_t page, uint8_t *Data);
+
+void AT45DB_WriteBytes_OnOnePage(uint16_t page, uint8_t *Data, uint16_t len);
+
+#endif
+
+#endif

controller_yy_app/hardware/hard_inc/hard_flash_gd25q16.h

@@ -0,0 +1,75 @@
+
+#pragma once
+
+#include "ver_config.h"
+
+#ifdef GD25Q16_FLASH
+
+#include "stdint.h"
+
+#define AT45DB_BUFFER_1_WRITE 0x84       /* 写入第一缓冲区 */
+#define AT45DB_BUFFER_2_WRITE 0x87       /* 写入第二缓冲区 */
+#define AT45DB_BUFFER_1_WRITE_FLASH 0x82 /* 写入第一缓冲区 */
+#define AT45DB_BUFFER_2_WRITE_FLASH 0x85 /* 写入第二缓冲区 */
+
+#define AT45DB_BUFFER_1_READ 0xD4 /* 读取第一缓冲区 */
+#define AT45DB_BUFFER_2_READ 0xD6 /* 读取第二缓冲区 */
+#define AT45DB_B1_TO_MM_PAGE_PROG_WITH_ERASE                                   \
+    0x83 /* 将第一缓冲区的数据写入主存储器（擦除模式）*/
+#define GD25Q16_MM_PAGE_PROG_WITH_ERASE                                   \
+    0x02 /* 将第二缓冲区的数据写入主存储器（擦除模式）*/
+#define AT45DB_MM_PAGE_TO_B1_XFER                                              \
+    0x53 /* 将主存储器的指定页数据加载到第一缓冲区    */
+#define AT45DB_MM_PAGE_TO_B2_XFER                                              \
+    0x55 /* 将主存储器的指定页数据加载到第二缓冲区    */
+#define AT45DB_PAGE_ERASE 0x81   /* 页删除（每页512/528字节） */
+#define AT45DB_SECTOR_ERASE 0x7C /* 扇区擦除（每扇区128K字节）*/
+#define GD25Q16SECTOR_ERASE 0x20 /* 扇区擦除（每扇区4K字节）*/
+#define GD25Q16_READ_STATE_REGISTER 0X05 /* 读取状态寄存器 */
+#define GD25Q16_WRITE 0x02               /* 写入数据 */
+#define AT45DB_BLACK_ERASE 0x50         /* 块删除(每块4KByte)*/
+#define GD25Q16_MM_PAGE_READ 0X03        /* 直接读主存储器的内存页*/
+#define AT45DB_Continuous_Read 0x0B
+#define GD25Q16_WRITE_ENABLE 0x06       /* 写使能 */
+#define GD25Q16_WRITE_DISABLE 0x04      /* 写禁止 */
+
+#define FLASH_PAGE_TOTAL 4096 // 0..4095
+#define FLASH_PAGE_SIZE 512
+#define FLASH_SIZE_KBYTE 2112
+
+
+
+
+//浮点 联合体
+typedef union {
+    float value;
+    unsigned char byte[4];
+} f_bytes;
+
+//整数 联合体
+typedef union {
+    short value;
+    unsigned char byte[2];
+} i_bytes;
+
+void flash_at45db_init(void);
+
+uint8_t AT45DB_Check(void);
+uint8_t AT45DB_IS_BUSY(void);
+
+void AT45DB_Read_Bytes(uint32_t add, uint8_t *pdata, uint16_t len);
+void AT45DB_Write_Bytes(uint32_t add, uint8_t *pdata, uint16_t len);
+
+int16_t AT45DB_Read_int16(uint32_t add);
+void AT45DB_Write_int16(uint32_t add, int16_t wvalue);
+
+void AT45DB_ReadPage(uint16_t Page_Add, uint8_t *pdata);
+void AT45DB_WritePage(uint16_t page, uint8_t *Data);
+
+void AT45DB_WriteBytes_OnOnePage(uint16_t page, uint8_t *Data, uint16_t len);
+
+void gd25q16_cs_high();
+
+void gd25q16_cs_low();
+
+#endif

controller_yy_app/hardware/hard_inc/hard_hdma_int.h

@@ -0,0 +1,38 @@
+#pragma once
+#include <stdbool.h>
+// DMA UART3
+#define IMU_UART3_TX_DMA_REQ        HPM_DMA_SRC_UART3_TX /* TX DMA 请求号 */
+//#define IMU_UART3_RX_DMA_REQ        HPM_DMA_SRC_UART3_RX /* RX DMA 请求号 */
+
+#define IMU_UART3_DMA_CONTROLLER         HPM_HDMA            /* DMA 控制器 */
+#define IMU_UART3_DMAMUX_CONTROLLER      HPM_DMAMUX          /* DMAMUX 控制器 */
+#define IMU_UART3_TX_DMA_CH        0                   /* TX DMA 通道 */
+//#define IMU_UART3_RX_DMA_CH        1                   /* RX DMA 通道 */
+
+
+#define IMU_UART3_TX_DMAMUX_CH       DMA_SOC_CHN_TO_DMAMUX_CHN(IMU_UART3_DMA_CONTROLLER, IMU_UART3_TX_DMA_CH)
+//#define IMU_UART3_RX_DMAMUX_CH     DMA_SOC_CHN_TO_DMAMUX_CHN(IMU_UART3_DMA_CONTROLLER, IMU_UART3_RX_DMA_CH)
+
+#define IMU_UART3_DMA_IRQ           IRQn_HDMA           /* DMA 中断号 */
+#define IMU_UART3_DMA_IRQ_RANK      1
+
+//
+// DMA UART2
+#define SBUS_UART2_TX_DMA_REQ        HPM_DMA_SRC_UART2_TX /* TX DMA 请求号 */
+//#define SBUS_UART2_RX_DMA_REQ        HPM_DMA_SRC_UART2_RX /* RX DMA 请求号 */
+
+#define SBUS_UART2_DMA_CONTROLLER         HPM_HDMA            /* DMA 控制器 */
+#define SBUS_UART2_DMAMUX_CONTROLLER      HPM_DMAMUX          /* DMAMUX 控制器 */
+#define SBUS_UART2_TX_DMA_CH              2                   /* TX DMA 通道 */
+//#define SBUS_UART2_RX_DMA_CH        3                   /* RX DMA 通道 */
+
+
+#define SBUS_UART2_TX_DMAMUX_CH       DMA_SOC_CHN_TO_DMAMUX_CHN(SBUS_UART2_DMA_CONTROLLER, SBUS_UART2_TX_DMA_CH)
+//#define SBUS_UART2_RX_DMAMUX_CH     DMA_SOC_CHN_TO_DMAMUX_CHN(SBUS_UART2_DMA_CONTROLLER, SBUS_UART2_RX_DMA_CH)
+
+#define SBUS_UART2_DMA_IRQ           IRQn_HDMA           /* DMA 中断号 */   
+#define SBUS_UART2_DMA_IRQ_RANK      1 
+
+
+extern volatile bool uart2_tx_dma_done;
+extern volatile bool uart3_tx_dma_done;

controller_yy_app/hardware/hard_inc/hard_imu_uart3.h

@@ -0,0 +1,28 @@
+
+#ifndef __HARD_IMU_UART3_H
+#define __HARD_IMU_UART3_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+
+// HDMA 必须4字节对齐
+// 接收IMU数据的DMA数组的长度-定长
+#define UART3_RECIEVE_LENDTH 512
+#define UART3_TX_MAX_LENGTH 512
+
+extern uint8_t USART3_DMA_Recieve_Buf[UART3_RECIEVE_LENDTH];
+extern uint8_t USART3_Tx_Buf[UART3_TX_MAX_LENGTH];
+
+void imu_uart3_init(unsigned int bps);
+void UART3_Put_Char(unsigned char data);
+
+void open_usart3_dma_tx(unsigned short count);
+
+bool usart3_tx_isbusy(void);
+
+void control_board_led_on(void);
+
+void imu_uart3_test(void);
+
+#endif

controller_yy_app/hardware/hard_inc/hard_rc_sbus.h

@@ -0,0 +1,16 @@
+
+#ifndef __HARD_RC_SBUS_H
+#define __HARD_RC_SBUS_H
+
+#include <stdint.h>
+
+#define SBUS_BPS 100000 // SUBS的通信波特率
+
+//这个值要设置的比25大一些。正好是25的话获取的DMA_GetCurrDataCounter(DMA1_Stream5)会是25.
+#define UART2_RX_LENDTH_MAX 30
+extern unsigned char sbus_rx_buf[UART2_RX_LENDTH_MAX];
+
+extern void rc_sbus_init(uint32_t baud);
+// 由于hpm6750没有串口空闲中断， 所以采用串口超时中断的而形式 FIFO深度16字节
+extern void uart2_sbus_test(void);
+#endif

controller_yy_app/hardware/hard_inc/hard_sbus_out.h

@@ -0,0 +1,16 @@
+#ifndef __HARD_SBUS_OUT_H
+#define __HARD_SBUS_OUT_H
+
+#include <stdint.h>
+#include <stdbool.h>
+void sbus_out_init(uint32_t baud);
+void pwm_to_sbus(short *out_pwm, short *out_subs);
+
+void sbusout_af_pwm_init(uint32_t pwm_period);
+void sbus_out_af_pwm_set_pulse_value(uint32_t pulse_value);
+
+void sbus_uart2_out_test(void);
+
+extern volatile bool uart2_tx_dma_done;
+
+#endif

controller_yy_app/hardware/hard_inc/hard_sbusout_af_pump.h

@@ -0,0 +1,11 @@
+
+
+#ifndef __HARD_SBUSOUT_AF_PUMP_H
+#define __HARD_SBUSOUT_AF_PUMP_H
+
+
+void sbusout_af_pump_init(void);
+
+#endif
+
+

controller_yy_app/hardware/hard_inc/hard_sdio_sd.h

@@ -0,0 +1,8 @@
+// 使用官方的SDMMC例程包
+#ifndef  __HARD_SDIO_SD_H
+#define  __HARD_SDIO_SD_H
+
+#include "hpm_sdmmc_sd.h"
+
+
+#endif

controller_yy_app/hardware/hard_inc/hard_system.h

@@ -0,0 +1,11 @@
+#ifndef __HARD_SYSTEM_H
+#define __HARD_SYSTEM_H
+
+
+void sys_reset(void);
+
+void u3_dma_disable(void);
+
+void system_test(void);
+#endif
+

controller_yy_app/hardware/hard_inc/hard_system_delay.h

@@ -0,0 +1,11 @@
+#ifndef __HARD_SYSTEM_DELAY_H
+#define __HARD_SYSTEM_DELAY_H
+
+
+void system_delay_init(unsigned char SYSCLK);
+void system_delay_ms(unsigned short nms);
+void system_delay_us(unsigned int nus);
+void cpu_delay_test(void);
+
+#endif
+

controller_yy_app/hardware/hard_inc/hard_system_time.h

@@ -0,0 +1,10 @@
+#ifndef __HARD_SYSTEM_TIME_H
+#define __HARD_SYSTEM_TIME_H
+
+
+void system_time_init(void);
+
+unsigned int hard_micros(void);
+
+void timer0_test(void);
+#endif

controller_yy_app/hardware/hard_inc/hard_system_timer.h

@@ -0,0 +1,9 @@
+#ifndef __HARD_SYSTEM_TIMER_H
+#define __HARD_SYSTEM_TIMER_H
+
+
+void system_timer_init(void);
+void timer1_test(void);
+
+#endif
+

controller_yy_app/hardware/hard_inc/hard_uart_rx_dma_soft_idle.h

@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2021 HPMicro
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef UART_RX_DMA_IDLE_CONF_H
+#define UART_RX_DMA_IDLE_CONF_H
+
+#include "hpm_trgm_drv.h"
+#include "hpm_trgmmux_src.h"
+#include "hpm_gptmr_drv.h"
+#include "hpm_uart_drv.h"
+#ifdef HPMSOC_HAS_HPMSDK_DMAV2
+#include "hpm_dmav2_drv.h"
+#else
+#include "hpm_dma_drv.h"
+#endif
+#include "hpm_dmamux_drv.h"
+#include "hpm_clock_drv.h"
+typedef struct uart_info {
+    UART_Type *ptr;
+    uint32_t baudrate;
+    uint8_t *buff_addr;
+} uart_info_t;
+
+typedef struct dma_info {
+    DMA_Type *ptr;
+    uint8_t ch;
+    uint8_t *dst_addr;
+    uint32_t buff_size;
+} dma_info_t;
+
+typedef struct dmamux_info {
+    DMAMUX_Type *ptr;
+    uint8_t ch;
+    uint8_t src;
+} dmamux_info_t;
+
+typedef struct trgm_info {
+    TRGM_Type *ptr;
+    uint32_t pin_index;
+    uint8_t input_src;
+    uint8_t output_gptmr_in;
+    uint8_t output_gptmr_synci;
+} trgm_info_t;
+
+typedef struct gptmr_info {
+    GPTMR_Type *ptr;
+    clock_name_t clock_name;
+    uint8_t cmp_ch;
+    uint8_t cap_ch;
+    uint8_t irq_index;
+} gptmr_info_t;
+
+/*! @brief Initailize structure of uart to reveive indefinite length data */
+typedef struct uart_rx_flexiable_data_context {
+    volatile bool uart_rx_idle;
+    volatile uint32_t uart_receive_data_size;
+    uart_info_t uart_info;
+    dma_info_t dma_info;
+    dmamux_info_t dmamux_info;
+    trgm_info_t trgm_info;
+    gptmr_info_t gptmr_info;
+} uart_rx_flexiable_data_context_t;
+
+#endif /* UART_DEMO_CONF_H */
+

controller_yy_app/hardware/hard_rc_subs.c

@@ -0,0 +1,159 @@
+#include "board.h"
+#include "hard_rc_sbus.h"
+#include "stdbool.h"
+#include "hpm_uart_drv.h"
+#include "test.h"
+// 与串口3一致
+// 如果使用发送和接收都是串口2 那就不需要在sbus_out里面再次进行串口初始化
+/*hpm6750没有串口空闲中断 不能使用UART iRQ+DMA 这里采用的是FIFO+超时中断*/
+#define SBUS_UART2_RX                  HPM_UART2
+#define SBUS_UART2_CLK_NAME            clock_uart2
+#define SBUS_UART2_IRQ                 IRQn_UART2
+#define SBUS_UART2_BAUD                115200
+#define SBUS_UART2_IRQ_RANK            2
+
+/* 接收缓冲区 */
+ATTR_PLACE_AT_NONCACHEABLE uint8_t sbus_rx_buf[UART2_RX_LENDTH_MAX];
+
+/* 全局变量 */
+static volatile uint16_t sbus_rx_index = 0;
+static volatile bool sbus_frame_ready = false;
+static volatile uint16_t sbus_frame_length = 0;
+static volatile uint32_t rx_interrupt_count = 0;
+
+/* 外部函数声明 */
+extern void recv_rcsbus_data_hookfunction(unsigned int len, uint8_t *data);
+
+/**
+ * @brief UART中断服务函数
+ */
+SDK_DECLARE_EXT_ISR_M(SBUS_UART2_IRQ, uart2_sbus_isr)
+void uart2_sbus_isr(void)
+{
+    uint8_t irq_id = uart_get_irq_id(SBUS_UART2_RX);
+    uint8_t count = 0;
+    
+    /* 处理FIFO阈值中断 - 批量读取数据 */
+    if (irq_id == uart_intr_id_rx_data_avail) {
+        rx_interrupt_count++;
+        
+        /* 从FIFO中读取数据 */
+        while (uart_check_status(SBUS_UART2_RX, uart_stat_data_ready)) {
+            count++;
+            sbus_rx_buf[sbus_rx_index++] = uart_read_byte(SBUS_UART2_RX);
+            
+            /* 防止缓冲区溢出 */
+            if (sbus_rx_index >= UART2_RX_LENDTH_MAX) {
+                sbus_rx_index = 0;  /* 环形缓冲，溢出后从头开始 */
+            }
+            
+            /*in order to ensure rx fifo there are remaining bytes*/
+            if (count > 12) {
+                break;
+            }
+        }
+    }
+    
+    /* 处理FIFO超时中断 - 一帧数据接收完成 */
+    if (irq_id == uart_intr_id_rx_timeout) {
+        rx_interrupt_count++;
+        
+        /* 读取FIFO中剩余的所有数据 */
+        while (uart_check_status(SBUS_UART2_RX, uart_stat_data_ready)) {
+            sbus_rx_buf[sbus_rx_index++] = uart_read_byte(SBUS_UART2_RX);
+            // 回调解析数据   
+        }
+        // recv_rcsbus_data_hookfunction(sbus_rx_index, sbus_rx_buf);
+        /* 标记一帧SBUS数据接收完成 */
+
+        sbus_frame_ready = true;
+        sbus_frame_length = sbus_rx_index;  /* 记录当前帧长度 */
+        sbus_rx_index = 0;
+    }
+}
+
+static void rc_uart2_pin_config(void) // 如果使用的是同一个串口的rx 和tx 就初始化1次 
+{
+    HPM_IOC->PAD[IOC_PAD_PB21].FUNC_CTL = IOC_PB21_FUNC_CTL_UART2_RXD;
+
+    HPM_IOC->PAD[IOC_PAD_PB22].FUNC_CTL = IOC_PB22_FUNC_CTL_UART2_TXD;
+}
+/**
+ * @brief SBUS初始化
+ */
+void rc_sbus_init(uint32_t baud)
+{
+    uart_config_t config = {0};
+    hpm_stat_t stat;
+    
+    /* 初始化UART */
+    rc_uart2_pin_config();
+
+    clock_set_source_divider(SBUS_UART2_CLK_NAME, clk_src_osc24m, 1);
+    clock_add_to_group(SBUS_UART2_CLK_NAME, 0);
+    uint32_t freq = clock_get_frequency(SBUS_UART2_CLK_NAME);
+    printf("uart2 clk fre %d\r\n", freq);
+    /* UART默认配置 */
+    uart_default_config(SBUS_UART2_RX, &config);
+    
+    /* SBUS参数配置 */
+    config.baudrate = baud;                    /* 100000bps */
+    config.word_length = word_length_8_bits;       /* 9位数据（8位+偶校验） */
+    config.num_of_stop_bits = stop_bits_2;                 /* 2停止位 */
+    config.parity = parity_even;                     /* 偶校验 */
+    config.fifo_enable = true;                       /* 使能FIFO */
+    config.src_freq_in_hz = clock_get_frequency(SBUS_UART2_CLK_NAME);
+    
+    /*  设置FIFO阈值 - 设为接近一帧的长度 要小于实际期望接收的字节数 25 */
+    config.rx_fifo_level = uart_rx_fifo_trg_gt_three_quarters; /* 约24字节 */
+
+    
+    stat = uart_init(SBUS_UART2_RX, &config);
+    if (stat != status_success) {
+        printf("SBUS UART2 RX init failed\n");
+        while (1);
+    }
+    
+    /* 使能UART中断（FIFO阈值+超时） */
+    uart_enable_irq(SBUS_UART2_RX, uart_intr_rx_data_avail_or_timeout);
+    
+    /* 使能中断控制器 */
+    intc_m_enable_irq_with_priority(SBUS_UART2_IRQ, SBUS_UART2_IRQ_RANK);
+    
+    /* 初始化变量 */
+    sbus_rx_index = 0;
+    sbus_frame_ready = false;
+    rx_interrupt_count = 0;
+    
+    printf("SBUS initialized with FIFO + timeout interrupt\n");
+}
+
+
+/* uart2 rx_irq  demo 2026/03/15 pass*/
+#ifdef UART2_RX_TEST
+
+void uart2_sbus_test(void)
+{
+  // char c = 0;
+  rc_sbus_init(SBUS_UART2_BAUD);
+
+  while(1)
+  {
+      // c++;
+      board_delay_ms(500);
+      if(sbus_frame_ready)
+      {
+        sbus_frame_ready = false;
+
+        while(sbus_frame_length--)
+        {
+            printf(" %c ",sbus_rx_buf[sbus_frame_length]);
+        }
+      }
+    
+      // UART3_Put_Char(c); // pass
+    
+  }
+
+}
+#endif

controller_yy_app/hardware/hard_sbus_out.c

@@ -0,0 +1,408 @@
+#include "hard_rc_sbus.h"
+#include "stdbool.h"
+#include "board.h"
+#include "hpm_uart_drv.h"
+#include "hpm_pwm_drv.h"
+#include "hpm_dma_drv.h"
+#include "hpm_dmamux_drv.h"
+#include "hard_sbus_out.h"
+#include "test.h"
+#include "hard_hdma_int.h"
+#include "main.h"
+/* 实际项目没用到该接口 没有发送sbus
+*/
+#define SBUS_UART2_TX                  HPM_UART2
+#define SBUS_UART2_CLK_NAME            clock_uart2
+#define SBUS_UART2_IRQ                 IRQn_UART2
+#define SBUS_UART2_BAUD                115200
+#define SBUS_UART2_IRQ_RANK            2
+
+/* SBUS PWM CONFIG*/
+
+#define SBUS_PWM0              HPM_PWM0
+#define SBUS_PWM0_CLOCK_NAME     
+#define SBUS_PWM0_OUT          1  
+#define SBUS_PWM0_CMP          0  // pwm通道关联的cmp通道 
+
+
+
+/* 发送缓冲区大小*/
+#define UART2_TX_LENDTH_MAX 25
+ATTR_PLACE_AT_NONCACHEABLE static uint8_t uart2_dma_tx_buf[UART2_TX_LENDTH_MAX] = {0};
+
+
+/* 静态函数声明 */
+static void Initial_UART2_TX(uint32_t baudrate);
+static void UART2_TX_DMA_Config(void);
+static void UART2_TX_NVIC_Config(void);
+/**
+ * @brief 开启PWM输出 要求来自同一个pwm 不然都得重新配置
+ */
+
+static void sbusout_af_pwm_gpio_config(void)
+{
+   
+    HPM_IOC->PAD[IOC_PAD_PC00].FUNC_CTL = IOC_PC00_FUNC_CTL_PWM0_P_1; // 使用PWM0 1通道
+}
+
+static void sbusout_af_pwm_timer_config(uint32_t sbus_af_pwm_period)
+{
+    pwm_cmp_config_t cmp_config = {0};
+    pwm_config_t pwm_config = {0};
+
+    // 准备pwm_config
+    pwm_get_default_pwm_config(SBUS_PWM0, &pwm_config); // 填充 pwm_config_t 默认值
+    pwm_config.enable_output = true;
+    pwm_config.dead_zone_in_half_cycle = 0;
+#ifdef WAVE_INV 
+    pwm_config.invert_output = true;
+#else
+    pwm_config.invert_output = false;
+#endif
+    // 准备cmp_config
+    pwm_get_default_cmp_config(SBUS_PWM0, &cmp_config); // 填充 pwm_cmp_config_t 默认值
+    cmp_config.mode = pwm_cmp_mode_output_compare; // 设置PWM工作模式为输出
+    cmp_config.cmp = PRESCALER_FACTOR*1000;  // 设置初始CMP值，这样直接设置为 1/2 则后续不需要更新即可生成 50% 占空比
+    // cmp_config.cmp = pwm_reload + 1; // CMP > RLD, 由于计数器值 CNT 始终达不到 CMPx，比较器输出 OCx 会保持逻辑 0
+    cmp_config.update_trigger = pwm_shadow_register_update_on_modify; // 设置CMP影子寄存器值生效时刻为 更新后的下一个周期
+    // pwm_shadow_register_update_on_modify 这种方式下一个指令周期就会重装CMP，可能会导致PWM波形不完整，手册不推荐这种方式
+
+    pwm_stop_counter(SBUS_PWM0); // 停止计数（没有也可以）
+    pwm_set_reload(SBUS_PWM0, 0, sbus_af_pwm_period*PRESCALER_FACTOR); // 设置RLD寄存器
+    pwm_set_start_count(SBUS_PWM0, 0, 0); // 设置STA寄存器
+
+    // 使用给定参数对PWM通道进行设置
+    if (status_success != pwm_setup_waveform(SBUS_PWM0, SBUS_PWM0_OUT, &pwm_config, SBUS_PWM0_CMP, &cmp_config, 1)) {
+        printf("failed to setup waveform\\n");
+        while(1);
+    }
+   
+    pwm_start_counter(SBUS_PWM0); // 开始计数（PWM输出开始）
+    pwm_issue_shadow_register_lock_event(SBUS_PWM0); // 锁定影子寄存器
+
+    // 和 cmp = pwm_reload + 1 一起使用，也可以得到 50% 占空比的 PWM波形
+    // 在这里更新CMP影子寄存器，下一个周期CMP寄存器会得到更新，这种方式便于动态更新PWM波形
+    // pwm_update_raw_cmp_edge_aligned(pwm_x, cmp_index, pwm_reload / 2); // 50 % HIGH
+}
+
+/* @brief sbus pwm 输出占空比 */
+void sbus_out_af_pwm_set_pulse_value(uint32_t pulse_value)
+{
+    /* 更新比较值以改变占空比 */
+    pwm_cmp_update_cmp_value(SBUS_PWM0, SBUS_PWM0_CMP, pulse_value, 0);
+}
+
+void sbusout_af_pwm_init(uint32_t pwm_period)
+{
+    sbusout_af_pwm_gpio_config();
+    sbusout_af_pwm_timer_config(pwm_period);
+}
+
+static void rc_uart2_pin_config(void) // 如果使用的是同一个串口的rx 和tx 就初始化1次 
+{
+    HPM_IOC->PAD[IOC_PAD_PB21].FUNC_CTL = IOC_PB21_FUNC_CTL_UART2_RXD;
+
+    HPM_IOC->PAD[IOC_PAD_PB22].FUNC_CTL = IOC_PB22_FUNC_CTL_UART2_TXD;
+}
+/*--------------------------------------------------------------------------*/
+/* SBUS 输出初始化（UART + DMA）                                           */
+/*--------------------------------------------------------------------------*/
+void sbus_out_init(uint32_t baud)
+{
+    Initial_UART2_TX(baud);
+    UART2_TX_DMA_Config();
+    UART2_TX_NVIC_Config();
+}
+
+static void Initial_UART2_TX(uint32_t baudrate)
+{
+   uart_config_t config = {0};
+    hpm_stat_t stat;
+    
+    /* 初始化UART */
+    rc_uart2_pin_config();
+
+    clock_set_source_divider(SBUS_UART2_CLK_NAME, clk_src_osc24m, 1);
+    clock_add_to_group(SBUS_UART2_CLK_NAME, 0);
+    uint32_t freq = clock_get_frequency(SBUS_UART2_CLK_NAME);
+    printf("uart2 clk fre %d\r\n", freq);
+    /* UART默认配置 */
+    uart_default_config(SBUS_UART2_TX, &config);
+    
+    /* SBUS参数配置 */
+    config.baudrate = baudrate;                    /* 100000bps */
+    config.word_length = word_length_8_bits;       /* 9位数据（8位+偶校验） */
+    config.num_of_stop_bits = stop_bits_2;                 /* 2停止位 */
+    config.parity = parity_even;                     /* 偶校验 */
+    config.fifo_enable = true;                       /* 使能FIFO */
+    config.src_freq_in_hz = clock_get_frequency(SBUS_UART2_CLK_NAME);
+    
+    /*  设置FIFO阈值 - 设为接近一帧的长度 要小于实际期望接收的字节数 25 */
+    config.rx_fifo_level = uart_rx_fifo_trg_gt_three_quarters; /* 约24字节 */
+
+    
+    stat = uart_init(SBUS_UART2_TX, &config);
+    if (stat != status_success) {
+        printf("SBUS UART2 RX init failed\n");
+        while (1);
+    } 
+}
+
+static void UART2_TX_DMA_Config(void)
+{
+    dma_handshake_config_t handshake_config;
+    dma_channel_config_t channel_config;
+    /* 使能 DMA 时钟 */
+    // HDMA 时钟来源于系统总线时钟（AHB）
+    
+    /* ========== 配置 TX DMA 通道 ========== */
+    /* 初始化 DMA 通道 */
+    dma_default_channel_config(SBUS_UART2_DMA_CONTROLLER, &channel_config);
+    
+    /* 配置 DMAMUX：将 UART3 TX 请求连接到 TX DMA 通道 */
+    dmamux_config(SBUS_UART2_DMAMUX_CONTROLLER, SBUS_UART2_TX_DMAMUX_CH, SBUS_UART2_TX_DMA_REQ, true);
+    
+    /* 配置 TX 握手参数 */
+    dma_default_handshake_config(SBUS_UART2_DMA_CONTROLLER, &handshake_config);
+}
+
+
+static void UART2_TX_NVIC_Config(void)
+{
+
+      intc_m_enable_irq_with_priority(SBUS_UART2_DMA_IRQ, SBUS_UART2_DMA_IRQ_RANK);
+}
+
+
+void UART2_Put_Char(uint8_t DataToSend)
+{
+     /* 等待发送寄存器空 */
+    while (!uart_check_status(SBUS_UART2_TX, uart_stat_transmitter_empty));
+    
+    /* 发送数据 */
+    uart_send_byte(SBUS_UART2_TX, DataToSend);
+    
+    /* 等待发送完成 */
+    while (!uart_check_status(SBUS_UART2_TX, uart_stat_transmitter_empty));
+}
+
+
+static bool usart2_tx_isbusy(void)
+{
+    return !uart2_tx_dma_done;
+}
+
+void open_dma1_stream6_tx(unsigned short count)
+{
+     dma_handshake_config_t handshake_config;
+    
+    /* 等待上次发送完成 */
+    uint32_t timeout = 1000000;
+    while (usart2_tx_isbusy() && timeout--) {
+        __asm("nop");
+    }
+    if (timeout == 0) {
+        // 超时处理，复位DMA
+        dma_abort_channel(SBUS_UART2_DMA_CONTROLLER, SBUS_UART2_TX_DMA_CH);
+        uart2_tx_dma_done = true;  // 强制完成
+    }  
+      
+    /* 重新配置 TX 传输大小 */
+    dma_default_handshake_config(SBUS_UART2_DMA_CONTROLLER, &handshake_config);
+    handshake_config.ch_index = SBUS_UART2_TX_DMA_CH;
+    handshake_config.dst = (uint32_t)&SBUS_UART2_TX->THR;
+    handshake_config.dst_fixed = true;
+    handshake_config.src = core_local_mem_to_sys_address(0, (uint32_t)uart2_dma_tx_buf);
+    handshake_config.src_fixed = false;
+    handshake_config.data_width = DMA_TRANSFER_WIDTH_BYTE;
+    handshake_config.size_in_byte = count;
+    
+    dma_setup_handshake(SBUS_UART2_DMA_CONTROLLER, &handshake_config, true); // true表示触发一次
+}
+
+/*--------------------------------------------------------------------------*/
+/* PWM 值转 SBUS 数值（原样保留）                                          */
+/*--------------------------------------------------------------------------*/
+void pwm_to_sbus(short *out_pwm, short *out_subs)
+{
+    int i = 0;
+
+    for (i = 0; i < 8; i++)
+    {
+        out_subs[i] = ((out_pwm[i] - 860) / 0.625f);
+    }
+
+#ifdef ALL_CHANNELS
+    for (i = 8; i < 14; i++)
+    {
+        out_subs[i] = ((out_pwm[i] - 860) / 0.625f);
+    }
+#endif
+}
+
+/*--------------------------------------------------------------------------*/
+/* SBUS 数据打包（原样保留）                                               */
+/*--------------------------------------------------------------------------*/
+void sbus_channel_packet(short *rc_ch)
+{
+    /* assemble the SBUS packet */
+
+    // SBUS header
+    uart2_dma_tx_buf[0] = 0x0f;
+
+    // 16 channels of 11 bit data
+    uart2_dma_tx_buf[1] = (uint8_t)((rc_ch[0] & 0x07FF));
+    uart2_dma_tx_buf[2] =
+        (uint8_t)((rc_ch[0] & 0x07FF) >> 8 | (rc_ch[1] & 0x07FF) << 3);
+    uart2_dma_tx_buf[3] =
+        (uint8_t)((rc_ch[1] & 0x07FF) >> 5 | (rc_ch[2] & 0x07FF) << 6);
+    uart2_dma_tx_buf[4] = (uint8_t)((rc_ch[2] & 0x07FF) >> 2);
+    uart2_dma_tx_buf[5] =
+        (uint8_t)((rc_ch[2] & 0x07FF) >> 10 | (rc_ch[3] & 0x07FF) << 1);
+
+    uart2_dma_tx_buf[6] =
+        (uint8_t)((rc_ch[3] & 0x07FF) >> 7 | (rc_ch[4] & 0x07FF) << 4);
+    uart2_dma_tx_buf[7] =
+        (uint8_t)((rc_ch[4] & 0x07FF) >> 4 | (rc_ch[5] & 0x07FF) << 7);
+    uart2_dma_tx_buf[8] = (uint8_t)((rc_ch[5] & 0x07FF) >> 1);
+    uart2_dma_tx_buf[9] =
+        (uint8_t)((rc_ch[5] & 0x07FF) >> 9 | (rc_ch[6] & 0x07FF) << 2);
+    uart2_dma_tx_buf[10] =
+        (uint8_t)((rc_ch[6] & 0x07FF) >> 6 | (rc_ch[7] & 0x07FF) << 5);
+    uart2_dma_tx_buf[11] = (uint8_t)((rc_ch[7] & 0x07FF) >> 3);
+
+#ifdef ALL_CHANNELS
+    uart2_dma_tx_buf[12] = (uint8_t)((rc_ch[8] & 0x07FF));
+    uart2_dma_tx_buf[13] =
+        (uint8_t)((rc_ch[8] & 0x07FF) >> 8 | (rc_ch[9] & 0x07FF) << 3);
+    uart2_dma_tx_buf[14] =
+        (uint8_t)((rc_ch[9] & 0x07FF) >> 5 | (rc_ch[10] & 0x07FF) << 6);
+    uart2_dma_tx_buf[15] = (uint8_t)((rc_ch[10] & 0x07FF) >> 2);
+    uart2_dma_tx_buf[16] =
+        (uint8_t)((rc_ch[10] & 0x07FF) >> 10 | (rc_ch[11] & 0x07FF) << 1);
+    uart2_dma_tx_buf[17] =
+        (uint8_t)((rc_ch[11] & 0x07FF) >> 7 | (rc_ch[12] & 0x07FF) << 4);
+    uart2_dma_tx_buf[18] =
+        (uint8_t)((rc_ch[12] & 0x07FF) >> 4 | (rc_ch[13] & 0x07FF) << 7);
+    uart2_dma_tx_buf[19] = (uint8_t)((rc_ch[13] & 0x07FF) >> 1);
+    uart2_dma_tx_buf[20] =
+        (uint8_t)((rc_ch[13] & 0x07FF) >> 9 | (rc_ch[14] & 0x07FF) << 2);
+    uart2_dma_tx_buf[21] =
+        (uint8_t)((rc_ch[14] & 0x07FF) >> 6 | (rc_ch[15] & 0x07FF) << 5);
+    uart2_dma_tx_buf[22] = (uint8_t)((rc_ch[15] & 0x07FF) >> 3);
+
+    // flags
+    uart2_dma_tx_buf[23] = 0x00;
+#endif
+
+    switch (uart2_dma_tx_buf[24])
+    {
+    case 0x04:
+        uart2_dma_tx_buf[24] = 0x14;
+        break;
+
+    case 0x14:
+        uart2_dma_tx_buf[24] = 0x24;
+        break;
+
+    case 0x24:
+        uart2_dma_tx_buf[24] = 0x34;
+        break;
+
+    case 0x34:
+    default:
+        uart2_dma_tx_buf[24] = 0x04;
+        break;
+    }
+
+    // DMA方式发送
+    open_dma1_stream6_tx(UART2_TX_LENDTH_MAX);
+}
+
+#ifdef UART2_TX_TEST
+/* uart2 tx_dma  demo 2026/03/16 pass*/
+void sbus_uart2_out_test(void)
+{
+    short test_channels[16];
+    
+    /* 1. 初始化SBUS硬件 */
+    sbus_out_init(SBUS_UART2_BAUD);
+    printf("SBUS初始化完成\r\n");
+    
+    /* 2. 构造测试数据：通道0=最大值，通道1=最小值，其他=中位值 */
+    for (int i = 0; i < 16; i++) {
+        test_channels[i] = 1024;  // 中位值
+    }
+    test_channels[0] = 1876;  // 最大值
+    test_channels[1] = 172;   // 最小值
+    
+    printf("发送测试数据: Ch0=%d, Ch1=%d, Ch2=%d\r\n", 
+           test_channels[0], test_channels[1], test_channels[2]);
+    
+    /* 3. 打包并发送 */
+    sbus_channel_packet(test_channels);
+    
+    /* 4. 等待发送完成 */
+    printf("等待发送完成...\r\n");
+    while(usart2_tx_isbusy()) {
+        board_delay_ms(1);
+    }
+    printf("发送完成!\r\n");
+}
+#endif
+/*
+
+// sbus最大通道数18
+#define SBUS_MAX_CHANNEL 18
+// sbus一包数据25个字节
+#define SBUS_FRAME_SIZE 25
+// sbus起始位
+#define SBUS_FRAME_SYNC_BYTE 0x0F
+//第24个字节的解析
+#define SBUS_FLAG_CHANNEL_17 (1 << 0)
+#define SBUS_FLAG_CHANNEL_18 (1 << 1)
+#define SBUS_FLAG_SIGNAL_LOSS (1 << 2)
+#define SBUS_FLAG_FAILSAFE_ACTIVE (1 << 3)
+
+struct sbusframe_s {
+  unsigned char sync_byte;
+  // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
+  unsigned int chan0 : 11;
+  unsigned int chan1 : 11;
+  unsigned int chan2 : 11;
+  unsigned int chan3 : 11;
+  unsigned int chan4 : 11;
+  unsigned int chan5 : 11;
+  unsigned int chan6 : 11;
+  unsigned int chan7 : 11;
+  unsigned int chan8 : 11;
+  unsigned int chan9 : 11;
+  unsigned int chan10 : 11;
+  unsigned int chan11 : 11;
+  unsigned int chan12 : 11;
+  unsigned int chan13 : 11;
+  unsigned int chan14 : 11;
+  unsigned int chan15 : 11;
+  unsigned char flags;
+
+   // The endByte is 0x00 on FrSky and some futaba RX's, on Some SBUS2 RX's the
+   // value indicates the telemetry byte that is sent after every 4th sbus
+frame.
+   //
+   // See
+   //
+https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101027349
+   // and
+   //
+https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101706023
+
+  unsigned char end_byte;
+} __attribute__((__packed__));
+
+typedef union {
+  uint8_t sbus_data[SBUS_FRAME_SIZE];
+  struct sbusframe_s frame;
+} sbusframe_t;
+static sbusframe_t sbusframe;
+
+*/

controller_yy_app/hardware/hard_sbusout_af_pump.c

@@ -0,0 +1,109 @@
+#if 0
+#include "hard_sbusout_af_pump.h"
+
+#include "stm32f4xx.h"
+
+#define SBUSOUT_AF_PUMP_PERIOD 20000 // subsout复用为水泵输出
+#define PUMP_DEF_VALUE 1000
+
+static void sbusout_af_pump_gpio_config(void);
+static void sbusout_af_pump_timer_config(void);
+
+/**************************实现函数********************************************
+*函数原型:		void sbusout_af_pump_init(void)
+*功　　能:	    PWM输出 初始化。
+*******************************************************************************/
+void sbusout_af_pump_init(void)
+{
+    sbusout_af_pump_gpio_config();
+    sbusout_af_pump_timer_config();
+}
+
+/**************************实现函数********************************************
+*函数原型:		static void sbusout_af_pump_gpio_config(void)
+*功　　能:	    配置PWM引脚为输出，并开启相应的时钟信号 和引脚复用时钟信号
+*******************************************************************************/
+static void sbusout_af_pump_gpio_config(void)
+{
+    GPIO_InitTypeDef GPIO_InitStructure;
+
+    /* GPIOA and GPIOB and GPIOC clock enable */
+    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
+
+    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
+    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; // 复用推挽输出
+    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
+    GPIO_Init(GPIOA, &GPIO_InitStructure);
+
+    GPIO_PinAFConfig(GPIOA, GPIO_PinSource2, GPIO_AF_TIM9);
+}
+/**************************实现函数********************************************
+*函数原型:		static void sbusout_af_pump_timer_config(void)
+*功　　能:	    配置PWM的对应定时器
+
+//通用定时器3中断初始化,使用定时器3的溢出中断来确定HZ数
+//TIM_Period：自动重装值。
+//TIM_Prescaler：时钟预分频数
+//定时器溢出时间计算方法:Tout=((arr+1)*(psc+1))/Ft us.
+//Ft=定时器工作频率,单位:Mhz
+//这里使用的是定时器3!
+
+//来自STM32F4手册：高速APB2域的最大允许频率是84MHZ，低速APB1域的最大允许频率是42MHZ
+*******************************************************************************/
+static void sbusout_af_pump_timer_config(void)
+{
+    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+    TIM_OCInitTypeDef TIM_OCInitStructure;
+    //开定时器时钟
+    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM9, ENABLE);
+
+    //=============Time base configuration========================
+    // PWM周期,2.5ms,400hz
+    TIM_TimeBaseStructure.TIM_Period = SBUSOUT_AF_PUMP_PERIOD;
+    //设置预分频：即为1MHz。1ms = 1000次计数
+    TIM_TimeBaseStructure.TIM_Prescaler = 168 - 1;
+    //设置时钟分频系数：不分频
+    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
+    //向上计数模式
+    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+
+    //初始化定时器
+    TIM_TimeBaseStructure.TIM_Period = SBUSOUT_AF_PUMP_PERIOD; // PWM周期
+    TIM_TimeBaseInit(TIM9, &TIM_TimeBaseStructure);
+
+    //===================TIM_OCInitStructure base
+    //configuration==================
+    //配置为PWM模式1
+    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
+    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+    //设置跳变值，当计数器计数到大于这个值时，电平发生跳变
+    //,,TIMx->CCRx的值，设置占空比，在更新电机的地方实时更新这个值
+    TIM_OCInitStructure.TIM_Pulse = PUMP_DEF_VALUE;
+    //当定时器计数值小于CCR1_Val时为高电平
+    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
+
+    //===================================================================================
+    //*	PA2  TIM9_CHA
+    //*
+    //* 每一个通用定时器可以产生四路频率一致占空比可调节的独立的PWM波形，
+    //* TIM_OC1Init  1通道	TIM_OC2Init  2通道	TIM_OC3Init  3通道	TIM_OC4Init
+    //4通道
+    //*
+    //* 哪个管脚对应哪个TIM的哪个CH由硬件设计已经决定
+    //========================================================================================
+    //=================使能通道=================================
+    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+
+    // TIME9的通道1 SUBSOUT_AF_PWM
+    TIM_OC1Init(TIM9, &TIM_OCInitStructure);
+    TIM_OC1PreloadConfig(TIM9, TIM_OCPreload_Enable);
+
+    //使能TIMx重载寄存器ARR
+    TIM_ARRPreloadConfig(TIM9, ENABLE);
+
+    //使能定时器x
+    TIM_Cmd(TIM9, ENABLE);
+}
+#endif

controller_yy_app/hardware/hard_sdio_sd.c

@@ -0,0 +1,105 @@
+#include "board.h"
+#include "hpm_sdxc_drv.h"
+#include "hard_sdio_sd.h"
+// 使用官方的SDMMC例程包
+// 修改时要修改初始化io 和时钟 在port文件内
+// 这里负责操作初始化IO 和 时钟的接口 注意在port文件填写对应的回调函数
+void sdxc_cd_pin(SDXC_Type *ptr, bool as_gpio)
+{
+    uint32_t cd_pad_ctl = IOC_PAD_PAD_CTL_DS_SET(6) | IOC_PAD_PAD_CTL_PE_SET(1) | IOC_PAD_PAD_CTL_PS_SET(1);
+    if (ptr == HPM_SDXC1) {
+        /* SDXC1.CDN */
+        uint32_t cd_func_alt = as_gpio ? IOC_PD28_FUNC_CTL_GPIO_D_28 : IOC_PD28_FUNC_CTL_SDC1_CDN;
+        HPM_IOC->PAD[IOC_PAD_PD28].FUNC_CTL = cd_func_alt;
+        HPM_IOC->PAD[IOC_PAD_PD28].PAD_CTL = cd_pad_ctl;
+    }
+}
+
+uint32_t sd_configure_clock(SDXC_Type *ptr, uint32_t freq, bool need_inverse)
+{
+    uint32_t actual_freq = 0;
+    do {
+        if (ptr != HPM_SDXC1) {
+            break;
+        }
+        clock_name_t sdxc_clk = (ptr == HPM_SDXC0) ? clock_sdxc0 : clock_sdxc1;
+        sdxc_enable_inverse_clock(ptr, false);
+        sdxc_enable_sd_clock(ptr, false);
+        /* Configure the clock below 400KHz for the identification state */
+        if (freq <= 400000UL) {
+            clock_set_source_divider(sdxc_clk, clk_src_osc24m, 63);
+        }
+            /* configure the clock to 24MHz for the SDR12/Default speed */
+        else if (freq <= 26000000UL) {
+            clock_set_source_divider(sdxc_clk, clk_src_osc24m, 1);
+        }
+            /* Configure the clock to 50MHz for the SDR25/High speed/50MHz DDR/50MHz SDR */
+        else if (freq <= 52000000UL) {
+            clock_set_source_divider(sdxc_clk, clk_src_pll1_clk1, 8);
+        }
+            /* Configure the clock to 100MHz for the SDR50 */
+        else if (freq <= 100000000UL) {
+            clock_set_source_divider(sdxc_clk, clk_src_pll1_clk1, 4);
+        }
+            /* Configure the clock to 166MHz for SDR104/HS200/HS400  */
+        else if (freq <= 208000000UL) {
+            clock_set_source_divider(sdxc_clk, clk_src_pll2_clk0, 2);
+        }
+            /* For other unsupported clock ranges, configure the clock to 24MHz */
+        else {
+            clock_set_source_divider(sdxc_clk, clk_src_osc24m, 1);
+        }
+        if (need_inverse) {
+            sdxc_enable_inverse_clock(ptr, true);
+        }
+        sdxc_enable_sd_clock(ptr, true);
+        actual_freq = clock_get_frequency(sdxc_clk);
+    } while (false);
+
+    return actual_freq;
+}
+
+void sdxc_cmd_pin(SDXC_Type *ptr, bool open_drain, bool is_1v8)
+{
+    uint32_t cmd_func_ctl = IOC_PAD_FUNC_CTL_ALT_SELECT_SET(17) | IOC_PAD_FUNC_CTL_LOOP_BACK_SET(1);
+    uint32_t cmd_pad_ctl = IOC_PAD_PAD_CTL_MS_SET(is_1v8) | IOC_PAD_PAD_CTL_DS_SET(6) | IOC_PAD_PAD_CTL_PE_SET(1) |
+                           IOC_PAD_PAD_CTL_PS_SET(1);
+    if (open_drain) {
+        cmd_pad_ctl |= IOC_PAD_PAD_CTL_OD_MASK;
+    }
+
+    if (ptr == HPM_SDXC1) {
+        /* SDXC1.CMD */
+        HPM_IOC->PAD[IOC_PAD_PD21].FUNC_CTL = cmd_func_ctl;
+        HPM_IOC->PAD[IOC_PAD_PD21].PAD_CTL = cmd_pad_ctl;
+    }
+}
+
+void sdxc_clk_data_pins(SDXC_Type *ptr, uint32_t width, bool is_1v8)
+{
+    uint32_t func_ctl = IOC_PAD_FUNC_CTL_ALT_SELECT_SET(17);
+    uint32_t pad_ctl = IOC_PAD_PAD_CTL_MS_SET(is_1v8) | IOC_PAD_PAD_CTL_DS_SET(6) | IOC_PAD_PAD_CTL_PE_SET(1) |
+                       IOC_PAD_PAD_CTL_PS_SET(1);
+
+    if (ptr == HPM_SDXC1) {
+        /* SDXC1.CLK */
+        HPM_IOC->PAD[IOC_PAD_PD22].FUNC_CTL = func_ctl;
+        HPM_IOC->PAD[IOC_PAD_PD22].PAD_CTL = pad_ctl;
+
+        /* SDXC1.DATA0 */
+        HPM_IOC->PAD[IOC_PAD_PD18].FUNC_CTL = func_ctl;
+        HPM_IOC->PAD[IOC_PAD_PD18].PAD_CTL = pad_ctl;
+        if ((width == 4)) {
+            /* SDXC1.DATA1 */
+            HPM_IOC->PAD[IOC_PAD_PD17].FUNC_CTL = func_ctl;
+            HPM_IOC->PAD[IOC_PAD_PD17].PAD_CTL = pad_ctl;
+            /* SDXC1.DATA2 */
+            HPM_IOC->PAD[IOC_PAD_PD27].FUNC_CTL = func_ctl;
+            HPM_IOC->PAD[IOC_PAD_PD27].PAD_CTL = pad_ctl;
+            /* SDXC1.DATA3 */
+            HPM_IOC->PAD[IOC_PAD_PD26].FUNC_CTL = func_ctl;
+            HPM_IOC->PAD[IOC_PAD_PD26].PAD_CTL = pad_ctl;
+        }
+    }
+
+}

controller_yy_app/hardware/hard_system.c

@@ -0,0 +1,49 @@
+#include "hpm_dma_drv.h"
+#include "board.h"
+#include "hard_system.h"
+#include "hpm_ppor_drv.h"
+#include "test.h"
+#define U3_DMA HPM_HDMA  // 关的是串口三的接收dma 这里没有空闲中断 不使用dma模式接收
+#define U3_DMA_CH 1
+
+void sys_reset(void)
+{
+    /*Enable software reset*/
+    ppor_reset_mask_set_source_enable(HPM_PPOR, ppor_reset_software);
+    /* software reset*/
+    printf("%s\n", __func__);
+    ppor_sw_reset(HPM_PPOR, 10);
+}
+
+/**
+ * @brief 禁用指定DMA通道
+ * @note HPM6750的DMA1 Stream1对应：
+ *       - DMA控制器：DMA0（HPM6750主DMA控制器）
+ *   
+ */
+void u3_dma_disable(void)
+{
+    // 禁用HDMA通道 1
+    dma_disable_channel(U3_DMA, U3_DMA_CH);
+    
+}
+
+
+
+#ifdef SYSTEM_TEST
+/*
+ reset demo test 20260115 pass
+*/
+void system_test(void)
+{
+    sys_reset();
+     /*If a software reset occurs, clear the reset flag and print the message*/
+    if (ppor_reset_get_flags(HPM_PPOR) & ppor_reset_software) {
+        ppor_reset_clear_flags(HPM_PPOR, ppor_reset_software);
+        ppor_reset_mask_clear_source_enable(HPM_PPOR, ppor_reset_software);
+        printf("Software reset has occurred\n");
+        while (1) {
+        };
+    }
+
+#endif

controller_yy_app/hardware/hard_system_delay.c

@@ -0,0 +1,37 @@
+#include "board.h"
+#include "hard_system_delay.h"
+#include "hpm_clock_drv.h"
+#include "test.h"
+void system_delay_init(unsigned char SYSCLK)
+{
+   ;// 使用的是cpu库延时 最大648M 64bit计数  可等us 将近1h
+}
+
+void system_delay_ms(unsigned short nms)
+{
+   clock_cpu_delay_ms(nms);
+}
+
+
+void system_delay_us(unsigned int nus)
+{
+   clock_cpu_delay_us(nus);
+}
+
+/*cpu delay test 2026/3/14 测试通过*/
+#ifdef CPU_DELAY_TEST
+#include "bsp_V8M_YY_led.h"
+
+
+void cpu_delay_test(void)
+{
+  system_delay_init(84);
+  v8m_yy_led_init();
+  while(1)
+  {
+    system_delay_ms(1000);
+    v8m_yy_led_toggle(V8M_YY_LED_R);
+    printf("1000ms \r\n");
+  }
+}
+#endif

controller_yy_app/hardware/hard_system_time.c

@@ -0,0 +1,86 @@
+#include "board.h"
+#include "hpm_gptmr_drv.h"
+#include "hard_system_time.h"
+#include "test.h"
+// 需要修改分频配置
+#define SYSTEM_TIMER_0               HPM_GPTMR0
+#define SYSTEM_TIMER_0_CLK_NAME      clock_gptmr0
+#define SYSTEM_TIMER_0_CH            0
+#define SYSTEM_TIMER_0_IRQ           IRQn_GPTMR0
+#define SYSTEM_TIMER_0_TICK_US       (1)
+#define SYSTEM_TIMER_0_RELOAD        0xFFFFFFFF
+
+void system_time_init(void)
+{
+    uint32_t gptmr_freq;
+    gptmr_channel_config_t config;
+
+
+   
+    clock_set_source_divider(SYSTEM_TIMER_0_CLK_NAME, clk_src_osc24m, 24); // 1Mhz
+    clock_add_to_group(SYSTEM_TIMER_0_CLK_NAME, 0); // 添加到组
+    gptmr_freq = clock_get_frequency(SYSTEM_TIMER_0_CLK_NAME);
+    printf("timer0 fre: %d\r\n", gptmr_freq);
+    gptmr_channel_get_default_config(SYSTEM_TIMER_0, &config);
+    config.reload = SYSTEM_TIMER_0_RELOAD; // 32bit timer0 71.58min
+    gptmr_channel_config(SYSTEM_TIMER_0, SYSTEM_TIMER_0_CH, &config, false);
+    gptmr_start_counter(SYSTEM_TIMER_0, SYSTEM_TIMER_0_CH);
+
+    gptmr_enable_irq(SYSTEM_TIMER_0, GPTMR_CH_RLD_IRQ_MASK(SYSTEM_TIMER_0_CH));
+    intc_m_enable_irq_with_priority(SYSTEM_TIMER_0_IRQ, 1);
+}
+
+
+SDK_DECLARE_EXT_ISR_M(SYSTEM_TIMER_0_IRQ, timer0_isr)
+void timer0_isr(void)
+{
+    if (gptmr_check_status(SYSTEM_TIMER_0, GPTMR_CH_RLD_STAT_MASK(SYSTEM_TIMER_0_CH))) {
+        gptmr_clear_status(SYSTEM_TIMER_0, GPTMR_CH_RLD_STAT_MASK(SYSTEM_TIMER_0_CH));
+        //time_hookfunction();
+        // printf("timer test\r\n");
+    }
+}
+
+
+unsigned int hard_micros(void)
+{ 
+  unsigned int ret;
+  // 返回当前定时器通道计数值
+  ret = gptmr_channel_get_counter(SYSTEM_TIMER_0,SYSTEM_TIMER_0_CH, gptmr_counter_type_normal);
+  return ret;
+}
+/**
+ * @brief 微秒延时
+ */
+static void hard_delay_us(unsigned int us)
+{
+    unsigned int start = hard_micros();
+    while ((hard_micros() - start) < us) {
+        // 忙等待
+    }
+}
+
+/**
+ * @brief 毫秒延时
+ */
+static void hard_delay_ms(unsigned int ms)
+{
+    hard_delay_us(ms * 1000);
+}
+
+
+/*timer0 test 2026/3/14 测试通过*/
+#ifdef TIMER0_TEST
+#include "bsp_V8M_YY_led.h"
+void timer0_test(void)
+{
+  system_time_init();
+  v8m_yy_led_init();
+  while(1)
+  {
+    hard_delay_ms(1000);
+    v8m_yy_led_toggle(V8M_YY_LED_G);
+    printf("1000ms \r\n");
+  }
+}
+#endif

controller_yy_app/hardware/hard_system_timer.c

@@ -0,0 +1,82 @@
+#include "board.h"
+#include "hpm_gptmr_drv.h"
+// #include "soft_time.h"
+#include "hard_system_timer.h"
+#include "test.h"
+// 后续要设置定时器分频 一个定时器可以配置4个通道,每个通道的计数重装载值可以不同 由于定时器太多了 就不用同一个了
+// 共享同一个硬件模块、时钟源、中断向量 每个通道的配置、计数器、比较值、工作模式都是独立的
+#define SYSTEM_TIMER_1               HPM_GPTMR1
+#define SYSTEM_TIMER_1_CLK_NAME      clock_gptmr1
+#define SYSTEM_TIMER_1_CH            0
+#define SYSTEM_TIMER_1_IRQ           IRQn_GPTMR1
+#define SYSTEM_TIMER_1_TICK_US       (1)
+#define SYSTEM_TIMER_1_RELOAD        2500
+void system_timer_init(void)
+{
+    uint32_t gptmr_freq;
+    gptmr_channel_config_t config;
+
+    clock_set_source_divider(SYSTEM_TIMER_1_CLK_NAME, clk_src_osc24m, 24); // 1Mhz
+    clock_add_to_group(SYSTEM_TIMER_1_CLK_NAME, 0); // 添加到组
+    gptmr_freq = clock_get_frequency(SYSTEM_TIMER_1_CLK_NAME);
+    printf("timer1 fre: %d\r\n", gptmr_freq);
+    gptmr_channel_get_default_config(SYSTEM_TIMER_1, &config);
+    config.reload = SYSTEM_TIMER_1_RELOAD; // 32bit timer1 2.5ms 
+    gptmr_channel_config(SYSTEM_TIMER_1, SYSTEM_TIMER_1_CH, &config, false);
+    gptmr_start_counter(SYSTEM_TIMER_1, SYSTEM_TIMER_1_CH);
+
+    gptmr_enable_irq(SYSTEM_TIMER_1, GPTMR_CH_RLD_IRQ_MASK(SYSTEM_TIMER_1_CH));
+    intc_m_enable_irq_with_priority(SYSTEM_TIMER_1_IRQ, 1);
+  
+}
+
+extern void timer_hookfunction(void);
+
+static uint32_t s_count = 0;
+
+SDK_DECLARE_EXT_ISR_M(SYSTEM_TIMER_1_IRQ, timer1_250us_isr)
+void timer1_250us_isr(void)
+{
+    if (gptmr_check_status(SYSTEM_TIMER_1, GPTMR_CH_RLD_STAT_MASK(SYSTEM_TIMER_1_CH))) {
+        gptmr_clear_status(SYSTEM_TIMER_1, GPTMR_CH_RLD_STAT_MASK(SYSTEM_TIMER_1_CH));
+        // timer_hookfunction();
+        s_count++;
+    }
+}
+
+
+/*timer1 test 2026/3/14 测试通过*/
+#ifdef TIMER1_TEST
+
+#include "bsp_V8M_YY_led.h"
+void timer1_test(void)
+{
+  system_timer_init();
+  v8m_yy_led_init();
+  // 注：如果while(1)轮询太快可能导致401中断触发变灯 然后下一次中断没触发呢 又扫过来了再次进入应用 也就是1个周期触发两次
+  // 实际while(1)的运行一次的时间要大于中断1次的时间最好 也就是2.5ms
+  while(1) 
+  {
+    if(s_count % 400 == 1)
+    {
+      board_delay_ms(3);
+      v8m_yy_led_toggle(V8M_YY_LED_R);
+      printf("s_count %d\r\n", s_count);
+      printf("1000ms \r\n");
+    }
+    
+  }
+}
+/*
+异常
+s_count 7201
+1000ms 
+s_count 7201
+1000ms 
+
+正常
+s_count 17602
+1000ms 
+
+*/
+#endif

controller_yy_app/linkers/gcc/user_linker.ld

@@ -0,0 +1,330 @@
+/*
+ * Copyright (c) 2021-2023 HPMicro
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+ENTRY(_start)
+
+STACK_SIZE = _stack_size;
+HEAP_SIZE = _heap_size;
+
+MEMORY
+{
+    XPI0 (rx) : ORIGIN = 0x80000000, LENGTH = _flash_size
+    ILM (wx) : ORIGIN = 0x00000000, LENGTH = 256K
+    DLM (w) : ORIGIN = 0x00080000, LENGTH = 256K
+    AXI_SRAM (wx) : ORIGIN = 0x01080000, LENGTH = 768K    /* AXI SRAM */
+    SHARE_RAM (w) : ORIGIN = 0x0117C000, LENGTH = 16K
+    AHB_SRAM (w) : ORIGIN = 0xF0300000, LENGTH = 32k
+    APB_SRAM (w): ORIGIN = 0xF40F0000, LENGTH = 8k
+    SDRAM (wx) : ORIGIN = 0x40000000, LENGTH = (_extram_size - 4M)
+    NONCACHEABLE_RAM (wx) : ORIGIN = 0x40000000 + (_extram_size - 4M), LENGTH = 4M
+}
+
+__nor_cfg_option_load_addr__ = ORIGIN(XPI0) + 0x400;
+__boot_header_load_addr__ = ORIGIN(XPI0) + 0x1000;
+__app_load_addr__ = ORIGIN(XPI0) + 0x3000;
+__boot_header_length__ = __boot_header_end__ - __boot_header_start__;
+__app_offset__ = __app_load_addr__ - __boot_header_load_addr__;
+
+SECTIONS
+{
+    .nor_cfg_option __nor_cfg_option_load_addr__ : {
+        KEEP(*(.nor_cfg_option))
+    } > XPI0
+
+    .boot_header __boot_header_load_addr__ : {
+        __boot_header_start__ = .;
+        KEEP(*(.boot_header))
+        KEEP(*(.fw_info_table))
+        KEEP(*(.dc_info))
+        __boot_header_end__ = .;
+    } > XPI0
+
+    .start __app_load_addr__ : {
+        . = ALIGN(8);
+        KEEP(*(.start))
+    } > XPI0
+
+    __vector_load_addr__ = ADDR(.start) + SIZEOF(.start);
+    .vectors ORIGIN(ILM) : AT(__vector_load_addr__) {
+        . = ALIGN(8);
+        __vector_ram_start__ = .;
+        KEEP(*(.vector_table))
+        KEEP(*(.isr_vector))
+        KEEP(*(.isr_vector.*))
+        . = ALIGN(8);
+        __vector_ram_end__ = .;
+    } > ILM
+
+    .text (__vector_load_addr__ + SIZEOF(.vectors)) : {
+        . = ALIGN(8);
+        *(EXCLUDE_FILE (nx*.o*) .text)
+        *(EXCLUDE_FILE (nx*.o*) .text*)
+        *(EXCLUDE_FILE (nx*.o*) .rodata)
+        *(EXCLUDE_FILE (nx*.o*) .rodata*)
+        *(EXCLUDE_FILE (nx*.o*) .srodata)
+        *(EXCLUDE_FILE (nx*.o*) .srodata*)
+
+        *(.hash)
+        *(.dyn*)
+        *(.gnu*)
+        *(.pl*)
+
+        KEEP (*(.init))
+        KEEP (*(.fini))
+
+        /* section information for usbh class */
+        . = ALIGN(8);
+        __usbh_class_info_start__ = .;
+        KEEP(*(.usbh_class_info))
+        __usbh_class_info_end__ = .;
+
+        /* RT-Thread related sections - Start */
+        /* section information for finsh shell */
+        . = ALIGN(4);
+        __fsymtab_start = .;
+        KEEP(*(FSymTab))
+        __fsymtab_end = .;
+        . = ALIGN(4);
+        __vsymtab_start = .;
+        KEEP(*(VSymTab))
+        __vsymtab_end = .;
+        . = ALIGN(4);
+
+        . = ALIGN(4);
+        __rt_init_start = .;
+        KEEP(*(SORT(.rti_fn*)))
+        __rt_init_end = .;
+        . = ALIGN(4);
+
+        /* section information for modules */
+        . = ALIGN(4);
+        __rtmsymtab_start = .;
+        KEEP(*(RTMSymTab))
+        __rtmsymtab_end = .;
+
+        /* RT-Thread related sections - end */
+        . = ALIGN(8);
+    } > XPI0
+
+    .eh_frame :
+    {
+        __eh_frame_start = .;
+        KEEP(*(.eh_frame))
+        __eh_frame_end = .;
+    }  > XPI0
+
+    .eh_frame_hdr :
+    {
+        KEEP(*(.eh_frame_hdr))
+    }  > XPI0
+    __eh_frame_hdr_start = SIZEOF(.eh_frame_hdr) > 0 ? ADDR(.eh_frame_hdr) : 0;
+    __eh_frame_hdr_end = SIZEOF(.eh_frame_hdr) > 0 ? . : 0;
+
+    .rel : {
+        KEEP(*(.rel*))
+    } > XPI0
+
+    PROVIDE (__etext = .);
+    PROVIDE (_etext = .);
+    PROVIDE (etext = .);
+
+    __data_load_addr__ = etext;
+    .data : AT(__data_load_addr__) {
+        . = ALIGN(8);
+        __data_start__ = .;
+        __global_pointer$ = . + 0x800;
+        *(.data)
+        *(.data*)
+        *(.sdata)
+        *(.sdata*)
+
+        KEEP(*(.jcr))
+        KEEP(*(.dynamic))
+        KEEP(*(.got*))
+        KEEP(*(.got))
+        KEEP(*(.gcc_except_table))
+        KEEP(*(.gcc_except_table.*))
+
+        . = ALIGN(8);
+        PROVIDE(__preinit_array_start = .);
+        KEEP(*(.preinit_array))
+        PROVIDE(__preinit_array_end = .);
+
+        . = ALIGN(8);
+        PROVIDE(__init_array_start = .);
+        KEEP(*(SORT_BY_INIT_PRIORITY(.init_array.*)))
+        KEEP(*(.init_array))
+        PROVIDE(__init_array_end = .);
+
+        . = ALIGN(8);
+        PROVIDE(__finit_array_start = .);
+        KEEP(*(SORT_BY_INIT_PRIORITY(.finit_array.*)))
+        KEEP(*(.finit_array))
+        PROVIDE(__finit_array_end = .);
+
+        . = ALIGN(8);
+        KEEP(*crtbegin*.o(.ctors))
+        KEEP(*(EXCLUDE_FILE (*crtend*.o) .ctors))
+        KEEP(*(SORT(.ctors.*)))
+        KEEP(*(.ctors))
+
+        . = ALIGN(8);
+        KEEP(*crtbegin*.o(.dtors))
+        KEEP(*(EXCLUDE_FILE (*crtend*.o) .dtors))
+        KEEP(*(SORT(.dtors.*)))
+        KEEP(*(.dtors))
+        . = ALIGN(8);
+
+        __data_end__ = .;
+        PROVIDE (__edata = .);
+        PROVIDE (_edata = .);
+        PROVIDE (edata = .);
+    } > SDRAM
+
+    __fast_load_addr__ = etext + SIZEOF(.data);
+    .fast : AT(__fast_load_addr__) {
+        . = ALIGN(8);
+        PROVIDE(__ramfunc_start__ = .);
+        *(.fast)
+        *(.fast.*)
+        nx*.o*(.text)
+        nx*.o*(.text*)
+        nx*.o*(.rodata)
+        nx*.o*(.rodata*)
+        nx*.o*(.srodata)
+        nx*.o*(.srodata*)
+        . = ALIGN(8);
+        PROVIDE(__ramfunc_end__ = .);
+    } > ILM
+
+    __tdata_load_addr__ = etext + SIZEOF(.data) + SIZEOF(.fast);
+    .tdata : AT(__tdata_load_addr__) {
+        . = ALIGN(8);
+        PROVIDE(__tdata_start__ = .);
+        *(.tdata)
+        *(.tdata.*)
+        *(.gnu.linkonce.td.*)
+        . = ALIGN(8);
+        PROVIDE(__tdata_end__ = .);
+    } > SDRAM
+
+    .tbss (NOLOAD) : {
+        . = ALIGN(8);
+        PROVIDE(__tbss_start__ = .);
+        __thread_pointer$ = .;
+        *(.tbss)
+        *(.tbss.*)
+        *(.gnu.linkonce.tb.*)
+        *(.tcommon)
+        . = ALIGN(8);
+        PROVIDE(__tbss_end__ = .);
+    } > SDRAM
+
+    __noncacheable_init_load_addr__ = etext + SIZEOF(.data) + SIZEOF(.fast) + SIZEOF(.tdata);
+    .noncacheable.init : AT(__noncacheable_init_load_addr__) {
+        . = ALIGN(8);
+        __noncacheable_init_start__ = .;
+        KEEP(*(.noncacheable.init))
+        KEEP(*(.noncacheable.init.*))
+        __noncacheable_init_end__ = .;
+        . = ALIGN(8);
+    } > NONCACHEABLE_RAM
+
+    __fast_ram_init_load_addr__ = etext + SIZEOF(.data) + SIZEOF(.fast) + SIZEOF(.tdata) + SIZEOF(.noncacheable.init);
+    .fast_ram.init : AT(__fast_ram_init_load_addr__) {
+        . = ALIGN(8);
+        __fast_ram_init_start__ = .;
+        KEEP(*(.fast_ram.init))
+        KEEP(*(.fast_ram.init.*))
+        __fast_ram_init_end__ = .;
+        . = ALIGN(8);
+    } > DLM
+
+    .bss (NOLOAD) : {
+        . = ALIGN(8);
+        __bss_start__ = .;
+        *(.bss)
+        *(.bss*)
+        *(.sbss*)
+        *(.scommon)
+        *(.scommon*)
+        *(.dynsbss*)
+        *(COMMON)
+        . = ALIGN(8);
+        _end = .;
+        __bss_end__ = .;
+    } > SDRAM
+
+    .framebuffer (NOLOAD) : {
+        . = ALIGN(8);
+        KEEP(*(.framebuffer))
+        KEEP(*(.framebuffer.*))
+        . = ALIGN(8);
+    } > SDRAM
+
+    .heap (NOLOAD) : {
+        . = ALIGN(8);
+        __heap_start__ = .;
+        . += HEAP_SIZE;
+        __heap_end__ = .;
+    } > SDRAM
+
+    .noncacheable (NOLOAD) : {
+        . = ALIGN(8);
+        KEEP(*(.noncacheable))  /* Legacy sections, kept for backwards compatibility */
+        KEEP(*(.noncacheable.non_init))
+        KEEP(*(.noncacheable.non_init.*))
+        __noncacheable_bss_start__ = .;
+        KEEP(*(.noncacheable.bss))
+        KEEP(*(.noncacheable.bss.*))
+        __noncacheable_bss_end__ = .;
+        . = ALIGN(8);
+    } > NONCACHEABLE_RAM
+
+    .sh_mem (NOLOAD) : {
+        KEEP(*(.sh_mem))
+        KEEP(*(.sh_mem.*))
+    } > SHARE_RAM
+
+    .ahb_sram (NOLOAD) : {
+        KEEP(*(.ahb_sram))
+        KEEP(*(.ahb_sram.*))
+    } > AHB_SRAM
+
+    .apb_sram (NOLOAD) : {
+        KEEP(*(.backup_sram))
+        KEEP(*(.backup_sram.*))
+    } > APB_SRAM
+
+    .fast_ram (NOLOAD) : {
+        . = ALIGN(8);
+        KEEP(*(.fast_ram))  /* Legacy sections, kept for backwards compatibility */
+        KEEP(*(.fast_ram.non_init))
+        KEEP(*(.fast_ram.non_init.*))
+        __fast_ram_bss_start__ = .;
+        KEEP(*(.fast_ram.bss))
+        KEEP(*(.fast_ram.bss.*))
+        __fast_ram_bss_end__ = .;
+        . = ALIGN(8);
+    } > DLM
+
+    .stack (NOLOAD) : {
+        . = ALIGN(16);
+        __stack_base__ = .;
+        . += STACK_SIZE;
+        . = ALIGN(16);
+        PROVIDE (_stack = .);
+        PROVIDE (_stack_safe = .);
+    } > DLM
+
+    __noncacheable_start__ = ORIGIN(NONCACHEABLE_RAM);
+    __noncacheable_end__ = ORIGIN(NONCACHEABLE_RAM) + LENGTH(NONCACHEABLE_RAM);
+    __share_mem_start__ = ORIGIN(SHARE_RAM);
+    __share_mem_end__ = ORIGIN(SHARE_RAM) + LENGTH(SHARE_RAM);
+
+    __fw_size__ = SIZEOF(.start) + SIZEOF(.vectors) + SIZEOF(.rel) + SIZEOF(.text) + SIZEOF(.data) + SIZEOF(.fast) + SIZEOF(.tdata) + SIZEOF(.noncacheable.init) + SIZEOF(.fast_ram.init);
+    __last_addr__ = __fast_ram_init_load_addr__ + SIZEOF(.fast_ram.init);
+    ASSERT(((__fw_size__ <= LENGTH(XPI0)) && (__last_addr__ <= (ORIGIN(XPI0) + LENGTH(XPI0)))), "******  FAILED! XPI0 has not enough space!  ******")
+}

controller_yy_app/linkers/iar/user_linker.icf

@@ -0,0 +1,128 @@
+/*
+ * Copyright (c) 2024 HPMicro
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+define exported symbol _link_file_version_2 = 1;
+
+define memory with size = 4G;
+
+/* Sizes */
+if (isdefinedsymbol(CSTACK_SIZE)) {
+  define symbol __size_cstack__        = CSTACK_SIZE;
+} else {
+  define symbol __size_cstack__        = 0x4000;
+}
+
+if (isdefinedsymbol(HEAP_SIZE)) {
+  define symbol __size_heap__          = HEAP_SIZE;
+} else {
+  define symbol __size_heap__          = 0x4000;
+}
+
+if (isdefinedsymbol(_flash_size)) {
+  define symbol __size_flash__          = _flash_size;
+} else {
+  define symbol __size_flash__          = 8M;
+}
+
+if (isdefinedsymbol(_extram_size)) {
+  define symbol __size_exram__          = _extram_size;
+} else {
+  define symbol __size_exram__          = 16M;
+}
+
+define symbol __size_safestack__        = 256;
+
+/* Regions */
+define region NOR_CFG_OPTION = mem:[ from 0x80000400 size 0x0C00 ];
+define region BOOT_HEADER = mem:[ from 0x80001000 size 0x2000 ];
+define region XPI0 = mem:[from 0x80003000 size (__size_flash__ - 0x3000) ];   /* XPI0 */
+define region ILM = mem:[from 0x00000000 size 256k];         /* ILM */
+define region DLM = mem:[from 0x00080000 size 256k];         /* DLM */
+define region AXI_SRAM = mem:[from 0x01080000 size 768k];    /* AXI SRAM */
+define region SHARE_RAM = mem:[from 0x0117C000 size 16k];
+define region SDRAM  = mem:[from 0x40000000 size __size_exram__ - 4M];
+define region NONCACHEABLE_RAM = mem:[from 0x40000000 + __size_exram__ - 4M size 4M];
+define region AHB_SRAM = mem:[from 0xF0300000 size 32k];
+define region APB_SRAM = mem:[from 0xF40F0000 size 8k];
+
+/* Blocks */
+define block vectors with fixed order                       { section .vector_table, section .isr_vector };
+
+define block HEAP  with size = __size_heap__,  alignment = 8 { };
+define block CSTACK with size = __size_cstack__, alignment = 16 { };
+define block SAFESTACK with size = __size_safestack__, alignment = 8 { };
+define block RW_DATA with static base GPREL { rw data };
+
+define block boot_header with fixed order                    { section .boot_header, section .fw_info_table, section .dc_info };
+define block cherryusb_usbh_class_info                       { section .usbh_class_info };
+define block framebuffer                                     { section .framebuffer, section .framebuffer.* };
+
+define block rtthread_FSymTab                                { section FSymTab };
+define block rtthread_VSymTab                                { section VSymTab };
+define block rtthread_rti_fn with alphabetical order         { section .rti_fn* };
+define block rtthread_RTMSymTab                              { section RTMSymTab };
+
+/* Symbols */
+define exported symbol __nor_cfg_option_load_addr__ = start(NOR_CFG_OPTION);
+define exported symbol __boot_header_load_addr__ = start(BOOT_HEADER);
+define exported symbol __app_load_addr__ = start(XPI0);
+define exported symbol __app_offset__ = __app_load_addr__ - __boot_header_load_addr__;
+define exported symbol __boot_header_length__ = size(BOOT_HEADER);
+define exported symbol __fw_size__ = 0x1000;
+
+define exported symbol __noncacheable_start__ = start(NONCACHEABLE_RAM);
+define exported symbol __noncacheable_end__ = end(NONCACHEABLE_RAM) + 1;
+define exported symbol __share_mem_start__ = start(SHARE_RAM);
+define exported symbol __share_mem_end__ = end(SHARE_RAM) + 1;
+
+/* Initialization */
+do not initialize                           { section .noncacheable, section .fast_ram };    // Legacy sections, kept for backwards compatibility
+do not initialize                           { section .non_init, section .non_init.*, section .*.non_init, section .*.non_init.* };
+do not initialize                           { section .no_init, section .no_init.*, section .*.no_init, section .*.no_init.* };   // Legacy sections, kept for backwards compatibility
+do not initialize                           { section .noinit, section .noinit.*, section .*.noinit, section .*.noinit.* };       // Legacy sections, used by some SDKs/HALs
+do not initialize                           { section .backup_sram, section .backup_sram.*};
+
+initialize by copy with packing=auto        { section .noncacheable.init, section .noncacheable.init.*, section .fast_ram.init, section .fast_ram.init.* };
+initialize by copy with packing=auto        { section .fast, section .fast.*, section .*.fast, section .*.fast.* };               // "RAM Code" sections
+initialize by copy with packing=none        { section .vector_table };
+initialize by copy with packing=none        { section .isr_vector };
+initialize by copy                          { readwrite };
+
+/* Placement */
+place in NOR_CFG_OPTION                     { section .nor_cfg_option };
+place in BOOT_HEADER                        { block boot_header };
+place at start of XPI0                      { symbol _start };
+place in XPI0                               { block rtthread_FSymTab,
+                                              block rtthread_VSymTab,
+                                              block rtthread_rti_fn,
+                                              block rtthread_RTMSymTab,
+                                              block cherryusb_usbh_class_info,
+                                              readonly
+                                            };
+place at start of ILM                       { block vectors };
+place in ILM                                {
+                                              section .fast, section .fast.*,                       // "ramfunc" section
+                                            };
+place in SDRAM                              { block framebuffer };
+place in AXI_SRAM | SDRAM                   {
+                                              block RW_DATA
+                                            };
+place in NONCACHEABLE_RAM                   { section .noncacheable.non_init, section .noncacheable.non_init.*, section .noncacheable.init, section .noncacheable.init.*, section .noncacheable.bss, section .noncacheable.bss.*, section .noncacheable };  // Noncacheable
+place in SHARE_RAM                          { section .sh_mem, section .sh_mem.*};                                     // Share memory
+place in AHB_SRAM                           { section .ahb_sram, section .ahb_sram.*};                                   // AHB SRAM memory
+place in APB_SRAM                           { section .backup_sram, section .backup_sram.*};                                // Backup SRAM memory
+place in DLM                                { section .fast_ram.init, section .fast_ram.init.*, section .fast_ram.non_init,  section .fast_ram.non_init.*, section .fast_ram.bss, section .fast_ram.bss.*, section .fast_ram };   // Fast access memory
+place in SDRAM                              { block HEAP };                                         // Heap reserved block
+place at end of DLM                         { block CSTACK, block SAFESTACK };                      // Stack reserved block, Safe Stack reserved block
+
+/* Keep */
+keep                                        { section .nor_cfg_option, section .boot_header, section .fw_info_table, section .dc_info };
+keep                                        { section .usbh_class_info,
+                                              section FSymTab,
+                                              section VSymTab,
+                                              section .rti_fn*,
+                                              section RTMSymTab
+                                            };
+keep symbol __iar_cstart_init_gp;

controller_yy_app/linkers/segger/user_linker.icf

@@ -0,0 +1,138 @@
+/*
+ * Copyright (c) 2024 HPMicro
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+
+define memory with size = 4G;
+
+/* Regions */
+define region NOR_CFG_OPTION = [ from 0x80000400 size 0x0C00 ];
+define region BOOT_HEADER = [ from 0x80001000 size 0x2000 ];
+define region XPI0 = [from 0x80003000 size (_flash_size - 0x3000) ];   /* XPI0 */
+define region ILM   = [from 0x00000000 size 256k];        /* ILM */
+define region DLM   = [from 0x00080000 size 256k];        /* DLM */
+define region AXI_SRAM = [from 0x01080000 size 512k];    /* AXI SRAM0 */
+define region NONCACHEABLE_RAM = [from 0x01100000 size 256k];    /* AXI SRAM1 */
+define region SHARE_RAM = [from 0x0117C000 size 16k];
+define region AHB_SRAM = [from 0xF0300000 size 32k];
+define region APB_SRAM = [from 0xF40F0000 size 8k];
+
+/* Blocks */
+define block vectors with fixed order       { section .vector_table, section .isr_vector };
+define block ctors                          { section .ctors,     section .ctors.*, block with         alphabetical order { init_array } };
+define block dtors                          { section .dtors,     section .dtors.*, block with reverse alphabetical order { fini_array } };
+define block eh_frame                       { section .eh_frame,  section .eh_frame.* };
+define block tbss                           { section .tbss,      section .tbss.*  };
+define block tdata                          { section .tdata,     section .tdata.* };
+define block tls   with fixed order         { block tbss, block tdata };
+define block tdata_load                     { copy of block tdata };
+define block heap  with size = __HEAPSIZE__,  alignment = 8, /* fill =0x00, */ readwrite access { };
+define block stack with size = __STACKSIZE__, alignment = 16, /* fill =0xCD, */ readwrite access { };
+define block boot_header with fixed order                     { section .boot_header, section .fw_info_table, section .dc_info };
+define block cherryusb_usbh_class_info with alignment = 8     { section .usbh_class_info };
+define block framebuffer with alignment = 8                   { section .framebuffer };
+define block rtthread_FSymTab                                 { section FSymTab };
+define block rtthread_VSymTab                                 { section VSymTab };
+define block rtthread_rti_fn with alphabetical order          { section .rti_fn* };
+define block rtthread_RTMSymTab                               { section RTMSymTab };
+
+/* Symbols */
+define exported symbol __nor_cfg_option_load_addr__ = start of region NOR_CFG_OPTION;
+define exported symbol __boot_header_load_addr__ = start of region BOOT_HEADER;
+define exported symbol __app_load_addr__ = start of region XPI0;
+define exported symbol __app_offset__ = __app_load_addr__ - __boot_header_load_addr__;
+define exported symbol __boot_header_length__ = size of block boot_header;
+define exported symbol __fw_size__ = 0x1000;
+
+define exported symbol __noncacheable_start__ = start of region NONCACHEABLE_RAM;
+define exported symbol __noncacheable_end__ = end of region NONCACHEABLE_RAM + 1;
+define exported symbol __share_mem_start__ = start of region SHARE_RAM;
+define exported symbol __share_mem_end__ = end of region SHARE_RAM + 1;
+
+define exported symbol _stack_safe = end of block stack + 1;
+define exported symbol _stack = end of block stack + 1;
+
+define exported symbol __usbh_class_info_start__ = start of block cherryusb_usbh_class_info;
+define exported symbol __usbh_class_info_end__ = end of block cherryusb_usbh_class_info + 1;
+
+define exported symbol __fsymtab_start = start of block rtthread_FSymTab;
+define exported symbol __fsymtab_end = end of block rtthread_FSymTab + 1;
+
+define exported symbol __vsymtab_start = start of block rtthread_VSymTab;
+define exported symbol __vsymtab_end = end of block rtthread_VSymTab + 1;
+
+define exported symbol __rt_init_start = start of block rtthread_rti_fn;
+define exported symbol __rt_init_end = end of block rtthread_rti_fn + 1;
+
+define exported symbol __rtmsymtab_start = start of block rtthread_RTMSymTab;
+define exported symbol __rtmsymtab_end = end of block rtthread_RTMSymTab + 1;
+
+/* Initialization */
+do not initialize                           { section .noncacheable, section .fast_ram };
+do not initialize                           { section .non_init, section .non_init.*, section .*.non_init, section .*.non_init.* };
+do not initialize                           { section .no_init, section .no_init.*, section .*.no_init, section .*.no_init.* };   // Legacy sections, kept for backwards compatibility
+do not initialize                           { section .noinit, section .noinit.*, section .*.noinit, section .*.noinit.* };       // Legacy sections, used by some SDKs/HALs
+do not initialize                           { section .backup_sram};
+
+initialize by copy with packing=auto        { section .noncacheable.init, section .fast_ram.init };
+initialize by copy with packing=none        { section .data, section .data.*, section .*.data, section .*.data.* };               // Static data sections
+initialize by copy with packing=auto        { section .sdata, section .sdata.* };
+initialize by copy with packing=auto        { section .fast, section .fast.*, section .*.fast, section .*.fast.*, section .text.*nx* };               // "RAM Code" sections
+
+initialize by calling __SEGGER_init_heap    { block heap  };                                        // Init the heap if there is one
+initialize by calling __SEGGER_init_ctors   { block ctors };                                        // Call constructors for global objects which need to be constructed before reaching main (if any). Make sure this is done after setting up heap.
+initialize by copy { block vectors };
+
+/* Placement */
+place in NOR_CFG_OPTION                  { section .nor_cfg_option };
+place in BOOT_HEADER with fixed order    { block boot_header };
+place at start of XPI0 with fixed order  { symbol _start};
+place at start of ILM with fixed order   { block vectors };
+place in XPI0 with minimum size order    {
+                                           block tdata_load,                        // Thread-local-storage load image
+                                           block ctors,                             // Constructors block
+                                           block dtors,                             // Destructors block
+                                           block eh_frame,                          // Exception frames placed directly into flash overriding default placement (sections writable)
+                                           block rtthread_FSymTab,
+                                           block rtthread_VSymTab,
+                                           block rtthread_rti_fn,
+                                           block rtthread_RTMSymTab,
+                                           block cherryusb_usbh_class_info,
+                                           readonly,                                // Catch-all for readonly data (e.g. .rodata, .srodata)
+                                           readexec                                 // Catch-all for (readonly) executable code (e.g. .text)
+                                         };
+
+//
+// The GNU compiler creates these exception-related sections as writeable.
+// Override the section header flag and make them readonly so they can be
+// placed into flash.
+//
+define access readonly { section .gcc_except_table, section .gcc_except_table.* };
+define access readonly { section .eh_frame, section .eh_frame.* };
+define access readonly { section .sdata.DW.* };
+
+place in ILM                              {
+                                           section .fast, section .fast.*, section .text.*nx*,    // "ramfunc" section
+                                          };
+
+place in AXI_SRAM                         { block framebuffer };
+
+place in AXI_SRAM                         {
+                                            block tls,                                            // Thread-local-storage block
+                                            readwrite,                                            // Catch-all for initialized/uninitialized data sections (e.g. .data, .noinit)
+                                            zeroinit                                              // Catch-all for zero-initialized data sections (e.g. .bss)
+                                          };
+
+
+place in NONCACHEABLE_RAM                 { section .noncacheable, section .noncacheable.init, section .noncacheable.bss };  // Noncacheable
+place in SHARE_RAM                        { section .sh_mem};                                     // Share memory
+place in AHB_SRAM                         { section .ahb_sram};                                   // AHB SRAM memory
+place in APB_SRAM                         { section .backup_sram};                                // Backup SRAM memory
+place in DLM                              { section .fast_ram.init, section .fast_ram, section .fast_ram.bss};   // Fast access memory
+place in DLM                              { block heap };                                         // Heap reserved block
+place at end of DLM                       { block stack };                                        // Stack reserved block
+
+/* Keep */
+keep { section .nor_cfg_option, section .boot_header, section .fw_info_table, section .dc_info };
+keep { section .usbh_class_info, section FSymTab, section VSymTab, section .rti_fn*, section RTMSymTab };

controller_yy_app/matrix/axis_angle.h

@@ -0,0 +1,30 @@
+#pragma once
+#include "helpler_funtions.h"
+#include "math.h"
+
+/**
+ * @brief 使用四元数构造轴角向量
+ *
+ * @param Q 四元数
+ * @param axisAngle 轴角
+ */
+static inline void AxisAngle_ByQuaternion(const float Q[4], float axisAngle[3])
+{
+    float q_imag[3] = {Q[1], Q[2], Q[3]};
+    float q_imag_mag = sqrtf(q_imag[0] * q_imag[0] + q_imag[1] * q_imag[1] +
+                             q_imag[2] * q_imag[2]);
+    float r;
+    if (q_imag_mag > 1e-10)
+    {
+        r = 2 * atan2f(q_imag_mag, Q[0]) / q_imag_mag;
+    }
+    else
+    {
+        r = 2 * sign(Q[0]);
+    }
+
+    for (int i = 0; i < 3; ++i)
+    {
+        axisAngle[i] = r * q_imag[i];
+    }
+}

controller_yy_app/matrix/dcm.h

@@ -0,0 +1,67 @@
+#pragma once
+
+#include <math.h>
+
+/**
+ * @brief 通过四元数构造旋转矩阵
+ * 
+ * @param Q 
+ * @param dcm 
+ */
+static inline void DCM_ByQuaternion(const float Q[4], float dcm[3][3])
+{
+    float Q0 = Q[0], Q1 = Q[1], Q2 = Q[2], Q3 = Q[3];
+    float Q0s = Q[0] * Q[0];
+    float Q1s = Q[1] * Q[1];
+    float Q2s = Q[2] * Q[2];
+    float Q3s = Q[3] * Q[3];
+    dcm[0][0] = Q0s + Q1s - Q2s - Q3s;
+    dcm[0][1] = 2.0f * (Q1 * Q2 - Q0 * Q3);
+    dcm[0][2] = 2.0f * (Q0 * Q2 + Q1 * Q3);
+    dcm[1][0] = 2.0f * (Q1 * Q2 + Q0 * Q3);
+    dcm[1][1] = Q0s - Q1s + Q2s - Q3s;
+    dcm[1][2] = 2.0f * (Q2 * Q3 - Q0 * Q1);
+    dcm[2][0] = 2.0f * (Q1 * Q3 - Q0 * Q2);
+    dcm[2][1] = 2.0f * (Q0 * Q1 + Q2 * Q3);
+    dcm[2][2] = Q0s - Q1s - Q2s + Q3s;
+}
+
+/**
+ * @brief 通过欧拉角构造旋转矩阵
+ *  欧拉角定义顺序必须为 航向->俯仰->横滚 rad
+ * @param yaw 航向角
+ * @param pitch 俯仰角
+ * @param roll 横滚角
+ * @param dcm 旋转矩阵, TbN阵
+ */
+static inline void DCM_ByEuler(float yaw, float pitch, float roll, float dcm[3][3])
+{
+    dcm[0][0] = cosf(roll) * cosf(yaw) - sinf(pitch) * sinf(roll) * sinf(yaw);
+    dcm[0][1] = -cosf(pitch) * sinf(yaw);
+    dcm[0][2] = sinf(roll) * cosf(yaw) + cosf(roll) * sinf(pitch) * sinf(yaw);
+
+    dcm[1][0] = cosf(roll) * sinf(yaw) + sinf(roll) * sinf(pitch) * cosf(yaw);
+    dcm[1][1] = cosf(pitch) * cosf(yaw);
+    dcm[1][2] = sinf(roll) * sinf(yaw) - cosf(roll) * sinf(pitch) * cosf(yaw);
+
+    dcm[2][0] = -sinf(roll) * cosf(pitch);
+    dcm[2][1] = sinf(pitch);
+    dcm[2][2] = cosf(roll) * cosf(pitch);
+}
+
+/**
+ * @brief 使用 frame1 xyz轴向量 在 frame2 下的坐标，构建旋转矩阵
+ *  x, y, z 必须为互相正交的单位向量
+ * @param x frame1 x 轴在 frame2 系下的单位坐标
+ * @param y frame1 y 轴在 frame2 系下的单位坐标
+ * @param z frame1 z 轴在 frame2 系下的单位坐标
+ */
+static inline void DCM_ByXYZAxis(const float X[3], const float Y[3], const float Z[3], float dcm[3][3])
+{
+    for (int i = 0; i < 3; ++i)
+    {
+        dcm[i][0] = X[i];
+        dcm[i][1] = Y[i];
+        dcm[i][2] = Z[i];
+    }
+}

controller_yy_app/matrix/euler.c

@@ -0,0 +1,34 @@
+#include "euler.h"
+#include <math.h>
+#include "helpler_funtions.h"
+#include "quaternion.h"
+#include "dcm.h"
+
+inline void Euler_ByDcm(const float dcm[3][3], euler_angle_t *euler)
+{
+    const float eps = 1e-4f;
+    /* 由于欧拉角描述姿态存在奇异点, 需要单独对奇异情况下人为定义角值 */
+
+    /* 如果 y 轴非竖直朝上或下 */
+    if (fabsf(dcm[2][1]) < 1.0f - eps)
+    {
+        euler->pitch = atanf(dcm[2][1] / sqrtf(1.0f - dcm[2][1] * dcm[2][1]));
+        euler->yaw = atan2f(-dcm[0][1], dcm[1][1]);
+        euler->roll = atan2f(-dcm[2][0], dcm[2][2]);
+    }
+    /* 如果 Y 轴竖直朝上或下 */
+    else
+    {
+        euler->pitch = sign(dcm[2][1]) * 0.5f * M_PI;
+        euler->roll = 0.0f;
+        /* 此时人为定义 y 轴在水平面的投影反向为航向角  */
+        euler->yaw = atan2f(dcm[0][2], -dcm[1][2]);
+    }
+}
+
+ void Euler_ByQuaternion(const float Q[4], euler_angle_t *euler)
+{
+    float dcm[3][3];
+    DCM_ByQuaternion(Q, dcm);
+    Euler_ByDcm(dcm, euler);
+}

controller_yy_app/matrix/euler.h

@@ -0,0 +1,17 @@
+#pragma once
+
+/* 欧拉角类型, 顺序 航向->俯仰->横滚 */
+typedef struct euler_angle
+{
+    float yaw;
+    float pitch;
+    float roll;
+} euler_angle_t;
+
+
+void Euler_ByDcm(const float dcm[3][3], euler_angle_t *euler);
+
+void Euler_ByQuaternion(const float Q[4], euler_angle_t *euler);
+
+
+

controller_yy_app/matrix/flt_butter.c

@@ -0,0 +1,155 @@
+#include "flt_butter.h"
+
+#include <float.h>
+#include <math.h>
+#include <stdint.h>
+
+int butter3_filter_init(LpfButter3 *butter, const float b[4],
+                        const float a[4])
+{
+
+    for (uint8_t i = 0; i < 4; i++)
+    {
+        butter->B[i] = b[i];
+        butter->A[i] = a[i];
+        butter->X[i] = butter->Y[i] = 0.0f;
+    }
+
+    return 0;
+}
+
+int butter3_filter_reset(float sample, LpfButter3 *butter)
+{
+
+    for (uint8_t i = 0; i < 4; i++)
+    {
+        butter->X[i] = butter->Y[i] = sample;
+    }
+
+    return 0;
+}
+
+float butter3_filter_apply(float in, LpfButter3 *butter)
+{
+    float out;
+    butter->X[3] = in;
+    /* a(1)*y(n) = b(1)*x(n) + b(2)*x(n-1) + ... + b(nb+1)*x(n-nb)
+                         - a(2)*y(n-1) - ... - a(na+1)*y(n-na)  */
+    butter->Y[3] = butter->B[0] * butter->X[3] + butter->B[1] * butter->X[2] +
+                   butter->B[2] * butter->X[1] + butter->B[3] * butter->X[0] -
+                   butter->A[1] * butter->Y[2] - butter->A[2] * butter->Y[1] -
+                   butter->A[3] * butter->Y[0];
+
+    /* we assume a(1)=1 */
+    out = butter->Y[3];
+
+    /* move X and Y */
+    butter->X[0] = butter->X[1];
+    butter->X[1] = butter->X[2];
+    butter->X[2] = butter->X[3];
+    butter->Y[0] = butter->Y[1];
+    butter->Y[1] = butter->Y[2];
+    butter->Y[2] = butter->Y[3];
+
+    return out;
+}
+
+void butter2_filter_disable(LpfButter2 *lp)
+{
+    lp->_sample_freq = 0.f;
+    lp->_cutoff_freq = 0.f;
+
+    lp->_delay_element1 = 0.0f;
+    lp->_delay_element2 = 0.0f;
+
+    lp->_b0 = 1.f;
+    lp->_b1 = 0.f;
+    lp->_b2 = 0.f;
+
+    lp->_a1 = 0.f;
+    lp->_a2 = 0.f;
+}
+
+int butter2_filter_set_cutoff_freq(LpfButter2 *lp, float sample_freq,
+                                   float cutoff_freq)
+{
+    if ((sample_freq <= 0.f) || (cutoff_freq <= 0.f) ||
+        (cutoff_freq >= sample_freq / 2) || !isfinite(sample_freq) ||
+        !isfinite(cutoff_freq))
+    {
+        butter2_filter_disable(lp);
+        return -1;
+    }
+
+    // reset delay elements on filter change
+    lp->_delay_element1 = 0;
+    lp->_delay_element2 = 0;
+
+    cutoff_freq = cutoff_freq >= 1.f ? cutoff_freq : 1.f;
+    cutoff_freq =
+        cutoff_freq <= (sample_freq / 2) ? cutoff_freq : (sample_freq / 2);
+    // TODO: min based on actual numerical limit
+
+    lp->_sample_freq = sample_freq;
+    lp->_cutoff_freq = cutoff_freq;
+
+    const float fr = lp->_sample_freq / lp->_cutoff_freq;
+    const float ohm = tanf(M_PI / fr);
+    const float c = 1.f + 2.f * cosf(M_PI / 4.f) * ohm + ohm * ohm;
+
+    lp->_b0 = ohm * ohm / c;
+    lp->_b1 = 2.f * lp->_b0;
+    lp->_b2 = lp->_b0;
+
+    lp->_a1 = 2.f * (ohm * ohm - 1.f) / c;
+    lp->_a2 = (1.f - 2.f * cosf(M_PI / 4.f) * ohm + ohm * ohm) / c;
+
+    if (!isfinite(lp->_b0) || !isfinite(lp->_b1) || !isfinite(lp->_b2) ||
+        !isfinite(lp->_a1) || !isfinite(lp->_a2))
+    {
+        butter2_filter_disable(lp);
+        return -1;
+    }
+
+    return 0;
+}
+
+int butter2_filter_init(LpfButter2 *butter, float sample_freq,
+                        float cuttoff_freq)
+{
+    return butter2_filter_set_cutoff_freq(butter, sample_freq, cuttoff_freq);
+}
+
+float butter2_filter_apply(LpfButter2 *lp, float sample)
+{
+    float delay_element_0 =
+        sample - lp->_delay_element1 * lp->_a1 - lp->_delay_element2 * lp->_a2;
+
+    const float output = delay_element_0 * lp->_b0 +
+                         lp->_delay_element1 * lp->_b1 +
+                         lp->_delay_element2 * lp->_b2;
+    lp->_delay_element2 = lp->_delay_element1;
+    lp->_delay_element1 = delay_element_0;
+    return output;
+}
+
+float butter2_filter_reset(LpfButter2 *lp, float sample)
+{
+    const float input = isfinite(sample) ? sample : 0.0f;
+
+    if (fabsf(1 + lp->_a1 + lp->_a2) > FLT_EPSILON)
+    {
+        lp->_delay_element1 = lp->_delay_element2 = input / (1 + lp->_a1 + lp->_a2);
+
+        if (!isfinite(lp->_delay_element1) || !isfinite(lp->_delay_element2))
+        {
+            lp->_delay_element1 = lp->_delay_element2 = input;
+        }
+    }
+    else
+    {
+        lp->_delay_element1 = lp->_delay_element2 = input;
+    }
+
+    return butter2_filter_apply(lp, input);
+}

controller_yy_app/matrix/flt_butter.h

@@ -0,0 +1,76 @@
+/******************************************************************************
+ * Copyright 2020 The Firmament Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#ifndef BUTTER_H__
+#define BUTTER_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/* 30Hz cut-off frequency, 1000Hz sampling frequency */
+#define BUTTER3_B_1000S_30C        \
+  {                                \
+    0.0007, 0.0021, 0.0021, 0.0007 \
+  }
+#define BUTTER3_A_1000S_30C       \
+  {                               \
+    1.0, -2.6236, 2.3147, -0.6855 \
+  }
+
+  typedef struct
+  {
+    float A[4];
+    float B[4];
+    float X[4];
+    float Y[4];
+  } LpfButter3;
+
+  typedef struct
+  {
+    // 历史采样数据
+    float _delay_element1;
+    float _delay_element2;
+
+    // 权重系数
+    float _a1;
+    float _a2;
+    float _b0;
+    float _b1;
+    float _b2;
+
+    // 截至频率
+    float _cutoff_freq;
+
+    // 采样频率
+    float _sample_freq;
+  } LpfButter2;
+
+  int butter3_filter_init(LpfButter3 *butter, const float b[4], const float a[4]);
+  int butter3_filter_reset(float sample, LpfButter3 *butter);
+  float butter3_filter_apply(float in, LpfButter3 *butter);
+
+  int butter2_filter_init(LpfButter2 *butter, float sample_freq,
+                          float cuttoff_freq);
+  float butter2_filter_reset(LpfButter2 *lp, float sample);
+  float butter2_filter_apply(LpfButter2 *lp, float sample);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

controller_yy_app/matrix/helpler_funtions.h

@@ -0,0 +1,76 @@
+#pragma once
+
+#include <float.h>
+#include <stdbool.h>
+
+#include "math.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+static inline int sign(float val)
+{
+    return (FLT_EPSILON < val) - (val < FLT_EPSILON);
+}
+
+static inline double wrap_double(double x, double low, double high)
+{   
+    if (low <= x && x < high)
+    {
+        return x;
+    }
+
+    const double range = high - low;
+    const double inv_range = 1.0f / range;
+    const double num_wraps = floor((x - low) * inv_range);
+    return x - range * num_wraps;
+}
+
+static inline float wrap_float(float x, float low, float high)
+{
+    if (low <= x && x < high)
+    {
+        return x;
+    }
+
+    const float range = high - low;
+    const float inv_range = 1.0f / range;
+    const float num_wraps = floorf((x - low) * inv_range);
+    return x - range * num_wraps;
+}
+
+static inline float wrap_pi(float x)
+{
+    return wrap_float(x, -M_PI, M_PI);
+}
+
+static inline float wrap_180(float x)
+{
+    return wrap_float(x, -180.0f, 180.0f);
+}
+
+static inline float wrap_360(float x)
+{
+    return wrap_float(x, 0.0f, 360.0f);
+}
+
+static inline float constrain_float(float x, float low, float high)
+{
+    if (x < low)
+        return low;
+    else if (x > high)
+        return high;
+    else
+        return x;
+}
+
+static inline float rad_to_deg(float rad)
+{
+    return (rad * 180.0f / M_PI);
+}
+
+static inline float deg_to_rad(float deg)
+{
+    return (deg * M_PI / 180.0f);
+}

controller_yy_app/matrix/matrix.h

@@ -0,0 +1,231 @@
+#pragma once
+
+#include "math.h"
+
+/**
+ * @brief 向量求模
+ *
+ * @param V 向量
+ * @param n V的维度
+ * @return float V的模值
+ */
+static inline float Vector_GetNorm(const float *V, int n)
+{
+    float norm = 0.0f;
+    for (int i = 0; i < n; i++)
+        norm += V[i] * V[i];
+    norm = sqrtf(norm);
+    return norm;
+}
+
+/**
+ * @brief 向量 V 归一化
+ * @param V 向量
+ * @param n V的维度
+ */
+static inline void Vector_Normalize(float *V, int n)
+{
+    float norm = 0.0f;
+    for (int i = 0; i < n; i++)
+        norm += V[i] * V[i];
+    norm = sqrtf(norm);
+    if (norm > 1e-6f)
+    {
+        for (int i = 0; i < n; ++i)
+        {
+            V[i] /= norm;
+        }
+    }
+}
+
+static inline void Vector_ConstrainNorm(float *V, int n, float norm_max)
+{
+    if (norm_max > 0)
+    {
+        float norm = Vector_GetNorm(V, n);
+        if (norm > norm_max)
+        {
+            float ratio = norm_max / norm;
+            for (int i = 0; i < n; ++i)
+            {
+                V[i] *= ratio;
+            }
+        }
+    }
+}
+
+/**
+ * @brief 向量点乘
+ *
+ * @param V1 向量
+ * @param V2 向量
+ * @param n V1 V1 的维数
+ * @return float V1 * V2 的结果
+ */
+static inline float Vector_DotProduct(const float *V1, const float *V2, int n)
+{
+    float dotProduct = 0.0f;
+    for (int i = 0; i < n; ++i)
+        dotProduct += V1[i] * V2[i];
+    return dotProduct;
+}
+
+/**
+ * @brief 向量和实数乘
+ *
+ * @param V 向量
+ * @param k 实数
+ * @param n 向量维数
+ */
+static inline void Vector_DotProductRealNum(float *V, float k, int n)
+{
+    for (int i = 0; i < n; ++i)
+    {
+        V[i] *= k;
+    }
+}
+
+/**
+ * @brief 3 纬向量叉乘
+ *
+ * @param V1
+ * @param V2
+ * @param V V1 x V2
+ */
+static inline void Vector_CrossProduct_3D(const float V1[3], const float V2[3], float *V)
+{
+    V[0] = V1[1] * V2[2] - V1[2] * V2[1];
+    V[1] = V1[2] * V2[0] - V1[0] * V2[2];
+    V[2] = V1[0] * V2[1] - V1[1] * V2[0];
+}
+
+static inline void Matrix_Init(float *P, const float *datap, int m, int n)
+{
+    for (int i = 0; i < m; i++)
+    {
+        for (int j = 0; j < n; j++)
+        {
+            P[i * n + j] = datap[i * n + j];
+        }
+    }
+}
+
+/**
+ * @brief 矩阵转置
+ *
+ * @param M 要转置的矩阵
+ * @param M_t 转置后的矩阵
+ * @param m 矩阵行数
+ * @param n 矩阵列数
+ */
+static inline void Matrix_Transpose(float *M, float *M_t, int m, int n)
+{
+    for (int i = 0; i < n; i++)
+    {
+        for (int j = 0; j < m; j++)
+        {
+            M_t[i * m + j] = M[j * n + i];
+        }
+    }
+}
+
+/**
+ * @brief 矩阵乘法
+ *
+ * @param P 矩阵
+ * @param Q 矩阵
+ * @param R P * Q
+ * @param l P的行数
+ * @param m P的列数 M的行数
+ * @param n M的列数
+ */
+static inline void Matrix_Multiplication(const float *P, const float *Q, float *R, int l,
+                                         int m, int n)
+{
+    // 将 R 矩阵零化
+    for (int i = 0; i < l; i++)
+    {
+        for (int j = 0; j < n; j++)
+        {
+            R[i * n + j] = 0.0f;
+        }
+    }
+    // 计算 P * Q = R
+    for (int i = 0; i < l; i++)
+    {
+        for (int j = 0; j < n; j++)
+        {
+            for (int k = 0; k < m; k++)
+            {
+                R[i * n + j] += P[i * m + k] * Q[k * n + j];
+            }
+        }
+    }
+}
+
+/**
+ * @brief 矩阵和实数乘
+ *
+ * @param P 矩阵
+ * @param k 实数
+ * @param m P的行数
+ * @param n P的列数
+ */
+static inline void Matrix_Multiplication_RealNum(float *P, float k, int m, int n)
+{
+    for (int i = 0; i < m; i++)
+    {
+        for (int j = 0; j < n; j++)
+        {
+            P[i * n + j] *= k;
+        }
+    }
+}
+
+/**
+ * @brief 方阵的迹
+ *
+ * @param P 方阵
+ * @param m P的行列维度
+ * @return float
+ */
+static inline float Matrix_GetTrace(const float *P, int m)
+{
+    float trace = 0.0f;
+    for (int i = 0; i < m; ++i)
+        trace += P[i * (m + 1)];
+
+    return trace;
+}
+
+/**
+ * @brief 矩阵按元素加
+ *
+ */
+static inline void Matrix_AdditionByElement(const float *P, const float *Q, float *R, int m,
+                                            int n)
+{
+    for (int i = 0; i < m; ++i)
+    {
+        for (int j = 0; j < n; ++j)
+        {
+            R[i * n + j] = P[i * n + j] + Q[i * n + j];
+        }
+    }
+}
+
+/**
+ * @brief 矩阵按元数减
+ *
+ */
+static inline void Matrix_SubtractionByElement(const float *P, const float *Q, float *R,
+                                               int m, int n)
+{
+    for (int i = 0; i < m; ++i)
+    {
+        for (int j = 0; j < n; ++j)
+        {
+            R[i * n + j] = P[i * n + j] - Q[i * n + j];
+        }
+    }
+}

controller_yy_app/matrix/quaternion.c

@@ -0,0 +1,365 @@
+#include "quaternion.h"
+#include "matrix.h"
+#include "float.h"
+#include "math.h"
+#include "helpler_funtions.h"
+
+///\ @brief 四元数的简单构造
+void Quaternion(float q[4])
+{
+    q[0] = 1.0f;
+    for (int i = 1; i < 4; ++i)
+    {
+        q[i] = 0.0f;
+    }
+}
+
+///\ @brief 四元数归一化为单位四元数
+void Quaternion_Normalize(float Q[4])
+{
+    float q_norm = Vector_GetNorm(Q, 4);
+    Vector_DotProductRealNum(Q, 1.0F / q_norm, 4);
+}
+
+/**
+ * @brief 四元数规范化 保证实部不小于0
+ *
+ */
+void Quaternion_Canonicalize(float Q[4])
+{
+    for (int i = 0; i < 4; ++i)
+    {
+        if (fabsf(Q[i]) > FLT_EPSILON)
+        {
+            Vector_DotProductRealNum(Q, sign(Q[i]), 4);
+            return;
+        }
+    }
+}
+
+/**
+ * @brief 使用旋转矩阵构造四元数
+ *
+ * @param Q 四元数
+ * @param R 旋转矩阵
+ */
+void Quaternion_ByDcm(const float R[3][3], float Q[4])
+{
+    float t = Matrix_GetTrace(&R[0][0], 3);
+
+    if (t > 0.0f)
+    {
+        t = sqrtf(1.0f + t);
+        Q[0] = 0.5f * t;
+        t = 0.5f / t;
+        Q[1] = (R[2][1] - R[1][2]) * t;
+        Q[2] = (R[0][2] - R[2][0]) * t;
+        Q[3] = (R[1][0] - R[0][1]) * t;
+    }
+    else if (R[0][0] > R[0][1] && R[0][0] > R[2][2])
+    {
+        t = sqrtf(1.0f + R[0][0] - R[1][1] - R[2][2]);
+        Q[1] = 0.5F * t;
+        t = 0.5f / t;
+        Q[0] = (R[2][1] - R[1][2]) * t;
+        Q[2] = (R[1][0] + R[0][1]) * t;
+        Q[3] = (R[0][2] + R[2][0]) * t;
+    }
+    else if (R[1][1] > R[2][2])
+    {
+        t = sqrtf(1.0f - R[0][0] + R[1][1] - R[2][2]);
+        Q[2] = 0.5F * t;
+        t = 0.5f / t;
+        Q[0] = (R[0][2] - R[2][0]) * t;
+        Q[1] = (R[1][0] + R[0][1]) * t;
+        Q[3] = (R[2][1] + R[1][2]) * t;
+    }
+    else
+    {
+        t = sqrtf(1.0f - R[0][0] - R[1][1] + R[2][2]);
+        Q[3] = 0.5F * t;
+        t = 0.5f / t;
+        Q[0] = (R[1][0] - R[0][1]) * t;
+        Q[1] = (R[0][2] + R[2][0]) * t;
+        Q[2] = (R[2][1] + R[1][2]) * t;
+    }
+}
+
+/**
+ * @brief 使用欧拉角构造四元数
+ *
+ * @param euler 欧拉角, 顺序必须为 航向—>俯仰->横滚, 单位 rad
+ * @param Q 四元数
+ */
+void Quaternion_ByEuler(float yaw, float pitch, float roll, float Q[4])
+{
+    Q[0] = (cosf(yaw / 2) * cosf(pitch / 2) * cosf(roll / 2) -
+        sinf(yaw / 2) * sinf(pitch / 2) * sinf(roll / 2));
+    Q[1] = (cosf(yaw / 2) * sinf(pitch / 2) * cosf(roll / 2) -
+        sinf(yaw / 2) * cosf(pitch / 2) * sinf(roll / 2));
+    Q[2] = (cosf(yaw / 2) * cosf(pitch / 2) * sinf(roll / 2) +
+        sinf(yaw / 2) * sinf(pitch / 2) * cosf(roll / 2));
+    Q[3] = (cosf(yaw / 2) * sinf(pitch / 2) * sinf(roll / 2) +
+        sinf(yaw / 2) * cosf(pitch / 2) * cosf(roll / 2));
+}
+
+/**
+ * @brief 使用轴角构造四元数
+ *
+ * @param anxisAngle 旋转轴角向量, 模为转角 rad, 方向为转轴
+ * @param Q 四元数
+ */
+void Quaternion_ByAxisAngle(const float anxisAngle[3], float Q[4])
+{
+    float angle = Vector_GetNorm(anxisAngle, 3);
+
+    if (angle < 1e-10)
+    {
+        /* 如果 angle 太小, 相当于没转 */
+        Quaternion(Q);
+    }
+    else
+    {
+        float axis[3] = { anxisAngle[0], anxisAngle[1], anxisAngle[2] };
+        Vector_DotProductRealNum(axis, 1.0f / angle, 3);
+        float magnitude = sinf(angle * 0.5f);
+        Q[0] = cosf(angle * 0.5f);
+        for (int i = 0; i < 3; i++)
+        {
+            Q[i + 1] = axis[i] * magnitude;
+        }
+    }
+}
+
+/**
+ * @brief 使用向量, 构造由 V1->V2 的最短旋转四元数
+ *
+ * @param Vsrc 旋转前的向量
+ * @param Vdst 旋转后的向量
+ * @param Q V1->V2 的最速旋转四元数
+ */
+void Quaternion_ByTwoVectors(const float Vsrc[3], const float Vdst[3], float Q[4])
+{
+    float Vcr[3], dot;
+    const float eps = 1e-5;
+    Vector_CrossProduct_3D(Vsrc, Vdst, Vcr);
+    dot = Vector_DotProduct(Vsrc, Vdst, 3);
+
+    if (dot < 0.0f && Vector_GetNorm(Vcr, 3) < eps)
+    {
+        /* 判断 Vsrc Vdst 是接近 180 度 */
+        float Vtmp[3] = { fabsf(Vcr[0]), fabsf(Vcr[1]), fabsf(Vcr[2]) };
+        if (Vtmp[0] < Vtmp[1])
+        {
+            if (Vtmp[0] < Vtmp[2])
+            {
+                Vtmp[0] = 1;
+                Vtmp[1] = 0;
+                Vtmp[2] = 0;
+            }
+            else
+            {
+                Vtmp[0] = 0;
+                Vtmp[1] = 0;
+                Vtmp[2] = 1;
+            }
+        }
+        else
+        {
+            if (Vtmp[1] < Vtmp[2])
+            {
+                Vtmp[0] = 0;
+                Vtmp[1] = 1;
+                Vtmp[2] = 0;
+            }
+            else
+            {
+                Vtmp[0] = 0;
+                Vtmp[1] = 0;
+                Vtmp[2] = 1;
+            }
+        }
+        Q[0] = 0;
+        Vector_CrossProduct_3D(Vsrc, Vtmp, Vcr);
+    }
+    else
+    {
+        Q[0] = dot +
+            sqrt(Vector_DotProduct(Vsrc, Vsrc, 3) * Vector_DotProduct(Vdst, Vdst, 3));
+    }
+
+    Q[1] = Vcr[0];
+    Q[2] = Vcr[1];
+    Q[3] = Vcr[2];
+
+    Quaternion_Normalize(Q);
+}
+
+/**
+ * @brief 单位四元数乘法
+ *
+ * @param Q1
+ * @param Q2
+ * @param Q Q1 X Q2
+ */
+void Quaternion_Multiplication(const float Q1[4], const float Q2[4], float Q[4])
+{
+    Q[0] = Q1[0] * Q2[0] - Q1[1] * Q2[1] - Q1[2] * Q2[2] - Q1[3] * Q2[3];
+    Q[1] = Q1[1] * Q2[0] + Q1[0] * Q2[1] - Q1[3] * Q2[2] + Q1[2] * Q2[3];
+    Q[2] = Q1[2] * Q2[0] + Q1[3] * Q2[1] + Q1[0] * Q2[2] - Q1[1] * Q2[3];
+    Q[3] = Q1[3] * Q2[0] - Q1[2] * Q2[1] + Q1[1] * Q2[2] + Q1[0] * Q2[3];
+}
+
+/**
+ * @brief 四元数求逆
+ *
+ * @param Q
+ * @param Q_inv
+ */
+void Quaternion_Inversed(const float Q[4], float Q_inv[4])
+{
+    float q_norm = Vector_GetNorm(Q, 4);
+    Q_inv[0] = Q[0] / q_norm;
+    Q_inv[1] = -Q[1] / q_norm;
+    Q_inv[2] = -Q[2] / q_norm;
+    Q_inv[3] = -Q[3] / q_norm;
+}
+
+/**
+ * @brief 将四元数求逆
+ *
+ * @param Q
+ */
+void Quaternion_Invert(float Q[4])
+{
+    float Q_inv[4];
+    Quaternion_Inversed(Q, Q_inv);
+
+    for (int i = 0; i < 4; ++i)
+    {
+        Q[i] = Q_inv[i];
+    }
+}
+
+/**
+ * @brief 根据 frame1 的角速率, 求 q21 的微分
+ *  比如 frame1 为机体坐标系, frame2 为东北天坐标系
+ *  V1 为向量在 frame1 的坐标, V2 为向量在 fram2 的坐标
+ *  [0, V2] = q21 * [0, V1] * q21^-1
+ *  q21_deriv = q21 * [0, 0.5 * angle_rate1]
+ *
+ * @param q21 当前四元数
+ * @param angle_rate1 frame 1 下的三轴角速率
+ * @param q21_deriv q21 的微分
+ */
+void Quaternion_Derivative1(const float q21[4], const float angle_rate1[3], float q21_deriv[4])
+{
+    float v[4] = { 0,
+                  0.5f * angle_rate1[0],
+                  0.5f * angle_rate1[1],
+                  0.5f * angle_rate1[2] };
+
+    Quaternion_Multiplication(q21, v, q21_deriv);
+}
+
+/**
+ * @brief 根据 frame2 的角速率, 求 q21 的微分
+ *  比如 frame1 为机体坐标系, frame2 为东北天坐标系
+ *  V1 为向量在 frame1 的坐标, V2 为向量在 fram2 的坐标
+ *  q21 为 [0,V2] = q21 * [0,V1] * q21^-1
+ *  q21_deriv = [0, 0.5 * angle_rate1] * q21
+ *
+ * @param q21 当前四元数
+ * @param angle_rate1 frame 1 下的三轴角速率
+ * @param q21_deriv q21 的微分
+ */
+void Quaternion_Derivative2(const float q21[4], const float angle_rate2[3], float q21_deriv[4])
+{
+    float v[4] = { 0,
+                  0.5f * angle_rate2[0],
+                  0.5f * angle_rate2[1],
+                  0.5f * angle_rate2[2] };
+
+    Quaternion_Multiplication(v, q21, q21_deriv);
+}
+
+/**
+ * @brief 将四元数按照轴角进行旋转
+ *
+ * @param axisAngle 轴角向量
+ * @param Q 四元数
+ */
+void Quaternion_RotateByAxisAngle(const float axisAngle[3], float Q[4])
+{
+    /* 根据轴角向量计算旋转四元数 */
+    float q_rot[4], q_temp[4];
+    Quaternion_ByAxisAngle(axisAngle, q_rot);
+
+    Quaternion_Multiplication(q_rot, Q, q_temp);
+
+    for (int i = 0; i < 4; ++i)
+    {
+        Q[i] = q_temp[i];
+    }
+}
+
+/**
+ * @brief 通过四元数将 frame1 下的 V1 转到 frame2 下的 V2
+ *
+ * @param Q 四元数
+ * @param V1
+ * @param V2
+ */
+void Quaternion_Conj(const float Q[4], const float V1[3], float V2[3])
+{
+    float v_temp1[4] = { 0, V1[0], V1[1], V1[2] };
+    float v_temp2[4], v_temp3[4];
+
+    Quaternion_Multiplication(Q, v_temp1, v_temp2);
+    Quaternion_Inversed(Q, v_temp1);
+    Quaternion_Multiplication(v_temp2, v_temp1, v_temp3);
+
+    V2[0] = v_temp3[1];
+    V2[1] = v_temp3[2];
+    V2[2] = v_temp3[3];
+}
+
+/**
+ * @brief 通过四元数将 frame2 下的 V2 转到 frame1 下的 V1
+ *
+ * @param Q 四元数
+ * @param V1
+ * @param V2
+ */
+void Quaternion_ConjInv(const float Q[4], const float V2[3], float V1[3])
+{
+    float v_temp1[4] = { 0, V2[0], V2[1], V2[2] };
+    float v_temp2[4], v_temp3[4];
+
+    Quaternion_Multiplication(v_temp1, Q, v_temp2);
+    Quaternion_Inversed(Q, v_temp1);
+    Quaternion_Multiplication(v_temp1, v_temp2, v_temp3);
+
+    V1[0] = v_temp3[1];
+    V1[1] = v_temp3[2];
+    V1[2] = v_temp3[3];
+}
+
+/**
+ * @brief 通过四元数求其对应的旋转矩阵的 Z 轴
+ * 可参考由四元数构造旋转矩阵
+ *
+ * @param Q 四元数
+ * @param dcm_z 旋转矩阵 z 轴
+ */
+void Quaternion_GetDcmZ(const float Q[4], float dcm_z[3])
+{
+    float a = Q[0];
+    float b = Q[1];
+    float c = Q[2];
+    float d = Q[3];
+
+    dcm_z[0] = 2 * (a * c + b * d);
+    dcm_z[1] = 2 * (c * d - a * b);
+    dcm_z[2] = a * a - b * b - c * c + d * d;
+}

controller_yy_app/matrix/quaternion.h

@@ -0,0 +1,57 @@
+#pragma once
+
+#include "matrix.h"
+
+/// 四元数的简单构造
+void Quaternion(float q[4]);
+
+/// 四元数归一化
+void Quaternion_Normalize(float Q[4]);
+
+/// 四元数规范化
+void Quaternion_Canonicalize(float Q[4]);
+
+/// 使用旋转矩阵构造四元数
+void Quaternion_ByDcm(const float dcm[3][3], float Q[4]);
+
+/// 使用欧拉角构造四元数
+void Quaternion_ByEuler(float yaw, float pitch, float roll, float Q[4]);
+
+/// 使用旋转矩阵构造四元数
+void Quaternion_ByDcm(const float R[3][3], float Q[4]);
+
+/// 使用欧拉角构造四元数
+void Quaternion_ByEuler(float yaw, float pitch, float roll, float Q[4]);
+
+/// 使用轴角构造四元数
+void Quaternion_ByAxisAngle(const float anxisAngle[3], float Q[4]);
+
+/// 使用向量, 构造由 Vsrc->Vdst 的最短旋转四元数
+void Quaternion_ByTwoVectors(const float Vsrc[3], const float Vdst[3], float Q[4]);
+
+/// 四元数乘法
+void Quaternion_Multiplication(const float Q1[4], const float Q2[4], float Q[4]);
+
+/// 四元数求逆
+void Quaternion_Inversed(const float Q[4], float Q_inv[4]);
+
+/// 将四元数求逆
+void Quaternion_Invert(float Q[4]);
+
+/// 通过 frame1 的角速率求四元数 q21 的微分
+void Quaternion_Derivative1(const float q21[4], const float angle_rate1[3], float q21_deriv[4]);
+
+/// 通过 frame2 的角速率求四元数 q21 的微分
+void Quaternion_Derivative2(const float q21[4], const float angle_rate2[3], float q21_deriv[4]);
+
+/// 四元数通过轴角进行旋转
+void Quaternion_RotateByAxisAngle(const float axisAngle[3], float Q[4]);
+
+/// 通过四元数将 frame1 下的 V1 转到 frame2 下的 V2
+void Quaternion_Conj(const float Q[4], const float V1[3], float V2[3]);
+
+/// 通过四元数将 frame2 下的 V2 转到 frame1 下的 V1
+void Quaternion_ConjInv(const float Q[4], const float V2[3], float V1[3]);
+
+/// 通过四元数求其对应的旋转矩阵的 Z 轴
+void Quaternion_GetDcmZ(const float Q[4], float dcm_z[3]);

controller_yy_app/middleware/CANopenNode/301/CMakeLists.txt

@@ -0,0 +1,14 @@
+# Copyright (c) 2021-2024 HPMicro
+# SPDX-License-Identifier: BSD-3-Clause
+
+sdk_inc(.)
+sdk_src(CO_Emergency.c)
+sdk_src(CO_fifo.c)
+sdk_src(CO_HBconsumer.c)
+sdk_src(CO_NMT_Heartbeat.c)
+sdk_src(CO_PDO.c)
+sdk_src(CO_SDOclient.c)
+sdk_src(CO_SDOserver.c)
+sdk_src(CO_TIME.c)
+sdk_src(CO_SYNC.c)
+sdk_src(crc16-ccitt.c)

controller_yy_app/middleware/CANopenNode/301/CO_Emergency.c

@@ -0,0 +1,592 @@
+/*
+ * CANopen Emergency object.
+ *
+ * @file        CO_Emergency.c
+ * @ingroup     CO_Emergency
+ * @author      Janez Paternoster
+ * @copyright   2004 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <string.h>
+
+#include "301/CO_SDOserver.h"
+#include "301/CO_Emergency.h"
+
+/* verify configuration */
+#if CO_CONFIG_EM_ERR_STATUS_BITS_COUNT < (6*8) \
+    || CO_CONFIG_EM_ERR_STATUS_BITS_COUNT > 256
+#error CO_CONFIG_EM_ERR_STATUS_BITS_COUNT is not correct
+#endif
+
+#if (CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER
+/*
+ * Read received message from CAN module.
+ *
+ * Function will be called (by CAN receive interrupt) every time, when CAN
+ * message with correct identifier will be received. For more information and
+ * description of parameters see file CO_driver.h.
+ */
+static void CO_EM_receive(void *object, void *msg) {
+    CO_EM_t *em;
+
+    em = (CO_EM_t*)object;
+
+    if (em!=NULL && em->pFunctSignalRx!=NULL){
+        uint16_t ident = CO_CANrxMsg_readIdent(msg);
+        if (ident != 0x80) {
+            /* ignore sync messages (necessary if sync object is not used) */
+            uint8_t *data = CO_CANrxMsg_readData(msg);
+            uint16_t errorCode;
+            uint32_t infoCode;
+
+            memcpy(&errorCode, &data[0], sizeof(errorCode));
+            memcpy(&infoCode, &data[4], sizeof(infoCode));
+            em->pFunctSignalRx(ident,
+                               CO_SWAP_16(errorCode),
+                               data[2],
+                               data[3],
+                               CO_SWAP_32(infoCode));
+        }
+    }
+}
+#endif
+
+/*
+ * Function for accessing _Pre-Defined Error Field_ (index 0x1003) from SDO server.
+ *
+ * For more information see file CO_SDOserver.h.
+ */
+static CO_SDO_abortCode_t CO_ODF_1003(CO_ODF_arg_t *ODF_arg);
+static CO_SDO_abortCode_t CO_ODF_1003(CO_ODF_arg_t *ODF_arg){
+    CO_EMpr_t *emPr;
+    uint8_t value;
+    CO_SDO_abortCode_t ret = CO_SDO_AB_NONE;
+
+    emPr = (CO_EMpr_t*) ODF_arg->object;
+    value = ODF_arg->data[0];
+
+    if (ODF_arg->reading){
+        uint8_t noOfErrors;
+        noOfErrors = emPr->preDefErrNoOfErrors;
+
+        if (ODF_arg->subIndex == 0U){
+            ODF_arg->data[0] = noOfErrors;
+        }
+        else if (ODF_arg->subIndex > noOfErrors){
+            ret = CO_SDO_AB_NO_DATA;
+        }
+        else{
+            ret = CO_SDO_AB_NONE;
+        }
+    }
+    else{
+        /* only '0' may be written to subIndex 0 */
+        if (ODF_arg->subIndex == 0U){
+            if (value == 0U){
+                emPr->preDefErrNoOfErrors = 0U;
+            }
+            else{
+                ret = CO_SDO_AB_INVALID_VALUE;
+            }
+        }
+        else{
+            ret = CO_SDO_AB_READONLY;
+        }
+    }
+
+    return ret;
+}
+
+
+/*
+ * Function for accessing _COB ID EMCY_ (index 0x1014) from SDO server.
+ *
+ * For more information see file CO_SDOserver.h.
+ */
+static CO_SDO_abortCode_t CO_ODF_1014(CO_ODF_arg_t *ODF_arg);
+static CO_SDO_abortCode_t CO_ODF_1014(CO_ODF_arg_t *ODF_arg){
+    uint8_t *nodeId;
+    uint32_t value;
+    CO_SDO_abortCode_t ret = CO_SDO_AB_NONE;
+
+    nodeId = (uint8_t*) ODF_arg->object;
+    value = CO_getUint32(ODF_arg->data);
+
+    /* add nodeId to the value */
+    if (ODF_arg->reading){
+        CO_setUint32(ODF_arg->data, value + *nodeId);
+    }
+
+    return ret;
+}
+
+
+/******************************************************************************/
+CO_ReturnError_t CO_EM_init(
+        CO_EM_t                *em,
+        CO_EMpr_t              *emPr,
+        CO_SDO_t               *SDO,
+        uint8_t                *errorStatusBits,
+        uint8_t                 errorStatusBitsSize,
+        uint8_t                *errorRegister,
+        uint32_t               *preDefErr,
+        uint8_t                 preDefErrSize,
+        CO_CANmodule_t         *CANdevRx,
+        uint16_t                CANdevRxIdx,
+        CO_CANmodule_t         *CANdevTx,
+        uint16_t                CANdevTxIdx,
+        uint16_t                CANidTxEM)
+{
+    uint8_t i;
+    CO_ReturnError_t ret = CO_ERROR_NO;
+
+    /* verify arguments */
+    if (em==NULL || emPr==NULL || SDO==NULL || errorStatusBits==NULL || errorStatusBitsSize<6U ||
+       errorRegister==NULL || preDefErr==NULL || CANdevTx==NULL
+#if (CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER
+       || CANdevRx==NULL
+#endif
+    ){
+        return CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    /* Configure object variables */
+    em->errorStatusBits         = errorStatusBits;
+    em->errorStatusBitsSize     = errorStatusBitsSize;
+    em->bufEnd                  = em->buf + (CO_EM_INTERNAL_BUFFER_SIZE * 8);
+    em->bufWritePtr             = em->buf;
+    em->bufReadPtr              = em->buf;
+    em->bufFull                 = 0U;
+    em->wrongErrorReport        = 0U;
+#if (CO_CONFIG_EM) & CO_CONFIG_FLAG_CALLBACK_PRE
+    em->pFunctSignalPre         = NULL;
+    em->functSignalObjectPre    = NULL;
+#endif
+#if (CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER
+    em->pFunctSignalRx          = NULL;
+#endif
+    emPr->em                    = em;
+    emPr->errorRegister         = errorRegister;
+    emPr->preDefErr             = preDefErr;
+    emPr->preDefErrSize         = preDefErrSize;
+    emPr->preDefErrNoOfErrors   = 0U;
+    emPr->inhibitEmTimer        = 0U;
+    emPr->CANerrorStatusOld     = 0U;
+
+    /* clear error status bits */
+    for(i=0U; i<errorStatusBitsSize; i++){
+        em->errorStatusBits[i] = 0U;
+    }
+
+    /* Configure Object dictionary entry at index 0x1003 and 0x1014 */
+    CO_OD_configure(SDO, OD_H1003_PREDEF_ERR_FIELD, CO_ODF_1003, (void*)emPr, 0, 0U);
+    CO_OD_configure(SDO, OD_H1014_COBID_EMERGENCY, CO_ODF_1014, (void*)&SDO->nodeId, 0, 0U);
+
+#if (CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER
+    /* configure SDO server CAN reception */
+    ret = CO_CANrxBufferInit(
+            CANdevRx,               /* CAN device */
+            CANdevRxIdx,            /* rx buffer index */
+            CO_CAN_ID_EMERGENCY,    /* CAN identifier */
+            0x780,                  /* mask */
+            0,                      /* rtr */
+            (void*)em,              /* object passed to receive function */
+            CO_EM_receive);         /* this function will process received message */
+#endif
+
+    /* configure emergency message CAN transmission */
+    emPr->CANdev = CANdevTx;
+    emPr->CANtxBuff = CO_CANtxBufferInit(
+            CANdevTx,            /* CAN device */
+            CANdevTxIdx,        /* index of specific buffer inside CAN module */
+            CANidTxEM,          /* CAN identifier */
+            0,                  /* rtr */
+            8U,                 /* number of data bytes */
+            0);                 /* synchronous message flag bit */
+
+    if (emPr->CANtxBuff == NULL) {
+        ret = CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    return ret;
+}
+
+
+#if (CO_CONFIG_EM) & CO_CONFIG_FLAG_CALLBACK_PRE
+/******************************************************************************/
+void CO_EM_initCallbackPre(
+        CO_EM_t                *em,
+        void                   *object,
+        void                  (*pFunctSignal)(void *object))
+{
+    if (em != NULL){
+        em->functSignalObjectPre = object;
+        em->pFunctSignalPre = pFunctSignal;
+    }
+}
+#endif
+
+
+#if (CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER
+/******************************************************************************/
+void CO_EM_initCallbackRx(
+        CO_EM_t                *em,
+        void                  (*pFunctSignalRx)(const uint16_t ident,
+                                                const uint16_t errorCode,
+                                                const uint8_t errorRegister,
+                                                const uint8_t errorBit,
+                                                const uint32_t infoCode))
+{
+    if (em != NULL){
+        em->pFunctSignalRx = pFunctSignalRx;
+    }
+}
+#endif
+
+
+/******************************************************************************/
+void CO_EM_process(
+        CO_EMpr_t              *emPr,
+        bool_t                  NMTisPreOrOperational,
+        uint32_t                timeDifference_us,
+        uint16_t                emInhTime_100us,
+        uint32_t               *timerNext_us)
+{
+    (void)timerNext_us; /* may be unused */
+
+    CO_EM_t *em = emPr->em;
+    uint8_t errorRegister;
+    uint8_t errorMask;
+    uint8_t i;
+    uint32_t emInhTime_us = (uint32_t)emInhTime_100us * 100;
+    uint16_t CANerrSt = emPr->CANdev->CANerrorStatus;
+
+    /* verify errors from driver */
+    if (CANerrSt != emPr->CANerrorStatusOld) {
+        uint16_t CANerrStChanged = CANerrSt ^ emPr->CANerrorStatusOld;
+        emPr->CANerrorStatusOld = CANerrSt;
+
+        if (CANerrStChanged & (CO_CAN_ERRTX_WARNING | CO_CAN_ERRRX_WARNING)) {
+            if (CANerrSt & (CO_CAN_ERRTX_WARNING | CO_CAN_ERRRX_WARNING))
+                CO_errorReport(em, CO_EM_CAN_BUS_WARNING, CO_EMC_NO_ERROR, 0);
+            else
+                CO_errorReset(em, CO_EM_CAN_BUS_WARNING, 0);
+        }
+
+        if (CANerrStChanged & CO_CAN_ERRTX_PASSIVE) {
+            if (CANerrSt & CO_CAN_ERRTX_PASSIVE)
+                CO_errorReport(em, CO_EM_CAN_TX_BUS_PASSIVE,
+                               CO_EMC_CAN_PASSIVE, 0);
+            else
+                CO_errorReset(em, CO_EM_CAN_TX_BUS_PASSIVE, 0);
+        }
+
+        if (CANerrStChanged & CO_CAN_ERRTX_BUS_OFF) {
+            if (CANerrSt & CO_CAN_ERRTX_BUS_OFF)
+                CO_errorReport(em, CO_EM_CAN_TX_BUS_OFF,
+                               CO_EMC_BUS_OFF_RECOVERED, 0);
+            else
+                CO_errorReset(em, CO_EM_CAN_TX_BUS_OFF, 0);
+        }
+
+        if (CANerrStChanged & CO_CAN_ERRTX_OVERFLOW) {
+            if (CANerrSt & CO_CAN_ERRTX_OVERFLOW)
+                CO_errorReport(em, CO_EM_CAN_TX_OVERFLOW,
+                               CO_EMC_CAN_OVERRUN, 0);
+            else
+                CO_errorReset(em, CO_EM_CAN_TX_OVERFLOW, 0);
+        }
+
+        if (CANerrStChanged & CO_CAN_ERRTX_PDO_LATE) {
+            if (CANerrSt & CO_CAN_ERRTX_PDO_LATE)
+                CO_errorReport(em, CO_EM_TPDO_OUTSIDE_WINDOW,
+                               CO_EMC_COMMUNICATION, 0);
+            else
+                CO_errorReset(em, CO_EM_TPDO_OUTSIDE_WINDOW, 0);
+        }
+
+        if (CANerrStChanged & CO_CAN_ERRRX_PASSIVE) {
+            if (CANerrSt & CO_CAN_ERRRX_PASSIVE)
+                CO_errorReport(em, CO_EM_CAN_RX_BUS_PASSIVE,
+                               CO_EMC_CAN_PASSIVE, 0);
+            else
+                CO_errorReset(em, CO_EM_CAN_RX_BUS_PASSIVE, 0);
+        }
+
+        if (CANerrStChanged & CO_CAN_ERRRX_OVERFLOW) {
+            if (CANerrSt & CO_CAN_ERRRX_OVERFLOW)
+                CO_errorReport(em, CO_EM_CAN_RXB_OVERFLOW,
+                               CO_EMC_CAN_OVERRUN, 0);
+            else
+                CO_errorReset(em, CO_EM_CAN_RXB_OVERFLOW, 0);
+        }
+    }
+
+    /* verify other errors */
+    if (em->wrongErrorReport != 0U){
+        CO_errorReport(em, CO_EM_WRONG_ERROR_REPORT, CO_EMC_SOFTWARE_INTERNAL, (uint32_t)em->wrongErrorReport);
+        em->wrongErrorReport = 0U;
+    }
+
+
+    /* calculate Error register */
+    errorRegister = 0U;
+    errorMask = (uint8_t)~(CO_ERR_REG_GENERIC_ERR | CO_ERR_REG_COMM_ERR | CO_ERR_REG_MANUFACTURER);
+    /* generic error */
+    if (em->errorStatusBits[5]){
+        errorRegister |= CO_ERR_REG_GENERIC_ERR;
+    }
+    /* communication error (overrun, error state) */
+    if (em->errorStatusBits[2] || em->errorStatusBits[3]){
+        errorRegister |= CO_ERR_REG_COMM_ERR;
+    }
+    /* Manufacturer */
+    for(i=6; i<em->errorStatusBitsSize; i++) {
+        if (em->errorStatusBits[i]) {
+            errorRegister |= CO_ERR_REG_MANUFACTURER;
+        }
+    }
+    *emPr->errorRegister = (*emPr->errorRegister & errorMask) | errorRegister;
+
+    /* inhibit time */
+    if (emPr->inhibitEmTimer < emInhTime_us) {
+        emPr->inhibitEmTimer += timeDifference_us;
+    }
+
+    /* send Emergency message. */
+    if (     NMTisPreOrOperational &&
+            !emPr->CANtxBuff->bufferFull &&
+            (em->bufReadPtr != em->bufWritePtr || em->bufFull))
+    {
+        uint32_t preDEF;    /* preDefinedErrorField */
+
+        if (emPr->inhibitEmTimer >= emInhTime_us) {
+            /* inhibit time elapsed, send message */
+
+            /* add error register */
+            em->bufReadPtr[2] = *emPr->errorRegister;
+
+#if (CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER
+            /* report also own emergency messages */
+            if (em->pFunctSignalRx != NULL) {
+                uint16_t errorCode;
+                uint32_t infoCode;
+                memcpy(&errorCode, &em->bufReadPtr[0], sizeof(errorCode));
+                memcpy(&infoCode, &em->bufReadPtr[4], sizeof(infoCode));
+                em->pFunctSignalRx(0,
+                                   CO_SWAP_16(errorCode),
+                                   em->bufReadPtr[2],
+                                   em->bufReadPtr[3],
+                                   CO_SWAP_32(infoCode));
+            }
+#endif
+
+            /* copy data to CAN emergency message */
+            memcpy(emPr->CANtxBuff->data, em->bufReadPtr, sizeof(emPr->CANtxBuff->data));
+            memcpy(&preDEF, em->bufReadPtr, sizeof(preDEF));
+            em->bufReadPtr += 8;
+
+            /* Update read buffer pointer and reset inhibit timer */
+            if (em->bufReadPtr == em->bufEnd){
+                em->bufReadPtr = em->buf;
+            }
+            emPr->inhibitEmTimer = 0U;
+
+            /* verify message buffer overflow, then clear full flag */
+            if (em->bufFull == 2U){
+                em->bufFull = 0U;    /* will be updated below */
+                CO_errorReport(em, CO_EM_EMERGENCY_BUFFER_FULL, CO_EMC_GENERIC, 0U);
+            }
+            else{
+                em->bufFull = 0;
+                CO_errorReset(em, CO_EM_EMERGENCY_BUFFER_FULL, 0);
+            }
+
+            /* write to 'pre-defined error field' (object dictionary, index 0x1003) */
+            if (emPr->preDefErr){
+                uint8_t j;
+
+                if (emPr->preDefErrNoOfErrors < emPr->preDefErrSize)
+                    emPr->preDefErrNoOfErrors++;
+                for(j=emPr->preDefErrNoOfErrors-1; j>0; j--)
+                    emPr->preDefErr[j] = emPr->preDefErr[j-1];
+                emPr->preDefErr[0] = preDEF;
+            }
+
+            /* send CAN message */
+            CO_CANsend(emPr->CANdev, emPr->CANtxBuff);
+
+        }
+#if (CO_CONFIG_EM) & CO_CONFIG_FLAG_TIMERNEXT
+        else if (timerNext_us != NULL) {
+            uint32_t diff;
+            /* check again after inhibit time elapsed */
+            diff = emInhTime_us - emPr->inhibitEmTimer;
+            if (*timerNext_us > diff) {
+                *timerNext_us = diff;
+            }
+        }
+#endif
+    }
+
+    return;
+}
+
+
+/******************************************************************************/
+void CO_errorReport(CO_EM_t *em, const uint8_t errorBit, const uint16_t errorCode, const uint32_t infoCode){
+    uint8_t index = errorBit >> 3;
+    uint8_t bitmask = 1 << (errorBit & 0x7);
+    uint8_t *errorStatusBits = 0;
+    bool_t sendEmergency = true;
+
+    if (em == NULL){
+        sendEmergency = false;
+    }
+    else if (index >= em->errorStatusBitsSize){
+        /* if errorBit value not supported, send emergency 'CO_EM_WRONG_ERROR_REPORT' */
+        em->wrongErrorReport = errorBit;
+        sendEmergency = false;
+    }
+    else{
+        errorStatusBits = &em->errorStatusBits[index];
+        /* if error was already reported, do nothing */
+        if ((*errorStatusBits & bitmask) != 0){
+            sendEmergency = false;
+        }
+    }
+
+    if (sendEmergency){
+        /* set error bit */
+        if (errorBit){
+            /* any error except NO_ERROR */
+            *errorStatusBits |= bitmask;
+        }
+
+        /* verify buffer full, set overflow */
+        if (em->bufFull){
+            em->bufFull = 2;
+        }
+        else{
+            uint8_t bufCopy[8];
+            uint16_t errorCodeSw = CO_SWAP_16(errorCode);
+            uint32_t infoCodeSw = CO_SWAP_32(infoCode);
+
+            /* prepare data for emergency message */
+            memcpy(&bufCopy[0], &errorCodeSw, sizeof(errorCodeSw));
+            bufCopy[2] = 0; /* error register will be set later */
+            bufCopy[3] = errorBit;
+            memcpy(&bufCopy[4], &infoCodeSw, sizeof(infoCodeSw));
+
+            /* copy data to the buffer, increment writePtr and verify buffer full */
+            CO_LOCK_EMCY();
+            memcpy(em->bufWritePtr, bufCopy, sizeof(bufCopy));
+            em->bufWritePtr += 8;
+
+            if (em->bufWritePtr == em->bufEnd) em->bufWritePtr = em->buf;
+            if (em->bufWritePtr == em->bufReadPtr) em->bufFull = 1;
+            CO_UNLOCK_EMCY();
+
+#if (CO_CONFIG_EM) & CO_CONFIG_FLAG_CALLBACK_PRE
+            /* Optional signal to RTOS, which can resume task, which handles CO_EM_process */
+            if (em->pFunctSignalPre != NULL) {
+                em->pFunctSignalPre(em->functSignalObjectPre);
+            }
+#endif
+        }
+    }
+}
+
+
+/******************************************************************************/
+void CO_errorReset(CO_EM_t *em, const uint8_t errorBit, const uint32_t infoCode){
+    uint8_t index = errorBit >> 3;
+    uint8_t bitmask = 1 << (errorBit & 0x7);
+    uint8_t *errorStatusBits = 0;
+    bool_t sendEmergency = true;
+
+    if (em == NULL){
+        sendEmergency = false;
+    }
+    else if (index >= em->errorStatusBitsSize){
+        /* if errorBit value not supported, send emergency 'CO_EM_WRONG_ERROR_REPORT' */
+        em->wrongErrorReport = errorBit;
+        sendEmergency = false;
+    }
+    else{
+        errorStatusBits = &em->errorStatusBits[index];
+        /* if error was allready cleared, do nothing */
+        if ((*errorStatusBits & bitmask) == 0){
+            sendEmergency = false;
+        }
+    }
+
+    if (sendEmergency){
+        /* erase error bit */
+        *errorStatusBits &= ~bitmask;
+
+        /* verify buffer full */
+        if (em->bufFull){
+            em->bufFull = 2;
+        }
+        else{
+            uint8_t bufCopy[8];
+            uint32_t infoCodeSw = CO_SWAP_32(infoCode);
+
+            /* prepare data for emergency message */
+            bufCopy[0] = 0;
+            bufCopy[1] = 0;
+            bufCopy[2] = 0; /* error register will be set later */
+            bufCopy[3] = errorBit;
+            memcpy(&bufCopy[4], &infoCodeSw, sizeof(infoCodeSw));
+
+            /* copy data to the buffer, increment writePtr and verify buffer full */
+            CO_LOCK_EMCY();
+            memcpy(em->bufWritePtr, bufCopy, sizeof(bufCopy));
+            em->bufWritePtr += 8;
+
+            if (em->bufWritePtr == em->bufEnd) em->bufWritePtr = em->buf;
+            if (em->bufWritePtr == em->bufReadPtr) em->bufFull = 1;
+            CO_UNLOCK_EMCY();
+
+#if (CO_CONFIG_EM) & CO_CONFIG_FLAG_CALLBACK_PRE
+            /* Optional signal to RTOS, which can resume task, which handles CO_EM_process */
+            if (em->pFunctSignalPre != NULL) {
+                em->pFunctSignalPre(em->functSignalObjectPre);
+            }
+#endif
+        }
+    }
+}
+
+
+/******************************************************************************/
+bool_t CO_isError(CO_EM_t *em, const uint8_t errorBit){
+    uint8_t index = errorBit >> 3;
+    uint8_t bitmask = 1 << (errorBit & 0x7);
+    bool_t ret = false;
+
+    if (em != NULL && index < em->errorStatusBitsSize){
+        if ((em->errorStatusBits[index] & bitmask) != 0){
+            ret = true;
+        }
+    }
+
+    return ret;
+}

controller_yy_app/middleware/CANopenNode/301/CO_Emergency.h

@@ -0,0 +1,491 @@
+/**
+ * CANopen Emergency protocol.
+ *
+ * @file        CO_Emergency.h
+ * @ingroup     CO_Emergency
+ * @author      Janez Paternoster
+ * @copyright   2004 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CO_EMERGENCY_H
+#define CO_EMERGENCY_H
+
+#include "301/CO_driver.h"
+
+/* default configuration, see CO_config.h */
+#ifndef CO_CONFIG_EM
+#define CO_CONFIG_EM (CO_CONFIG_EM_PRODUCER)
+#endif
+#ifndef CO_CONFIG_EM_ERR_STATUS_BITS_COUNT
+#define CO_CONFIG_EM_ERR_STATUS_BITS_COUNT (10*8)
+#endif
+#ifndef CO_CONFIG_ERR_CONDITION_GENERIC
+#define CO_CONFIG_ERR_CONDITION_GENERIC (em->errorStatusBits[5] != 0)
+#endif
+#ifndef CO_CONFIG_ERR_CONDITION_COMMUNICATION
+#define CO_CONFIG_ERR_CONDITION_COMMUNICATION (em->errorStatusBits[2] \
+                                            || em->errorStatusBits[3])
+#endif
+#ifndef CO_CONFIG_ERR_CONDITION_MANUFACTURER
+#define CO_CONFIG_ERR_CONDITION_MANUFACTURER (em->errorStatusBits[8] \
+                                           || em->errorStatusBits[9])
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_Emergency Emergency
+ * @ingroup CO_CANopen_301
+ * @{
+ *
+ * CANopen Emergency protocol.
+ *
+ * Error control and Emergency is used for control internal error state
+ * and for sending a CANopen Emergency message.
+ *
+ * In case of error condition stack or application calls CO_errorReport()
+ * function with indication of the error. Specific error condition is reported
+ * (with CANopen Emergency message) only the first time after it occurs.
+ * Internal state of the error condition is controlled with
+ * @ref CO_EM_errorStatusBits. Specific error condition can also be reset by
+ * CO_errorReset() function. If so, Emergency message is sent with
+ * CO_EM_NO_ERROR indication.
+ *
+ * Some error conditions are informative and some are critical. Critical error
+ * conditions sets the #CO_errorRegisterBitmask_t.
+ *
+ * Latest errors can be read from _Pre Defined Error Field_ (object dictionary,
+ * index 0x1003). @ref CO_EM_errorStatusBits can also be read form CANopen
+ * object dictionary.
+ *
+ * ###Emergency message contents:
+ *
+ *   Byte | Description
+ *   -----|-----------------------------------------------------------
+ *   0..1 | @ref CO_EM_errorCodes.
+ *   2    | #CO_errorRegisterBitmask_t.
+ *   3    | Index of error condition (see @ref CO_EM_errorStatusBits).
+ *   4..7 | Additional argument informative to CO_errorReport() function.
+ *
+ * ####Contents of _Pre Defined Error Field_ (object dictionary, index 0x1003):
+ * bytes 0..3 are equal to bytes 0..3 in the Emergency message.
+ *
+ * @see #CO_Default_CAN_ID_t
+ */
+
+
+/**
+ * CANopen Error register.
+ *
+ * In object dictionary on index 0x1001.
+ *
+ * Error register is calculated from critical internal @ref CO_EM_errorStatusBits.
+ * Generic and communication bits are calculated in CO_EM_process
+ * function, device profile or manufacturer specific bits may be calculated
+ * inside the application.
+ *
+ * Internal errors may prevent device to stay in NMT Operational state. Details
+ * are described in _Error Behavior_ object in Object Dictionary at index 0x1029.
+ */
+typedef enum{
+    CO_ERR_REG_GENERIC_ERR  = 0x01U, /**< bit 0, generic error */
+    CO_ERR_REG_CURRENT      = 0x02U, /**< bit 1, current */
+    CO_ERR_REG_VOLTAGE      = 0x04U, /**< bit 2, voltage */
+    CO_ERR_REG_TEMPERATURE  = 0x08U, /**< bit 3, temperature */
+    CO_ERR_REG_COMM_ERR     = 0x10U, /**< bit 4, communication error (overrun, error state) */
+    CO_ERR_REG_DEV_PROFILE  = 0x20U, /**< bit 5, device profile specific */
+    CO_ERR_REG_RESERVED     = 0x40U, /**< bit 6, reserved (always 0) */
+    CO_ERR_REG_MANUFACTURER = 0x80U  /**< bit 7, manufacturer specific */
+}CO_errorRegisterBitmask_t;
+
+
+/**
+ * @defgroup CO_EM_errorCodes CANopen Error codes
+ * @{
+ *
+ * Standard error codes according to CiA DS-301 and DS-401.
+ */
+#define CO_EMC_NO_ERROR                 0x0000U /**< 0x00xx, error Reset or No Error */
+#define CO_EMC_GENERIC                  0x1000U /**< 0x10xx, Generic Error */
+#define CO_EMC_CURRENT                  0x2000U /**< 0x20xx, Current */
+#define CO_EMC_CURRENT_INPUT            0x2100U /**< 0x21xx, Current, device input side */
+#define CO_EMC_CURRENT_INSIDE           0x2200U /**< 0x22xx, Current inside the device */
+#define CO_EMC_CURRENT_OUTPUT           0x2300U /**< 0x23xx, Current, device output side */
+#define CO_EMC_VOLTAGE                  0x3000U /**< 0x30xx, Voltage */
+#define CO_EMC_VOLTAGE_MAINS            0x3100U /**< 0x31xx, Mains Voltage */
+#define CO_EMC_VOLTAGE_INSIDE           0x3200U /**< 0x32xx, Voltage inside the device */
+#define CO_EMC_VOLTAGE_OUTPUT           0x3300U /**< 0x33xx, Output Voltage */
+#define CO_EMC_TEMPERATURE              0x4000U /**< 0x40xx, Temperature */
+#define CO_EMC_TEMP_AMBIENT             0x4100U /**< 0x41xx, Ambient Temperature */
+#define CO_EMC_TEMP_DEVICE              0x4200U /**< 0x42xx, Device Temperature */
+#define CO_EMC_HARDWARE                 0x5000U /**< 0x50xx, Device Hardware */
+#define CO_EMC_SOFTWARE_DEVICE          0x6000U /**< 0x60xx, Device Software */
+#define CO_EMC_SOFTWARE_INTERNAL        0x6100U /**< 0x61xx, Internal Software */
+#define CO_EMC_SOFTWARE_USER            0x6200U /**< 0x62xx, User Software */
+#define CO_EMC_DATA_SET                 0x6300U /**< 0x63xx, Data Set */
+#define CO_EMC_ADDITIONAL_MODUL         0x7000U /**< 0x70xx, Additional Modules */
+#define CO_EMC_MONITORING               0x8000U /**< 0x80xx, Monitoring */
+#define CO_EMC_COMMUNICATION            0x8100U /**< 0x81xx, Communication */
+#define CO_EMC_CAN_OVERRUN              0x8110U /**< 0x8110, CAN Overrun (Objects lost) */
+#define CO_EMC_CAN_PASSIVE              0x8120U /**< 0x8120, CAN in Error Passive Mode */
+#define CO_EMC_HEARTBEAT                0x8130U /**< 0x8130, Life Guard Error or Heartbeat Error */
+#define CO_EMC_BUS_OFF_RECOVERED        0x8140U /**< 0x8140, recovered from bus off */
+#define CO_EMC_CAN_ID_COLLISION         0x8150U /**< 0x8150, CAN-ID collision */
+#define CO_EMC_PROTOCOL_ERROR           0x8200U /**< 0x82xx, Protocol Error */
+#define CO_EMC_PDO_LENGTH               0x8210U /**< 0x8210, PDO not processed due to length error */
+#define CO_EMC_PDO_LENGTH_EXC           0x8220U /**< 0x8220, PDO length exceeded */
+#define CO_EMC_DAM_MPDO                 0x8230U /**< 0x8230, DAM MPDO not processed, destination object not available */
+#define CO_EMC_SYNC_DATA_LENGTH         0x8240U /**< 0x8240, Unexpected SYNC data length */
+#define CO_EMC_RPDO_TIMEOUT             0x8250U /**< 0x8250, RPDO timeout */
+#define CO_EMC_TIME_DATA_LENGTH         0x8260U /**< 0x8260, Unexpected TIME data length */
+#define CO_EMC_EXTERNAL_ERROR           0x9000U /**< 0x90xx, External Error */
+#define CO_EMC_ADDITIONAL_FUNC          0xF000U /**< 0xF0xx, Additional Functions */
+#define CO_EMC_DEVICE_SPECIFIC          0xFF00U /**< 0xFFxx, Device specific */
+
+#define CO_EMC401_OUT_CUR_HI            0x2310U /**< 0x2310, DS401, Current at outputs too high (overload) */
+#define CO_EMC401_OUT_SHORTED           0x2320U /**< 0x2320, DS401, Short circuit at outputs */
+#define CO_EMC401_OUT_LOAD_DUMP         0x2330U /**< 0x2330, DS401, Load dump at outputs */
+#define CO_EMC401_IN_VOLT_HI            0x3110U /**< 0x3110, DS401, Input voltage too high */
+#define CO_EMC401_IN_VOLT_LOW           0x3120U /**< 0x3120, DS401, Input voltage too low */
+#define CO_EMC401_INTERN_VOLT_HI        0x3210U /**< 0x3210, DS401, Internal voltage too high */
+#define CO_EMC401_INTERN_VOLT_LO        0x3220U /**< 0x3220, DS401, Internal voltage too low */
+#define CO_EMC401_OUT_VOLT_HIGH         0x3310U /**< 0x3310, DS401, Output voltage too high */
+#define CO_EMC401_OUT_VOLT_LOW          0x3320U /**< 0x3320, DS401, Output voltage too low */
+/** @} */
+
+
+/**
+ * @defgroup CO_EM_errorStatusBits Error status bits
+ * @{
+ *
+ * Internal indication of the error condition.
+ *
+ * Each error condition is specified by unique index from 0x00 up to 0xFF.
+ * Variable  (from manufacturer section in the Object
+ * Dictionary) contains up to 0xFF bits (32bytes) for the identification of the
+ * specific error condition. (Type of the variable is CANopen OCTET_STRING.)
+ *
+ * If specific error occurs in the stack or in the application, CO_errorReport()
+ * sets specific bit in the _Error Status Bits_ variable. If bit was already
+ * set, function returns without any action. Otherwise it prepares emergency
+ * message.
+ *
+ * CO_errorReport(), CO_errorReset() or CO_isError() functions are called
+ * with unique index as an argument. (However CO_errorReport(), for example, may
+ * be used with the same index on multiple places in the code.)
+ *
+ * Macros defined below are combination of two constants: index and
+ * @ref CO_EM_errorCodes. They represents specific error conditions. They are
+ * used as double argument for CO_errorReport(), CO_errorReset() and
+ * CO_isError() functions.
+ *
+ * Stack uses first 6 bytes of the _Error Status Bits_ variable. Device profile
+ * or application may define own macros for Error status bits using
+ * @ref CO_EM_MANUFACTURER_START and @ref CO_EM_MANUFACTURER_END values. Note that
+ * _Error Status Bits_ must be large enough (up to 32 bytes).
+ */
+#define CO_EM_NO_ERROR                  0x00U /**< 0x00, Error Reset or No Error */
+#define CO_EM_CAN_BUS_WARNING           0x01U /**< 0x01, communication, info, CAN bus warning limit reached */
+#define CO_EM_RXMSG_WRONG_LENGTH        0x02U /**< 0x02, communication, info, Wrong data length of the received CAN message */
+#define CO_EM_RXMSG_OVERFLOW            0x03U /**< 0x03, communication, info, Previous received CAN message wasn't processed yet */
+#define CO_EM_RPDO_WRONG_LENGTH         0x04U /**< 0x04, communication, info, Wrong data length of received PDO */
+#define CO_EM_RPDO_OVERFLOW             0x05U /**< 0x05, communication, info, Previous received PDO wasn't processed yet */
+#define CO_EM_CAN_RX_BUS_PASSIVE        0x06U /**< 0x06, communication, info, CAN receive bus is passive */
+#define CO_EM_CAN_TX_BUS_PASSIVE        0x07U /**< 0x07, communication, info, CAN transmit bus is passive */
+#define CO_EM_NMT_WRONG_COMMAND         0x08U /**< 0x08, communication, info, Wrong NMT command received */
+#define CO_EM_TIME_TIMEOUT              0x09U /**< 0x09, communication, info, TIME message timeout */
+#define CO_EM_TIME_LENGTH               0x0AU /**< 0x0A, communication, info, Unexpected TIME data length */
+#define CO_EM_0B_unused                 0x0BU /**< 0x0B, (unused) */
+#define CO_EM_0C_unused                 0x0CU /**< 0x0C, (unused) */
+#define CO_EM_0D_unused                 0x0DU /**< 0x0D, (unused) */
+#define CO_EM_0E_unused                 0x0EU /**< 0x0E, (unused) */
+#define CO_EM_0F_unused                 0x0FU /**< 0x0F, (unused) */
+
+#define CO_EM_10_unused                 0x10U /**< 0x10, (unused) */
+#define CO_EM_11_unused                 0x11U /**< 0x11, (unused) */
+#define CO_EM_CAN_TX_BUS_OFF            0x12U /**< 0x12, communication, critical, CAN transmit bus is off */
+#define CO_EM_CAN_RXB_OVERFLOW          0x13U /**< 0x13, communication, critical, CAN module receive buffer has overflowed */
+#define CO_EM_CAN_TX_OVERFLOW           0x14U /**< 0x14, communication, critical, CAN transmit buffer has overflowed */
+#define CO_EM_TPDO_OUTSIDE_WINDOW       0x15U /**< 0x15, communication, critical, TPDO is outside SYNC window */
+#define CO_EM_16_unused                 0x16U /**< 0x16, (unused) */
+#define CO_EM_17_unused                 0x17U /**< 0x17, (unused) */
+#define CO_EM_SYNC_TIME_OUT             0x18U /**< 0x18, communication, critical, SYNC message timeout */
+#define CO_EM_SYNC_LENGTH               0x19U /**< 0x19, communication, critical, Unexpected SYNC data length */
+#define CO_EM_PDO_WRONG_MAPPING         0x1AU /**< 0x1A, communication, critical, Error with PDO mapping */
+#define CO_EM_HEARTBEAT_CONSUMER        0x1BU /**< 0x1B, communication, critical, Heartbeat consumer timeout */
+#define CO_EM_HB_CONSUMER_REMOTE_RESET  0x1CU /**< 0x1C, communication, critical, Heartbeat consumer detected remote node reset */
+#define CO_EM_1D_unused                 0x1DU /**< 0x1D, (unused) */
+#define CO_EM_1E_unused                 0x1EU /**< 0x1E, (unused) */
+#define CO_EM_1F_unused                 0x1FU /**< 0x1F, (unused) */
+
+#define CO_EM_EMERGENCY_BUFFER_FULL     0x20U /**< 0x20, generic, info, Emergency buffer is full, Emergency message wasn't sent */
+#define CO_EM_21_unused                 0x21U /**< 0x21, (unused) */
+#define CO_EM_MICROCONTROLLER_RESET     0x22U /**< 0x22, generic, info, Microcontroller has just started */
+#define CO_EM_23_unused                 0x23U /**< 0x23, (unused) */
+#define CO_EM_24_unused                 0x24U /**< 0x24, (unused) */
+#define CO_EM_25_unused                 0x25U /**< 0x25, (unused) */
+#define CO_EM_26_unused                 0x26U /**< 0x26, (unused) */
+#define CO_EM_27_unused                 0x27U /**< 0x27, (unused) */
+
+#define CO_EM_WRONG_ERROR_REPORT        0x28U /**< 0x28, generic, critical, Wrong parameters to CO_errorReport() function */
+#define CO_EM_ISR_TIMER_OVERFLOW        0x29U /**< 0x29, generic, critical, Timer task has overflowed */
+#define CO_EM_MEMORY_ALLOCATION_ERROR   0x2AU /**< 0x2A, generic, critical, Unable to allocate memory for objects */
+#define CO_EM_GENERIC_ERROR             0x2BU /**< 0x2B, generic, critical, Generic error, test usage */
+#define CO_EM_GENERIC_SOFTWARE_ERROR    0x2CU /**< 0x2C, generic, critical, Software error */
+#define CO_EM_INCONSISTENT_OBJECT_DICT  0x2DU /**< 0x2D, generic, critical, Object dictionary does not match the software */
+#define CO_EM_CALCULATION_OF_PARAMETERS 0x2EU /**< 0x2E, generic, critical, Error in calculation of device parameters */
+#define CO_EM_NON_VOLATILE_MEMORY       0x2FU /**< 0x2F, generic, critical, Error with access to non volatile device memory */
+
+#define CO_EM_MANUFACTURER_START        0x30U /**< 0x30, manufacturer, info, This can be used by macros to calculate error codes */
+#define CO_EM_MANUFACTURER_END          0xFFU /**< 0xFF, manufacturer, info, This can be used by macros to check error codes */
+/** @} */
+
+
+/**
+ * Size of internal buffer, whwre emergencies are stored after CO_errorReport().
+ * Buffer is cleared by CO_EM_process().
+ */
+#define CO_EM_INTERNAL_BUFFER_SIZE      10
+
+
+/**
+ * Emergerncy object for CO_errorReport(). It contains error buffer, to which new emergency
+ * messages are written, when CO_errorReport() is called. This object is included in
+ * CO_EMpr_t object.
+ */
+typedef struct{
+    uint8_t            *errorStatusBits;        /**< From CO_EM_init() */
+    uint8_t             errorStatusBitsSize;    /**< From CO_EM_init() */
+
+    /** Internal buffer for storing unsent emergency messages.*/
+    uint8_t             buf[CO_EM_INTERNAL_BUFFER_SIZE * 8];
+    uint8_t            *bufEnd;             /**< End+1 address of the above buffer */
+    uint8_t            *bufWritePtr;        /**< Write pointer in the above buffer */
+    uint8_t            *bufReadPtr;         /**< Read pointer in the above buffer */
+    uint8_t             bufFull;            /**< True if above buffer is full */
+    uint8_t             wrongErrorReport;   /**< Error in arguments to CO_errorReport() */
+#if ((CO_CONFIG_EM) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+    /** From CO_EM_initCallbackPre() or NULL */
+    void              (*pFunctSignalPre)(void *object);
+    /** From CO_EM_initCallbackPre() or NULL */
+    void               *functSignalObjectPre;
+#endif
+#if ((CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER) || defined CO_DOXYGEN
+    /** From CO_EM_initCallbackRx() or NULL */
+    void              (*pFunctSignalRx)(const uint16_t ident,
+                                        const uint16_t errorCode,
+                                        const uint8_t errorRegister,
+                                        const uint8_t errorBit,
+                                        const uint32_t infoCode);
+#endif
+}CO_EM_t;
+
+
+/**
+ * Report error condition.
+ *
+ * Function is called on any error condition inside CANopen stack and may also
+ * be called by application on custom error condition. Emergency message is sent
+ * after the first occurance of specific error. In case of critical error, device
+ * will not be able to stay in NMT_OPERATIONAL state.
+ *
+ * Function is short and may be used form any task or interrupt.
+ *
+ * @param em Emergency object.
+ * @param errorBit from @ref CO_EM_errorStatusBits.
+ * @param errorCode from @ref CO_EM_errorCodes.
+ * @param infoCode 32 bit value is passed to bytes 4...7 of the Emergency message.
+ * It contains optional additional information inside emergency message.
+ */
+void CO_errorReport(CO_EM_t *em, const uint8_t errorBit, const uint16_t errorCode, const uint32_t infoCode);
+
+
+/**
+ * Reset error condition.
+ *
+ * Function is called if any error condition is solved. Emergency message is sent
+ * with @ref CO_EM_errorCodes 0x0000.
+ *
+ * Function is short and may be used form any task or interrupt.
+ *
+ * @param em Emergency object.
+ * @param errorBit from @ref CO_EM_errorStatusBits.
+ * @param infoCode 32 bit value is passed to bytes 4...7 of the Emergency message.
+ */
+void CO_errorReset(CO_EM_t *em, const uint8_t errorBit, const uint32_t infoCode);
+
+
+/**
+ * Check specific error condition.
+ *
+ * Function returns 1, if specific internal error is present. Otherwise it returns 0.
+ *
+ * @param em Emergency object.
+ * @param errorBit from @ref CO_EM_errorStatusBits.
+ *
+ * @return false: Error is not present.
+ * @return true: Error is present.
+ */
+bool_t CO_isError(CO_EM_t *em, const uint8_t errorBit);
+
+
+#ifdef CO_DOXYGEN
+/** Skip section, if CO_SDOserver.h is not included */
+    #define CO_SDO_SERVER_H
+#endif
+#ifdef CO_SDO_SERVER_H
+
+
+/**
+ * Error control and Emergency object. It controls internal error state and
+ * sends emergency message, if error condition was reported. Object is initialized
+ * by CO_EM_init(). It contains CO_EM_t object.
+ */
+typedef struct{
+    uint8_t            *errorRegister;  /**< From CO_EM_init() */
+    uint32_t           *preDefErr;      /**< From CO_EM_init() */
+    uint8_t             preDefErrSize;  /**< From CO_EM_init() */
+    uint8_t             preDefErrNoOfErrors;/**< Number of active errors in preDefErr */
+    uint32_t            inhibitEmTimer; /**< Internal timer for emergency message */
+    CO_EM_t            *em;             /**< CO_EM_t sub object is included here */
+    uint16_t            CANerrorStatusOld;/**< Old CAN error status bitfield */
+    CO_CANmodule_t     *CANdev;         /**< From CO_EM_init() */
+    CO_CANtx_t         *CANtxBuff;      /**< CAN transmit buffer */
+}CO_EMpr_t;
+
+
+/**
+ * Initialize Error control and Emergency object.
+ *
+ * Function must be called in the communication reset section.
+ *
+ * @param emPr This object will be initialized.
+ * @param em Emergency object defined separately. Will be included in emPr and
+ * initialized too.
+ * @param SDO SDO server object.
+ * @param errorStatusBits Pointer to _Error Status Bits_ array from Object Dictionary
+ * (manufacturer specific section). See @ref CO_EM_errorStatusBits.
+ * @param errorStatusBitsSize Total size of the above array. Must be >= 6.
+ * @param errorRegister Pointer to _Error Register_ (Object dictionary, index 0x1001).
+ * @param preDefErr Pointer to _Pre defined error field_ array from Object
+ * dictionary, index 0x1003.
+ * @param preDefErrSize Size of the above array.
+ * @param CANdevRx CAN device for Emergency reception.
+ * @param CANdevRxIdx Index of receive buffer in the above CAN device.
+ * @param CANdevTx CAN device for Emergency transmission.
+ * @param CANdevTxIdx Index of transmit buffer in the above CAN device.
+ * @param CANidTxEM CAN identifier for Emergency message.
+ *
+ * @return #CO_ReturnError_t CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ */
+CO_ReturnError_t CO_EM_init(
+        CO_EM_t                *em,
+        CO_EMpr_t              *emPr,
+        CO_SDO_t               *SDO,
+        uint8_t                *errorStatusBits,
+        uint8_t                 errorStatusBitsSize,
+        uint8_t                *errorRegister,
+        uint32_t               *preDefErr,
+        uint8_t                 preDefErrSize,
+        CO_CANmodule_t         *CANdevRx,
+        uint16_t                CANdevRxIdx,
+        CO_CANmodule_t         *CANdevTx,
+        uint16_t                CANdevTxIdx,
+        uint16_t                CANidTxEM);
+
+
+#if ((CO_CONFIG_EM) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+/**
+ * Initialize Emergency callback function.
+ *
+ * Function initializes optional callback function, which should immediately
+ * start processing of CO_EM_process() function.
+ * Callback is called from CO_errorReport() or CO_errorReset() function. Those
+ * functions are fast and may be called from any thread. Callback should
+ * immediately start mainline thread, which calls CO_EM_process() function.
+ *
+ * @param em This object.
+ * @param object Pointer to object, which will be passed to pFunctSignal(). Can be NULL
+ * @param pFunctSignal Pointer to the callback function. Not called if NULL.
+ */
+void CO_EM_initCallbackPre(
+        CO_EM_t               *em,
+        void                   *object,
+        void                  (*pFunctSignal)(void *object));
+#endif
+
+
+#if ((CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER) || defined CO_DOXYGEN
+/**
+ * Initialize Emergency received callback function.
+ *
+ * Function initializes optional callback function, which executes after
+ * error condition is received.
+ *
+ * _ident_ argument from callback contains CAN-ID of the emergency message. If
+ * _ident_ == 0, then emergency message was sent from this device.
+ *
+ * @remark Depending on the CAN driver implementation, this function is called
+ * inside an ISR or inside a mainline. Must be thread safe.
+ *
+ * @param em This object.
+ * @param pFunctSignalRx Pointer to the callback function. Not called if NULL.
+ */
+void CO_EM_initCallbackRx(
+        CO_EM_t                *em,
+        void                  (*pFunctSignalRx)(const uint16_t ident,
+                                                const uint16_t errorCode,
+                                                const uint8_t errorRegister,
+                                                const uint8_t errorBit,
+                                                const uint32_t infoCode));
+#endif
+
+
+/**
+ * Process Error control and Emergency object.
+ *
+ * Function must be called cyclically. It verifies some communication errors,
+ * calculates bit 0 and bit 4 from _Error register_ and sends emergency message
+ * if necessary.
+ *
+ * @param emPr This object.
+ * @param NMTisPreOrOperational True if this node is NMT_PRE_OPERATIONAL or NMT_OPERATIONAL.
+ * @param timeDifference_us Time difference from previous function call in [microseconds].
+ * @param emInhTime _Inhibit time EMCY_ in [100*us] (object dictionary, index 0x1015).
+ * @param [out] timerNext_us info to OS - see CO_process().
+ */
+void CO_EM_process(
+        CO_EMpr_t              *emPr,
+        bool_t                  NMTisPreOrOperational,
+        uint32_t                timeDifference_us,
+        uint16_t                emInhTime,
+        uint32_t               *timerNext_us);
+
+
+#endif
+
+/** @} */ /* CO_Emergency */
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+#endif /* CO_EMERGENCY_H */

controller_yy_app/middleware/CANopenNode/301/CO_HBconsumer.c

@@ -0,0 +1,553 @@
+/*
+ * CANopen Heartbeat consumer object.
+ *
+ * @file        CO_HBconsumer.c
+ * @ingroup     CO_HBconsumer
+ * @author      Janez Paternoster
+ * @copyright   2004 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "301/CO_driver.h"
+#include "301/CO_SDOserver.h"
+#include "301/CO_Emergency.h"
+#include "301/CO_NMT_Heartbeat.h"
+#include "301/CO_HBconsumer.h"
+
+#if (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_ENABLE
+
+/* Verify HB consumer configuration *******************************************/
+#if (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE \
+    && (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI
+#error CO_CONFIG_HB_CONS_CALLBACK_CHANGE and CO_CONFIG_HB_CONS_CALLBACK_MULTI cannot be set simultaneously!
+#endif
+
+/*
+ * Read received message from CAN module.
+ *
+ * Function will be called (by CAN receive interrupt) every time, when CAN
+ * message with correct identifier will be received. For more information and
+ * description of parameters see file CO_driver.h.
+ */
+static void CO_HBcons_receive(void *object, void *msg){
+    CO_HBconsNode_t *HBconsNode;
+    uint8_t DLC = CO_CANrxMsg_readDLC(msg);
+    uint8_t *data = CO_CANrxMsg_readData(msg);
+
+    HBconsNode = (CO_HBconsNode_t*) object; /* this is the correct pointer type of the first argument */
+
+    /* verify message length */
+    if (DLC == 1){
+        /* copy data and set 'new message' flag. */
+        HBconsNode->NMTstate = (CO_NMT_internalState_t)data[0];
+        CO_FLAG_SET(HBconsNode->CANrxNew);
+    }
+
+#if (CO_CONFIG_HB_CONS) & CO_CONFIG_FLAG_CALLBACK_PRE
+    /* Optional signal to RTOS, which can resume task, which handles HBcons. */
+    if (HBconsNode->pFunctSignalPre != NULL) {
+        HBconsNode->pFunctSignalPre(HBconsNode->functSignalObjectPre);
+    }
+#endif
+}
+
+
+/*
+ * OD function for accessing _Consumer Heartbeat Time_ (index 0x1016) from SDO server.
+ *
+ * For more information see file CO_SDOserver.h.
+ */
+static CO_SDO_abortCode_t CO_ODF_1016(CO_ODF_arg_t *ODF_arg)
+{
+    CO_HBconsumer_t *HBcons;
+    uint8_t NodeID;
+    uint16_t HBconsTime;
+    uint32_t value;
+    CO_ReturnError_t ret;
+
+    if (ODF_arg->reading){
+        return CO_SDO_AB_NONE;
+    }
+
+    HBcons = (CO_HBconsumer_t*) ODF_arg->object;
+    value = CO_getUint32(ODF_arg->data);
+    NodeID = (value >> 16U) & 0xFFU;
+    HBconsTime = value & 0xFFFFU;
+
+    if ((value & 0xFF800000U) != 0){
+        return CO_SDO_AB_PRAM_INCOMPAT;
+    }
+
+    ret = CO_HBconsumer_initEntry(HBcons, ODF_arg->subIndex-1U, NodeID, HBconsTime);
+    if (ret != CO_ERROR_NO) {
+        return CO_SDO_AB_PRAM_INCOMPAT;
+    }
+    return CO_SDO_AB_NONE;
+}
+
+
+/******************************************************************************/
+CO_ReturnError_t CO_HBconsumer_init(
+        CO_HBconsumer_t        *HBcons,
+        CO_EM_t                *em,
+        CO_SDO_t               *SDO,
+        const uint32_t          HBconsTime[],
+        CO_HBconsNode_t         monitoredNodes[],
+        uint8_t                 numberOfMonitoredNodes,
+        CO_CANmodule_t         *CANdevRx,
+        uint16_t                CANdevRxIdxStart)
+{
+    uint8_t i;
+    CO_ReturnError_t ret = CO_ERROR_NO;
+
+    /* verify arguments */
+    if (HBcons==NULL || em==NULL || SDO==NULL || HBconsTime==NULL ||
+        monitoredNodes==NULL || CANdevRx==NULL){
+        return CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    /* Configure object variables */
+    HBcons->em = em;
+    HBcons->HBconsTime = HBconsTime;
+    HBcons->monitoredNodes = monitoredNodes;
+    HBcons->numberOfMonitoredNodes = numberOfMonitoredNodes;
+    HBcons->allMonitoredActive = false;
+    HBcons->allMonitoredOperational = CO_NMT_UNKNOWN;
+    HBcons->NMTisPreOrOperationalPrev = false;
+    HBcons->CANdevRx = CANdevRx;
+    HBcons->CANdevRxIdxStart = CANdevRxIdxStart;
+#if (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE
+    HBcons->pFunctSignalNmtChanged = NULL;
+#endif
+
+    for(i=0; i<HBcons->numberOfMonitoredNodes; i++) {
+        uint8_t nodeId = (HBcons->HBconsTime[i] >> 16U) & 0xFFU;
+        uint16_t time = HBcons->HBconsTime[i] & 0xFFFFU;
+        ret = CO_HBconsumer_initEntry(HBcons, i, nodeId, time);
+#if (CO_CONFIG_HB_CONS) & CO_CONFIG_FLAG_CALLBACK_PRE
+            HBcons->monitoredNodes[i].pFunctSignalPre = NULL;
+#endif
+#if (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI
+            HBcons->monitoredNodes[i].pFunctSignalNmtChanged = NULL;
+            HBcons->monitoredNodes[i].pFunctSignalHbStarted = NULL;
+            HBcons->monitoredNodes[i].pFunctSignalTimeout = NULL;
+            HBcons->monitoredNodes[i].pFunctSignalRemoteReset = NULL;
+#endif
+    }
+
+    /* Configure Object dictionary entry at index 0x1016 */
+    CO_OD_configure(SDO, OD_H1016_CONSUMER_HB_TIME, CO_ODF_1016, (void*)HBcons, 0, 0);
+
+    return ret;
+}
+
+
+/******************************************************************************/
+CO_ReturnError_t CO_HBconsumer_initEntry(
+        CO_HBconsumer_t        *HBcons,
+        uint8_t                 idx,
+        uint8_t                 nodeId,
+        uint16_t                consumerTime_ms)
+{
+    CO_ReturnError_t ret = CO_ERROR_NO;
+
+    /* verify arguments */
+    if (HBcons == NULL || idx >= HBcons->numberOfMonitoredNodes) {
+        return CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    if ((consumerTime_ms != 0) && (nodeId != 0)){
+        uint8_t i;
+        /* there must not be more entries with same index and time different than zero */
+        for(i = 0U; i<HBcons->numberOfMonitoredNodes; i++){
+            uint32_t objectCopy = HBcons->HBconsTime[i];
+            uint8_t NodeIDObj = (objectCopy >> 16U) & 0xFFU;
+            uint16_t HBconsTimeObj = objectCopy & 0xFFFFU;
+            if ((idx != i) && (HBconsTimeObj != 0) && (nodeId == NodeIDObj)){
+                ret = CO_ERROR_ILLEGAL_ARGUMENT;
+            }
+        }
+    }
+
+    /* Configure one monitored node */
+    if (ret == CO_ERROR_NO) {
+        uint16_t COB_ID;
+
+        CO_HBconsNode_t * monitoredNode = &HBcons->monitoredNodes[idx];
+        monitoredNode->nodeId = nodeId;
+        monitoredNode->time_us = (int32_t)consumerTime_ms * 1000;
+        monitoredNode->NMTstate = CO_NMT_UNKNOWN;
+#if (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE \
+    || (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI
+        monitoredNode->NMTstatePrev = CO_NMT_UNKNOWN;
+#endif
+        CO_FLAG_CLEAR(monitoredNode->CANrxNew);
+
+        /* is channel used */
+        if (monitoredNode->nodeId && monitoredNode->time_us) {
+            COB_ID = monitoredNode->nodeId + CO_CAN_ID_HEARTBEAT;
+            monitoredNode->HBstate = CO_HBconsumer_UNKNOWN;
+        }
+        else {
+            COB_ID = 0;
+            monitoredNode->time_us = 0;
+            monitoredNode->HBstate = CO_HBconsumer_UNCONFIGURED;
+        }
+
+        /* configure Heartbeat consumer CAN reception */
+        if (monitoredNode->HBstate != CO_HBconsumer_UNCONFIGURED) {
+            ret = CO_CANrxBufferInit(HBcons->CANdevRx,
+                                     HBcons->CANdevRxIdxStart + idx,
+                                     COB_ID,
+                                     0x7FF,
+                                     0,
+                                     (void*)&HBcons->monitoredNodes[idx],
+                                     CO_HBcons_receive);
+        }
+    }
+    return ret;
+}
+
+
+#if (CO_CONFIG_HB_CONS) & CO_CONFIG_FLAG_CALLBACK_PRE
+/******************************************************************************/
+void CO_HBconsumer_initCallbackPre(
+        CO_HBconsumer_t        *HBcons,
+        void                   *object,
+        void                  (*pFunctSignal)(void *object))
+{
+    if (HBcons != NULL) {
+        uint8_t i;
+        for(i=0; i<HBcons->numberOfMonitoredNodes; i++) {
+            HBcons->monitoredNodes[i].pFunctSignalPre = pFunctSignal;
+            HBcons->monitoredNodes[i].functSignalObjectPre = object;
+        }
+    }
+}
+#endif
+
+
+#if (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE
+/******************************************************************************/
+void CO_HBconsumer_initCallbackNmtChanged(
+        CO_HBconsumer_t        *HBcons,
+        void                   *object,
+        void                  (*pFunctSignal)(uint8_t nodeId, uint8_t idx,
+                                              CO_NMT_internalState_t NMTstate,
+                                              void *object))
+{
+    if (HBcons==NULL) {
+        return;
+    }
+
+    HBcons->pFunctSignalNmtChanged = pFunctSignal;
+    HBcons->pFunctSignalObjectNmtChanged = object;
+}
+#endif
+
+
+#if (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI
+/******************************************************************************/
+void CO_HBconsumer_initCallbackNmtChanged(
+        CO_HBconsumer_t        *HBcons,
+        uint8_t                 idx,
+        void                   *object,
+        void                  (*pFunctSignal)(uint8_t nodeId, uint8_t idx,
+                                              CO_NMT_internalState_t NMTstate,
+                                              void *object))
+{
+    if (HBcons==NULL || idx>=HBcons->numberOfMonitoredNodes) {
+        return;
+    }
+
+    CO_HBconsNode_t * const monitoredNode = &HBcons->monitoredNodes[idx];
+    monitoredNode->pFunctSignalNmtChanged = pFunctSignal;
+    monitoredNode->pFunctSignalObjectNmtChanged = object;
+}
+
+
+/******************************************************************************/
+void CO_HBconsumer_initCallbackHeartbeatStarted(
+    CO_HBconsumer_t        *HBcons,
+    uint8_t                 idx,
+    void                   *object,
+    void                  (*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object))
+{
+    CO_HBconsNode_t *monitoredNode;
+
+    if (HBcons==NULL || idx>=HBcons->numberOfMonitoredNodes) {
+        return;
+    }
+
+    monitoredNode = &HBcons->monitoredNodes[idx];
+    monitoredNode->pFunctSignalHbStarted = pFunctSignal;
+    monitoredNode->functSignalObjectHbStarted = object;
+}
+
+
+/******************************************************************************/
+void CO_HBconsumer_initCallbackTimeout(
+    CO_HBconsumer_t        *HBcons,
+    uint8_t                 idx,
+    void                   *object,
+    void                  (*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object))
+{
+    CO_HBconsNode_t *monitoredNode;
+
+    if (HBcons==NULL || idx>=HBcons->numberOfMonitoredNodes) {
+        return;
+    }
+
+    monitoredNode = &HBcons->monitoredNodes[idx];
+    monitoredNode->pFunctSignalTimeout = pFunctSignal;
+    monitoredNode->functSignalObjectTimeout = object;
+}
+
+
+/******************************************************************************/
+void CO_HBconsumer_initCallbackRemoteReset(
+    CO_HBconsumer_t        *HBcons,
+    uint8_t                 idx,
+    void                   *object,
+    void                  (*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object))
+{
+    CO_HBconsNode_t *monitoredNode;
+
+    if (HBcons==NULL || idx>=HBcons->numberOfMonitoredNodes) {
+        return;
+    }
+
+    monitoredNode = &HBcons->monitoredNodes[idx];
+    monitoredNode->pFunctSignalRemoteReset = pFunctSignal;
+    monitoredNode->functSignalObjectRemoteReset = object;
+}
+#endif /* (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI */
+
+
+/******************************************************************************/
+void CO_HBconsumer_process(
+        CO_HBconsumer_t        *HBcons,
+        bool_t                  NMTisPreOrOperational,
+        uint32_t                timeDifference_us,
+        uint32_t               *timerNext_us)
+{
+    (void)timerNext_us; /* may be unused */
+
+    bool_t allMonitoredActiveCurrent = true;
+    uint8_t allMonitoredOperationalCurrent = CO_NMT_OPERATIONAL;
+
+    if (NMTisPreOrOperational && HBcons->NMTisPreOrOperationalPrev) {
+        for (uint8_t i=0; i<HBcons->numberOfMonitoredNodes; i++) {
+            uint32_t timeDifference_us_copy = timeDifference_us;
+            CO_HBconsNode_t * const monitoredNode = &HBcons->monitoredNodes[i];
+
+            if (monitoredNode->HBstate == CO_HBconsumer_UNCONFIGURED) {
+                /* continue, if node is not monitored */
+                continue;
+            }
+            /* Verify if received message is heartbeat or bootup */
+            if (CO_FLAG_READ(monitoredNode->CANrxNew)) {
+                if (monitoredNode->NMTstate == CO_NMT_INITIALIZING) {
+                    /* bootup message*/
+#if (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI
+                    if (monitoredNode->pFunctSignalRemoteReset != NULL) {
+                        monitoredNode->pFunctSignalRemoteReset(
+                            monitoredNode->nodeId, i,
+                            monitoredNode->functSignalObjectRemoteReset);
+                    }
+#endif
+                    if (monitoredNode->HBstate == CO_HBconsumer_ACTIVE) {
+                        CO_errorReport(HBcons->em,
+                                       CO_EM_HB_CONSUMER_REMOTE_RESET,
+                                       CO_EMC_HEARTBEAT, i);
+                    }
+                    monitoredNode->HBstate = CO_HBconsumer_UNKNOWN;
+
+                }
+                else {
+                    /* heartbeat message */
+#if (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI
+                    if (monitoredNode->HBstate != CO_HBconsumer_ACTIVE &&
+                        monitoredNode->pFunctSignalHbStarted != NULL) {
+                        monitoredNode->pFunctSignalHbStarted(
+                            monitoredNode->nodeId, i,
+                            monitoredNode->functSignalObjectHbStarted);
+                    }
+#endif
+                    monitoredNode->HBstate = CO_HBconsumer_ACTIVE;
+                    /* reset timer */
+                    monitoredNode->timeoutTimer = 0;
+                    timeDifference_us_copy = 0;
+                }
+                CO_FLAG_CLEAR(monitoredNode->CANrxNew);
+            }
+
+            /* Verify timeout */
+            if (monitoredNode->HBstate == CO_HBconsumer_ACTIVE) {
+                monitoredNode->timeoutTimer += timeDifference_us_copy;
+
+                if (monitoredNode->timeoutTimer >= monitoredNode->time_us) {
+                    /* timeout expired */
+#if (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI
+                    if (monitoredNode->pFunctSignalTimeout!=NULL) {
+                        monitoredNode->pFunctSignalTimeout(
+                            monitoredNode->nodeId, i,
+                            monitoredNode->functSignalObjectTimeout);
+                    }
+#endif
+                    CO_errorReport(HBcons->em, CO_EM_HEARTBEAT_CONSUMER,
+                                   CO_EMC_HEARTBEAT, i);
+                    monitoredNode->NMTstate = CO_NMT_UNKNOWN;
+                    monitoredNode->HBstate = CO_HBconsumer_TIMEOUT;
+                }
+
+#if (CO_CONFIG_HB_CONS) & CO_CONFIG_FLAG_TIMERNEXT
+                else if (timerNext_us != NULL) {
+                    /* Calculate timerNext_us for next timeout checking. */
+                    uint32_t diff = monitoredNode->time_us
+                                  - monitoredNode->timeoutTimer;
+                    if (*timerNext_us > diff) {
+                        *timerNext_us = diff;
+                    }
+                }
+#endif
+            }
+
+            if (monitoredNode->HBstate != CO_HBconsumer_ACTIVE) {
+                allMonitoredActiveCurrent = false;
+            }
+            if (monitoredNode->NMTstate != CO_NMT_OPERATIONAL) {
+                allMonitoredOperationalCurrent = CO_NMT_UNKNOWN;
+            }
+#if (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE \
+    || (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI
+            /* Verify, if NMT state of monitored node changed */
+            if (monitoredNode->NMTstate != monitoredNode->NMTstatePrev) {
+#if (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE
+                if (HBcons->pFunctSignalNmtChanged != NULL) {
+                    HBcons->pFunctSignalNmtChanged(
+                        monitoredNode->nodeId, i, monitoredNode->NMTstate,
+                        HBcons->pFunctSignalObjectNmtChanged);
+#else
+                if (monitoredNode->pFunctSignalNmtChanged != NULL) {
+                    monitoredNode->pFunctSignalNmtChanged(
+                        monitoredNode->nodeId, i, monitoredNode->NMTstate,
+                        monitoredNode->pFunctSignalObjectNmtChanged);
+#endif
+                }
+                monitoredNode->NMTstatePrev = monitoredNode->NMTstate;
+            }
+#endif
+        }
+    }
+    else if (NMTisPreOrOperational || HBcons->NMTisPreOrOperationalPrev) {
+        /* (pre)operational state changed, clear variables */
+        for(uint8_t i=0; i<HBcons->numberOfMonitoredNodes; i++) {
+            CO_HBconsNode_t * const monitoredNode = &HBcons->monitoredNodes[i];
+            monitoredNode->NMTstate = CO_NMT_UNKNOWN;
+#if (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE \
+    || (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI
+            monitoredNode->NMTstatePrev = CO_NMT_UNKNOWN;
+#endif
+            CO_FLAG_CLEAR(monitoredNode->CANrxNew);
+            if (monitoredNode->HBstate != CO_HBconsumer_UNCONFIGURED) {
+                monitoredNode->HBstate = CO_HBconsumer_UNKNOWN;
+            }
+        }
+        allMonitoredActiveCurrent = false;
+        allMonitoredOperationalCurrent = CO_NMT_UNKNOWN;
+    }
+
+    /* Clear emergencies when all monitored nodes becomes active.
+     * We only have one emergency index for all monitored nodes! */
+    if (!HBcons->allMonitoredActive && allMonitoredActiveCurrent) {
+        CO_errorReset(HBcons->em, CO_EM_HEARTBEAT_CONSUMER, 0);
+        CO_errorReset(HBcons->em, CO_EM_HB_CONSUMER_REMOTE_RESET, 0);
+    }
+
+    HBcons->allMonitoredActive = allMonitoredActiveCurrent;
+    HBcons->allMonitoredOperational = allMonitoredOperationalCurrent;
+    HBcons->NMTisPreOrOperationalPrev = NMTisPreOrOperational;
+}
+
+
+#if (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_QUERY_FUNCT
+/******************************************************************************/
+int8_t CO_HBconsumer_getIdxByNodeId(
+        CO_HBconsumer_t        *HBcons,
+        uint8_t                 nodeId)
+{
+    uint8_t i;
+    CO_HBconsNode_t *monitoredNode;
+
+    if (HBcons == NULL) {
+        return -1;
+    }
+
+    /* linear search for the node */
+    monitoredNode = &HBcons->monitoredNodes[0];
+    for(i=0; i<HBcons->numberOfMonitoredNodes; i++){
+        if (monitoredNode->nodeId == nodeId) {
+            return i;
+        }
+        monitoredNode ++;
+    }
+    /* not found */
+    return -1;
+}
+
+
+/******************************************************************************/
+CO_HBconsumer_state_t CO_HBconsumer_getState(
+        CO_HBconsumer_t        *HBcons,
+        uint8_t                 idx)
+{
+    CO_HBconsNode_t *monitoredNode;
+
+    if (HBcons==NULL || idx>=HBcons->numberOfMonitoredNodes) {
+        return CO_HBconsumer_UNCONFIGURED;
+    }
+
+    monitoredNode = &HBcons->monitoredNodes[idx];
+    return monitoredNode->HBstate;
+}
+
+/******************************************************************************/
+int8_t CO_HBconsumer_getNmtState(
+        CO_HBconsumer_t        *HBcons,
+        uint8_t                 idx,
+        CO_NMT_internalState_t *nmtState)
+{
+    CO_HBconsNode_t *monitoredNode;
+
+    if (HBcons==NULL || nmtState==NULL || idx>=HBcons->numberOfMonitoredNodes) {
+        return -1;
+    }
+    *nmtState = CO_NMT_INITIALIZING;
+
+    monitoredNode = &HBcons->monitoredNodes[idx];
+
+    if (monitoredNode->HBstate == CO_HBconsumer_ACTIVE) {
+      *nmtState = monitoredNode->NMTstate;
+      return 0;
+    }
+    return -1;
+}
+#endif /* (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_QUERY_FUNCT */
+
+#endif /* (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_ENABLE */

controller_yy_app/middleware/CANopenNode/301/CO_HBconsumer.h

@@ -0,0 +1,375 @@
+/**
+ * CANopen Heartbeat consumer protocol.
+ *
+ * @file        CO_HBconsumer.h
+ * @ingroup     CO_HBconsumer
+ * @author      Janez Paternoster
+ * @copyright   2004 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CO_HB_CONS_H
+#define CO_HB_CONS_H
+
+#include "301/CO_driver.h"
+
+/* default configuration, see CO_config.h */
+#ifndef CO_CONFIG_HB_CONS
+#define CO_CONFIG_HB_CONS (CO_CONFIG_HB_CONS_ENABLE)
+#endif
+
+#if ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_ENABLE) || defined CO_DOXYGEN
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_HBconsumer Heartbeat consumer
+ * @ingroup CO_CANopen_301
+ * @{
+ *
+ * CANopen Heartbeat consumer protocol.
+ *
+ * Heartbeat consumer monitors Heartbeat messages from remote nodes. If any
+ * monitored node don't send his Heartbeat in specified time, Heartbeat consumer
+ * sends emergency message. If all monitored nodes are operational, then
+ * variable _allMonitoredOperational_ inside CO_HBconsumer_t is set to true.
+ * Monitoring starts after the reception of the first HeartBeat (not bootup).
+ *
+ * Heartbeat set up is done by writing to the OD registers 0x1016 or by using
+ * the function _CO_HBconsumer_initEntry()_
+ *
+ * @see  @ref CO_NMT_Heartbeat
+ */
+
+/**
+ * Heartbeat state of a node
+ */
+typedef enum {
+  CO_HBconsumer_UNCONFIGURED = 0x00U,     /**< Consumer entry inactive */
+  CO_HBconsumer_UNKNOWN      = 0x01U,     /**< Consumer enabled, but no heartbeat received yet */
+  CO_HBconsumer_ACTIVE       = 0x02U,     /**< Heartbeat received within set time */
+  CO_HBconsumer_TIMEOUT      = 0x03U,     /**< No heatbeat received for set time */
+} CO_HBconsumer_state_t;
+
+
+/**
+ * One monitored node inside CO_HBconsumer_t.
+ */
+typedef struct {
+    /** Node Id of the monitored node */
+    uint8_t nodeId;
+    /** Of the remote node (Heartbeat payload) */
+    CO_NMT_internalState_t NMTstate;
+    /** Current heartbeat state */
+    CO_HBconsumer_state_t HBstate;
+    /** Time since last heartbeat received */
+    uint32_t timeoutTimer;
+    /** Consumer heartbeat time from OD */
+    uint32_t time_us;
+    /** Indication if new Heartbeat message received from the CAN bus */
+    volatile void *CANrxNew;
+#if ((CO_CONFIG_HB_CONS) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+    /** From CO_HBconsumer_initCallbackPre() or NULL */
+    void              (*pFunctSignalPre)(void *object);
+    /** From CO_HBconsumer_initCallbackPre() or NULL */
+    void               *functSignalObjectPre;
+#endif
+#if ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE) \
+    || ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI) \
+    || defined CO_DOXYGEN
+    /** Previous value of the remote node (Heartbeat payload) */
+    CO_NMT_internalState_t NMTstatePrev;
+#endif
+#if ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI) || defined CO_DOXYGEN
+    /** Callback for remote NMT changed event.
+     *  From CO_HBconsumer_initCallbackNmtChanged() or NULL. */
+    void (*pFunctSignalNmtChanged)(uint8_t nodeId, uint8_t idx,
+                                   CO_NMT_internalState_t state,
+                                   void *object);
+    /** Pointer to object */
+    void *pFunctSignalObjectNmtChanged;
+    /** Callback for heartbeat state change to active event.
+     *  From CO_HBconsumer_initCallbackHeartbeatStarted() or NULL. */
+    void (*pFunctSignalHbStarted)(uint8_t nodeId, uint8_t idx, void *object);
+    /** Pointer to object */
+    void *functSignalObjectHbStarted;
+    /** Callback for consumer timeout event.
+     *  From CO_HBconsumer_initCallbackTimeout() or NULL. */
+    void (*pFunctSignalTimeout)(uint8_t nodeId, uint8_t idx, void *object);
+    /** Pointer to object */
+    void *functSignalObjectTimeout;
+    /** Callback for remote reset event.
+     *  From CO_HBconsumer_initCallbackRemoteReset() or NULL. */
+    void (*pFunctSignalRemoteReset)(uint8_t nodeId, uint8_t idx, void *object);
+    /** Pointer to object */
+    void *functSignalObjectRemoteReset;
+#endif
+} CO_HBconsNode_t;
+
+
+/**
+ * Heartbeat consumer object.
+ *
+ * Object is initilaized by CO_HBconsumer_init(). It contains an array of
+ * CO_HBconsNode_t objects.
+ */
+typedef struct{
+    CO_EM_t            *em;               /**< From CO_HBconsumer_init() */
+    const uint32_t     *HBconsTime;       /**< From CO_HBconsumer_init() */
+    CO_HBconsNode_t    *monitoredNodes;   /**< From CO_HBconsumer_init() */
+    uint8_t             numberOfMonitoredNodes; /**< From CO_HBconsumer_init() */
+    /** True, if all monitored nodes are active or no node is
+        monitored. Can be read by the application */
+    bool_t              allMonitoredActive;
+    /** True, if all monitored nodes are NMT operational or no node is
+        monitored. Can be read by the application */
+    uint8_t             allMonitoredOperational;
+    bool_t              NMTisPreOrOperationalPrev; /**< previous state of var */
+    CO_CANmodule_t     *CANdevRx;         /**< From CO_HBconsumer_init() */
+    uint16_t            CANdevRxIdxStart; /**< From CO_HBconsumer_init() */
+#if ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE) || defined CO_DOXYGEN
+    /** Callback for remote NMT changed event.
+     *  From CO_HBconsumer_initCallbackNmtChanged() or NULL. */
+    void (*pFunctSignalNmtChanged)(uint8_t nodeId, uint8_t idx,
+                                   CO_NMT_internalState_t state,
+                                   void *object);
+    /** Pointer to object */
+    void *pFunctSignalObjectNmtChanged;
+#endif
+}CO_HBconsumer_t;
+
+
+/**
+ * Initialize Heartbeat consumer object.
+ *
+ * Function must be called in the communication reset section.
+ *
+ * @param HBcons This object will be initialized.
+ * @param em Emergency object.
+ * @param SDO SDO server object.
+ * @param HBconsTime Pointer to _Consumer Heartbeat Time_ array
+ * from Object Dictionary (index 0x1016). Size of array is equal to numberOfMonitoredNodes.
+ * @param monitoredNodes Pointer to the externaly defined array of the same size
+ * as numberOfMonitoredNodes.
+ * @param numberOfMonitoredNodes Total size of the above arrays.
+ * @param CANdevRx CAN device for Heartbeat reception.
+ * @param CANdevRxIdxStart Starting index of receive buffer in the above CAN device.
+ * Number of used indexes is equal to numberOfMonitoredNodes.
+ *
+ * @return #CO_ReturnError_t CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ */
+CO_ReturnError_t CO_HBconsumer_init(
+        CO_HBconsumer_t        *HBcons,
+        CO_EM_t                *em,
+        CO_SDO_t               *SDO,
+        const uint32_t          HBconsTime[],
+        CO_HBconsNode_t         monitoredNodes[],
+        uint8_t                 numberOfMonitoredNodes,
+        CO_CANmodule_t         *CANdevRx,
+        uint16_t                CANdevRxIdxStart);
+
+/**
+ * Initialize one Heartbeat consumer entry
+ *
+ * Calling this function has the same affect as writing to the corresponding
+ * entries in the Object Dictionary (index 0x1016)
+ * @remark The values in the Object Dictionary must be set manually by the
+ * calling function so that heartbeat consumer behaviour matches the OD value.
+ *
+ * @param HBcons This object.
+ * @param idx index of the node in HBcons object
+ * @param nodeId see OD 0x1016 description
+ * @param consumerTime_ms in milliseconds. see OD 0x1016 description
+ * @return
+ */
+CO_ReturnError_t CO_HBconsumer_initEntry(
+        CO_HBconsumer_t        *HBcons,
+        uint8_t                 idx,
+        uint8_t                 nodeId,
+        uint16_t                consumerTime_ms);
+
+#if ((CO_CONFIG_HB_CONS) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+/**
+ * Initialize Heartbeat consumer callback function.
+ *
+ * Function initializes optional callback function, which should immediately
+ * start processing of CO_HBconsumer_process() function.
+ * Callback is called after HBconsumer message is received from the CAN bus.
+ *
+ * @param HBcons This object.
+ * @param object Pointer to object, which will be passed to pFunctSignal(). Can be NULL
+ * @param pFunctSignal Pointer to the callback function. Not called if NULL.
+ */
+void CO_HBconsumer_initCallbackPre(
+        CO_HBconsumer_t        *HBcons,
+        void                   *object,
+        void                  (*pFunctSignal)(void *object));
+#endif
+
+#if ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE) \
+    || ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI) \
+    || defined CO_DOXYGEN
+/**
+ * Initialize Heartbeat consumer NMT changed callback function.
+ *
+ * Function initializes optional callback function, which is called when NMT
+ * state from the remote node changes.
+ *
+ * @param HBcons This object.
+ * @param idx index of the node in HBcons object (only when
+ *            CO_CONFIG_HB_CONS_CALLBACK_MULTI is enabled)
+ * @param object Pointer to object, which will be passed to pFunctSignal().
+ *               Can be NULL.
+ * @param pFunctSignal Pointer to the callback function. Not called if NULL.
+ */
+void CO_HBconsumer_initCallbackNmtChanged(
+        CO_HBconsumer_t        *HBcons,
+#if ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI) || defined CO_DOXYGEN
+        uint8_t                 idx,
+#endif
+        void                   *object,
+        void                  (*pFunctSignal)(uint8_t nodeId, uint8_t idx,
+                                              CO_NMT_internalState_t state,
+                                              void *object));
+#endif
+
+#if ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI) || defined CO_DOXYGEN
+/**
+ * Initialize Heartbeat consumer started callback function.
+ *
+ * Function initializes optional callback function, which is called for the
+ * first received heartbeat after activating hb consumer or timeout.
+ * Function may wake up external task, which handles this event.
+ *
+ * @param HBcons This object.
+ * @param idx index of the node in HBcons object
+ * @param object Pointer to object, which will be passed to pFunctSignal(). Can be NULL
+ * @param pFunctSignal Pointer to the callback function. Not called if NULL.
+ */
+void CO_HBconsumer_initCallbackHeartbeatStarted(
+        CO_HBconsumer_t        *HBcons,
+        uint8_t                 idx,
+        void                   *object,
+        void                  (*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object));
+
+/**
+ * Initialize Heartbeat consumer timeout callback function.
+ *
+ * Function initializes optional callback function, which is called when the node
+ * state changes from active to timeout. Function may wake up external task,
+ * which handles this event.
+ *
+ * @param HBcons This object.
+ * @param idx index of the node in HBcons object
+ * @param object Pointer to object, which will be passed to pFunctSignal(). Can be NULL
+ * @param pFunctSignal Pointer to the callback function. Not called if NULL.
+ */
+void CO_HBconsumer_initCallbackTimeout(
+        CO_HBconsumer_t        *HBcons,
+        uint8_t                 idx,
+        void                   *object,
+        void                  (*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object));
+
+/**
+ * Initialize Heartbeat consumer remote reset detected callback function.
+ *
+ * Function initializes optional callback function, which is called when a bootup
+ * message is received from the remote node. Function may wake up external task,
+ * which handles this event.
+ *
+ * @param HBcons This object.
+ * @param idx index of the node in HBcons object
+ * @param object Pointer to object, which will be passed to pFunctSignal(). Can be NULL
+ * @param pFunctSignal Pointer to the callback function. Not called if NULL.
+ */
+void CO_HBconsumer_initCallbackRemoteReset(
+        CO_HBconsumer_t        *HBcons,
+        uint8_t                 idx,
+        void                   *object,
+        void                  (*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object));
+#endif /* (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI */
+
+/**
+ * Process Heartbeat consumer object.
+ *
+ * Function must be called cyclically.
+ *
+ * @param HBcons This object.
+ * @param NMTisPreOrOperational True if this node is NMT_PRE_OPERATIONAL or NMT_OPERATIONAL.
+ * @param timeDifference_us Time difference from previous function call in [microseconds].
+ * @param [out] timerNext_us info to OS - see CO_process().
+ */
+void CO_HBconsumer_process(
+        CO_HBconsumer_t        *HBcons,
+        bool_t                  NMTisPreOrOperational,
+        uint32_t                timeDifference_us,
+        uint32_t               *timerNext_us);
+
+
+#if ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_QUERY_FUNCT) || defined CO_DOXYGEN
+/**
+ * Get the heartbeat producer object index by node ID
+ *
+ * @param HBcons This object.
+ * @param nodeId producer node ID
+ * @return index. -1 if not found
+ */
+int8_t CO_HBconsumer_getIdxByNodeId(
+        CO_HBconsumer_t        *HBcons,
+        uint8_t                 nodeId);
+
+/**
+ * Get the current state of a heartbeat producer by the index in OD 0x1016
+ *
+ * @param HBcons This object.
+ * @param idx object sub index
+ * @return #CO_HBconsumer_state_t
+ */
+CO_HBconsumer_state_t CO_HBconsumer_getState(
+        CO_HBconsumer_t        *HBcons,
+        uint8_t                 idx);
+
+/**
+ * Get the current NMT state of a heartbeat producer by the index in OD 0x1016
+ *
+ * NMT state is only available when heartbeat is enabled for this index!
+ *
+ * @param HBcons This object.
+ * @param idx object sub index
+ * @param [out] nmtState of this index
+ * @retval 0 NMT state has been received and is valid
+ * @retval -1 not valid
+ */
+int8_t CO_HBconsumer_getNmtState(
+        CO_HBconsumer_t        *HBcons,
+        uint8_t                 idx,
+        CO_NMT_internalState_t *nmtState);
+
+#endif /* (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_QUERY_FUNCT */
+
+/** @} */ /* CO_HBconsumer */
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+#endif /* (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_ENABLE */
+
+#endif /* CO_HB_CONS_H */

controller_yy_app/middleware/CANopenNode/301/CO_NMT_Heartbeat.c

@@ -0,0 +1,380 @@
+/*
+ * CANopen NMT and Heartbeat producer object.
+ *
+ * @file        CO_NMT_Heartbeat.c
+ * @ingroup     CO_NMT_Heartbeat
+ * @author      Janez Paternoster
+ * @copyright   2004 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "301/CO_NMT_Heartbeat.h"
+
+
+/*
+ * Read received message from CAN module.
+ *
+ * Function will be called (by CAN receive interrupt) every time, when CAN
+ * message with correct identifier will be received. For more information and
+ * description of parameters see file CO_driver.h.
+ */
+static void CO_NMT_receive(void *object, void *msg){
+    CO_NMT_t *NMT;
+    uint8_t nodeId;
+    uint8_t DLC = CO_CANrxMsg_readDLC(msg);
+    uint8_t *data = CO_CANrxMsg_readData(msg);
+
+    NMT = (CO_NMT_t*)object;   /* this is the correct pointer type of the first argument */
+
+    nodeId = data[1];
+
+    if ((DLC == 2) && ((nodeId == 0) || (nodeId == NMT->nodeId))){
+        uint8_t command = data[0];
+#if (CO_CONFIG_NMT) & (CO_CONFIG_NMT_CALLBACK_CHANGE | CO_CONFIG_FLAG_CALLBACK_PRE)
+        CO_NMT_internalState_t currentOperatingState = NMT->operatingState;
+#endif
+
+        switch(command){
+            case CO_NMT_ENTER_OPERATIONAL:
+                if ((*NMT->emPr->errorRegister) == 0U){
+                    NMT->operatingState = CO_NMT_OPERATIONAL;
+                }
+                break;
+            case CO_NMT_ENTER_STOPPED:
+                NMT->operatingState = CO_NMT_STOPPED;
+                break;
+            case CO_NMT_ENTER_PRE_OPERATIONAL:
+                NMT->operatingState = CO_NMT_PRE_OPERATIONAL;
+                break;
+            case CO_NMT_RESET_NODE:
+                NMT->resetCommand = CO_RESET_APP;
+                break;
+            case CO_NMT_RESET_COMMUNICATION:
+                NMT->resetCommand = CO_RESET_COMM;
+                break;
+            default:
+                break;
+        }
+
+#if (CO_CONFIG_NMT) & CO_CONFIG_NMT_CALLBACK_CHANGE
+        if (NMT->pFunctNMT!=NULL && currentOperatingState!=NMT->operatingState){
+            NMT->pFunctNMT(NMT->operatingState);
+        }
+#endif
+#if (CO_CONFIG_NMT) & CO_CONFIG_FLAG_CALLBACK_PRE
+    /* Optional signal to RTOS, which can resume task, which handles NMT. */
+    if (NMT->pFunctSignalPre != NULL && currentOperatingState!=NMT->operatingState) {
+        NMT->pFunctSignalPre(NMT->functSignalObjectPre);
+    }
+#endif
+    }
+}
+
+
+/******************************************************************************/
+CO_ReturnError_t CO_NMT_init(
+        CO_NMT_t               *NMT,
+        CO_EMpr_t              *emPr,
+        uint8_t                 nodeId,
+        uint16_t                firstHBTime_ms,
+        CO_CANmodule_t         *NMT_CANdevRx,
+        uint16_t                NMT_rxIdx,
+        uint16_t                CANidRxNMT,
+        CO_CANmodule_t         *NMT_CANdevTx,
+        uint16_t                NMT_txIdx,
+        uint16_t                CANidTxNMT,
+        CO_CANmodule_t         *HB_CANdevTx,
+        uint16_t                HB_txIdx,
+        uint16_t                CANidTxHB)
+{
+    CO_ReturnError_t ret = CO_ERROR_NO;
+
+    /* verify arguments */
+    if (NMT == NULL || emPr == NULL || NMT_CANdevRx == NULL || HB_CANdevTx == NULL
+#if (CO_CONFIG_NMT) & CO_CONFIG_NMT_MASTER
+        || NMT_CANdevTx == NULL
+#endif
+    ) {
+        return CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    /* clear the object */
+    memset(NMT, 0, sizeof(CO_NMT_t));
+
+    /* Configure object variables */
+    NMT->operatingState         = CO_NMT_INITIALIZING;
+    NMT->operatingStatePrev     = CO_NMT_INITIALIZING;
+    NMT->nodeId                 = nodeId;
+    NMT->firstHBTime            = (int32_t)firstHBTime_ms * 1000;
+    NMT->emPr                   = emPr;
+
+    /* configure NMT CAN reception */
+    ret = CO_CANrxBufferInit(
+            NMT_CANdevRx,       /* CAN device */
+            NMT_rxIdx,          /* rx buffer index */
+            CANidRxNMT,         /* CAN identifier */
+            0x7FF,              /* mask */
+            0,                  /* rtr */
+            (void*)NMT,         /* object passed to receive function */
+            CO_NMT_receive);    /* this function will process received message */
+
+#if (CO_CONFIG_NMT) & CO_CONFIG_NMT_MASTER
+    /* configure NMT CAN transmission */
+    NMT->NMT_CANdevTx = NMT_CANdevTx;
+    NMT->NMT_TXbuff = CO_CANtxBufferInit(
+            NMT_CANdevTx,       /* CAN device */
+            NMT_txIdx,          /* index of specific buffer inside CAN module */
+            CANidTxNMT,         /* CAN identifier */
+            0,                  /* rtr */
+            2,                  /* number of data bytes */
+            0);                 /* synchronous message flag bit */
+    if (NMT->NMT_TXbuff == NULL) {
+        ret = CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+#endif
+
+    /* configure HB CAN transmission */
+    NMT->HB_CANdevTx = HB_CANdevTx;
+    NMT->HB_TXbuff = CO_CANtxBufferInit(
+            HB_CANdevTx,        /* CAN device */
+            HB_txIdx,           /* index of specific buffer inside CAN module */
+            CANidTxHB,          /* CAN identifier */
+            0,                  /* rtr */
+            1,                  /* number of data bytes */
+            0);                 /* synchronous message flag bit */
+
+    if (NMT->HB_TXbuff == NULL) {
+        ret = CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    return ret;
+}
+
+
+#if (CO_CONFIG_NMT) & CO_CONFIG_FLAG_CALLBACK_PRE
+void CO_NMT_initCallbackPre(
+        CO_NMT_t               *NMT,
+        void                   *object,
+        void                  (*pFunctSignal)(void *object))
+{
+    if (NMT != NULL) {
+        NMT->pFunctSignalPre = pFunctSignal;
+        NMT->functSignalObjectPre = object;
+    }
+}
+#endif
+
+
+#if (CO_CONFIG_NMT) & CO_CONFIG_NMT_CALLBACK_CHANGE
+/******************************************************************************/
+void CO_NMT_initCallbackChanged(
+        CO_NMT_t               *NMT,
+        void                  (*pFunctNMT)(CO_NMT_internalState_t state))
+{
+    if (NMT != NULL){
+        NMT->pFunctNMT = pFunctNMT;
+        if (NMT->pFunctNMT != NULL){
+            NMT->pFunctNMT(NMT->operatingState);
+        }
+    }
+}
+#endif
+
+
+/******************************************************************************/
+CO_NMT_reset_cmd_t CO_NMT_process(
+        CO_NMT_t               *NMT,
+        uint32_t                timeDifference_us,
+        uint16_t                HBtime_ms,
+        uint32_t                NMTstartup,
+        uint8_t                 errorRegister,
+        const uint8_t           errorBehavior[],
+        uint32_t               *timerNext_us)
+{
+    (void)timerNext_us; /* may be unused */
+
+    uint8_t CANpassive;
+    CO_NMT_internalState_t currentOperatingState = NMT->operatingState;
+    uint32_t HBtime = (uint32_t)HBtime_ms * 1000;
+
+    NMT->HBproducerTimer += timeDifference_us;
+
+    /* Send heartbeat producer message if:
+     * - First start, send bootup message or
+     * - HB producer and Timer expired or
+     * - HB producer and NMT->operatingState changed, but not from initialised */
+    if ((NMT->operatingState == CO_NMT_INITIALIZING) ||
+        (HBtime != 0 && (NMT->HBproducerTimer >= HBtime ||
+                         NMT->operatingState != NMT->operatingStatePrev)
+    )) {
+        /* Start from the beginning. If OS is slow, time sliding may occur. However,
+         * heartbeat is not for synchronization, it is for health report. */
+        NMT->HBproducerTimer = 0;
+
+        NMT->HB_TXbuff->data[0] = (uint8_t) NMT->operatingState;
+        CO_CANsend(NMT->HB_CANdevTx, NMT->HB_TXbuff);
+
+        if (NMT->operatingState == CO_NMT_INITIALIZING) {
+            /* After bootup messages send first heartbeat earlier */
+            if (HBtime > NMT->firstHBTime) {
+                NMT->HBproducerTimer = HBtime - NMT->firstHBTime;
+            }
+
+            /* NMT slave self starting */
+            if (NMTstartup == 0x00000008U) NMT->operatingState = CO_NMT_OPERATIONAL;
+            else                           NMT->operatingState = CO_NMT_PRE_OPERATIONAL;
+        }
+    }
+    NMT->operatingStatePrev = NMT->operatingState;
+
+    /* CAN passive flag */
+    CANpassive = 0;
+    if (CO_isError(NMT->emPr->em, CO_EM_CAN_TX_BUS_PASSIVE) || CO_isError(NMT->emPr->em, CO_EM_CAN_RX_BUS_PASSIVE))
+        CANpassive = 1;
+
+    /* in case of error enter pre-operational state */
+    if (errorBehavior && (NMT->operatingState == CO_NMT_OPERATIONAL)){
+        if (CANpassive && (errorBehavior[2] == 0 || errorBehavior[2] == 2)) errorRegister |= 0x10;
+
+        if (errorRegister){
+            /* Communication error */
+            if (errorRegister & CO_ERR_REG_COMM_ERR){
+                if (errorBehavior[1] == 0){
+                    NMT->operatingState = CO_NMT_PRE_OPERATIONAL;
+                }
+                else if (errorBehavior[1] == 2){
+                    NMT->operatingState = CO_NMT_STOPPED;
+                }
+                else if (CO_isError(NMT->emPr->em, CO_EM_CAN_TX_BUS_OFF)
+                     || CO_isError(NMT->emPr->em, CO_EM_HEARTBEAT_CONSUMER)
+                     || CO_isError(NMT->emPr->em, CO_EM_HB_CONSUMER_REMOTE_RESET))
+                {
+                    if (errorBehavior[0] == 0){
+                        NMT->operatingState = CO_NMT_PRE_OPERATIONAL;
+                    }
+                    else if (errorBehavior[0] == 2){
+                        NMT->operatingState = CO_NMT_STOPPED;
+                    }
+                }
+            }
+
+            /* Generic error */
+            if (errorRegister & CO_ERR_REG_GENERIC_ERR){
+                if      (errorBehavior[3] == 0) NMT->operatingState = CO_NMT_PRE_OPERATIONAL;
+                else if (errorBehavior[3] == 2) NMT->operatingState = CO_NMT_STOPPED;
+            }
+
+            /* Device profile error */
+            if (errorRegister & CO_ERR_REG_DEV_PROFILE){
+                if      (errorBehavior[4] == 0) NMT->operatingState = CO_NMT_PRE_OPERATIONAL;
+                else if (errorBehavior[4] == 2) NMT->operatingState = CO_NMT_STOPPED;
+            }
+
+            /* Manufacturer specific error */
+            if (errorRegister & CO_ERR_REG_MANUFACTURER){
+                if      (errorBehavior[5] == 0) NMT->operatingState = CO_NMT_PRE_OPERATIONAL;
+                else if (errorBehavior[5] == 2) NMT->operatingState = CO_NMT_STOPPED;
+            }
+
+            /* if operational state is lost, send HB immediately. */
+            if (NMT->operatingState != CO_NMT_OPERATIONAL)
+                NMT->HBproducerTimer = HBtime;
+        }
+    }
+
+    if (currentOperatingState != NMT->operatingState) {
+#if (CO_CONFIG_NMT) & CO_CONFIG_NMT_CALLBACK_CHANGE
+        if (NMT->pFunctNMT != NULL) {
+            NMT->pFunctNMT(NMT->operatingState);
+        }
+#endif
+#if (CO_CONFIG_NMT) & CO_CONFIG_FLAG_TIMERNEXT
+        /* execute next CANopen processing immediately */
+        if (timerNext_us != NULL) {
+            *timerNext_us = 0;
+        }
+#endif
+    }
+
+#if (CO_CONFIG_NMT) & CO_CONFIG_FLAG_TIMERNEXT
+    /* Calculate, when next Heartbeat needs to be send and lower timerNext_us if necessary. */
+    if (HBtime != 0 && timerNext_us != NULL) {
+        if (NMT->HBproducerTimer < HBtime) {
+            uint32_t diff = HBtime - NMT->HBproducerTimer;
+            if (*timerNext_us > diff) {
+                *timerNext_us = diff;
+            }
+        } else {
+            *timerNext_us = 0;
+        }
+    }
+#endif
+
+    return (CO_NMT_reset_cmd_t) NMT->resetCommand;
+}
+
+
+#if (CO_CONFIG_NMT) & CO_CONFIG_NMT_MASTER
+/******************************************************************************/
+CO_ReturnError_t CO_NMT_sendCommand(CO_NMT_t *NMT,
+                                    CO_NMT_command_t command,
+                                    uint8_t nodeID)
+{
+    CO_ReturnError_t error = CO_ERROR_NO;
+
+    /* verify arguments */
+    if (NMT == NULL) {
+        error = CO_ERROR_TX_UNCONFIGURED;
+    }
+
+    /* Apply NMT command also to this node, if set so. */
+    if (error == CO_ERROR_NO && (nodeID == 0 || nodeID == NMT->nodeId)) {
+        switch (command) {
+        case CO_NMT_ENTER_OPERATIONAL:
+            if ((*NMT->emPr->errorRegister) == 0) {
+                NMT->operatingState = CO_NMT_OPERATIONAL;
+            }
+            break;
+        case CO_NMT_ENTER_STOPPED:
+            NMT->operatingState = CO_NMT_STOPPED;
+            break;
+        case CO_NMT_ENTER_PRE_OPERATIONAL:
+            NMT->operatingState = CO_NMT_PRE_OPERATIONAL;
+            break;
+        case CO_NMT_RESET_NODE:
+            NMT->resetCommand = CO_RESET_APP;
+            break;
+        case CO_NMT_RESET_COMMUNICATION:
+            NMT->resetCommand = CO_RESET_COMM;
+            break;
+        default:
+            error = CO_ERROR_ILLEGAL_ARGUMENT;
+            break;
+        }
+    }
+
+    /* Send NMT master message. */
+    if (error == CO_ERROR_NO) {
+        NMT->NMT_TXbuff->data[0] = command;
+        NMT->NMT_TXbuff->data[1] = nodeID;
+        error = CO_CANsend(NMT->NMT_CANdevTx, NMT->NMT_TXbuff);
+    }
+
+    return error;
+}
+#endif

controller_yy_app/middleware/CANopenNode/301/CO_NMT_Heartbeat.h

@@ -0,0 +1,302 @@
+/**
+ * CANopen Network management and Heartbeat producer protocol.
+ *
+ * @file        CO_NMT_Heartbeat.h
+ * @ingroup     CO_NMT_Heartbeat
+ * @author      Janez Paternoster
+ * @copyright   2004 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CO_NMT_HEARTBEAT_H
+#define CO_NMT_HEARTBEAT_H
+
+#include "301/CO_driver.h"
+#include "301/CO_SDOserver.h"
+#include "301/CO_Emergency.h"
+
+/* default configuration, see CO_config.h */
+#ifndef CO_CONFIG_NMT
+#define CO_CONFIG_NMT (0)
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_NMT_Heartbeat NMT and Heartbeat
+ * @ingroup CO_CANopen_301
+ * @{
+ *
+ * CANopen Network management and Heartbeat producer protocol.
+ *
+ * CANopen device can be in one of the #CO_NMT_internalState_t
+ *  - Initializing. It is active before CANopen is initialized.
+ *  - Pre-operational. All CANopen objects are active, except PDOs.
+ *  - Operational. Process data objects (PDOs) are active too.
+ *  - Stopped. Only Heartbeat producer and NMT consumer are active.
+ *
+ * NMT master can change the internal state of the devices by sending
+ * #CO_NMT_command_t.
+ *
+ * ###NMT message contents:
+ *
+ *   Byte | Description
+ *   -----|-----------------------------------------------------------
+ *     0  | #CO_NMT_command_t
+ *     1  | Node ID. If zero, command addresses all nodes.
+ *
+ * ###Heartbeat message contents:
+ *
+ *   Byte | Description
+ *   -----|-----------------------------------------------------------
+ *     0  | #CO_NMT_internalState_t
+ *
+ * @see #CO_Default_CAN_ID_t
+ */
+
+
+/**
+ * Internal network state of the CANopen node
+ */
+typedef enum{
+    CO_NMT_UNKNOWN                  = -1,   /**< Device state is unknown (for heartbeat consumer) */
+    CO_NMT_INITIALIZING             = 0,    /**< Device is initializing */
+    CO_NMT_PRE_OPERATIONAL          = 127,  /**< Device is in pre-operational state */
+    CO_NMT_OPERATIONAL              = 5,    /**< Device is in operational state */
+    CO_NMT_STOPPED                  = 4     /**< Device is stopped */
+}CO_NMT_internalState_t;
+
+
+/**
+ * Commands from NMT master.
+ */
+typedef enum{
+    CO_NMT_ENTER_OPERATIONAL        = 1,    /**< Start device */
+    CO_NMT_ENTER_STOPPED            = 2,    /**< Stop device */
+    CO_NMT_ENTER_PRE_OPERATIONAL    = 128,  /**< Put device into pre-operational */
+    CO_NMT_RESET_NODE               = 129,  /**< Reset device */
+    CO_NMT_RESET_COMMUNICATION      = 130   /**< Reset CANopen communication on device */
+}CO_NMT_command_t;
+
+
+/**
+ * Return code for CO_NMT_process() that tells application code what to
+ * reset.
+ */
+typedef enum{
+    CO_RESET_NOT  = 0,/**< Normal return, no action */
+    CO_RESET_COMM = 1,/**< Application must provide communication reset. */
+    CO_RESET_APP  = 2,/**< Application must provide complete device reset */
+    CO_RESET_QUIT = 3 /**< Application must quit, no reset of microcontroller (command is not requested by the stack.) */
+}CO_NMT_reset_cmd_t;
+
+
+/**
+ * NMT consumer and Heartbeat producer object, initialized by CO_NMT_init()
+ */
+typedef struct{
+    CO_NMT_internalState_t operatingState; /**< Current NMT operating state. */
+    CO_NMT_internalState_t operatingStatePrev; /**< Previous NMT operating state. */
+    uint8_t             resetCommand;   /**< If different than zero, device will reset */
+    uint8_t             nodeId;         /**< CANopen Node ID of this device */
+    uint32_t            HBproducerTimer;/**< Internal timer for HB producer */
+    uint32_t            firstHBTime;    /**< From CO_NMT_init() */
+    CO_EMpr_t          *emPr;           /**< From CO_NMT_init() */
+#if ((CO_CONFIG_NMT) & CO_CONFIG_NMT_MASTER) || defined CO_DOXYGEN
+    CO_CANmodule_t     *NMT_CANdevTx;   /**< From CO_NMT_init() */
+    CO_CANtx_t         *NMT_TXbuff;     /**< CAN transmit buffer for NMT master message */
+#endif
+    CO_CANmodule_t     *HB_CANdevTx;    /**< From CO_NMT_init() */
+    CO_CANtx_t         *HB_TXbuff;      /**< CAN transmit buffer for heartbeat message */
+#if ((CO_CONFIG_NMT) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+    /** From CO_NMT_initCallbackPre() or NULL */
+    void              (*pFunctSignalPre)(void *object);
+    /** From CO_NMT_initCallbackPre() or NULL */
+    void               *functSignalObjectPre;
+#endif
+#if ((CO_CONFIG_NMT) & CO_CONFIG_NMT_CALLBACK_CHANGE) || defined CO_DOXYGEN
+    void              (*pFunctNMT)(CO_NMT_internalState_t state); /**< From CO_NMT_initCallbackChanged() or NULL */
+#endif
+}CO_NMT_t;
+
+
+/**
+ * Initialize NMT and Heartbeat producer object.
+ *
+ * Function must be called in the communication reset section.
+ *
+ * @param NMT This object will be initialized.
+ * @param emPr Emergency main object.
+ * @param nodeId CANopen Node ID of this device.
+ * @param firstHBTime_ms Time between bootup and first heartbeat message in milliseconds.
+ * If firstHBTime is greater than _Producer Heartbeat time_
+ * (object dictionary, index 0x1017), latter is used instead.
+ * @param NMT_CANdevRx CAN device for NMT reception.
+ * @param NMT_rxIdx Index of receive buffer in above CAN device.
+ * @param CANidRxNMT CAN identifier for NMT receive message.
+ * @param NMT_CANdevTx CAN device for NMT master transmission.
+ * @param NMT_txIdx Index of transmit buffer in above CAN device.
+ * @param CANidTxNMT CAN identifier for NMT transmit message.
+ * @param HB_CANdevTx CAN device for HB transmission.
+ * @param HB_txIdx Index of transmit buffer in the above CAN device.
+ * @param CANidTxHB CAN identifier for HB message.
+ *
+ * @return #CO_ReturnError_t CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ */
+CO_ReturnError_t CO_NMT_init(
+        CO_NMT_t               *NMT,
+        CO_EMpr_t              *emPr,
+        uint8_t                 nodeId,
+        uint16_t                firstHBTime_ms,
+        CO_CANmodule_t         *NMT_CANdevRx,
+        uint16_t                NMT_rxIdx,
+        uint16_t                CANidRxNMT,
+        CO_CANmodule_t         *NMT_CANdevTx,
+        uint16_t                NMT_txIdx,
+        uint16_t                CANidTxNMT,
+        CO_CANmodule_t         *HB_CANdevTx,
+        uint16_t                HB_txIdx,
+        uint16_t                CANidTxHB);
+
+
+#if ((CO_CONFIG_NMT) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+/**
+ * Initialize NMT callback function after message preprocessed.
+ *
+ * Function initializes optional callback function, which should immediately
+ * start processing of CO_NMT_process() function.
+ * Callback is called after NMT message is received from the CAN bus.
+ *
+ * @param NMT This object.
+ * @param object Pointer to object, which will be passed to pFunctSignal(). Can be NULL
+ * @param pFunctSignal Pointer to the callback function. Not called if NULL.
+ */
+void CO_NMT_initCallbackPre(
+        CO_NMT_t               *NMT,
+        void                   *object,
+        void                  (*pFunctSignal)(void *object));
+#endif
+
+
+#if ((CO_CONFIG_NMT) & CO_CONFIG_NMT_CALLBACK_CHANGE) || defined CO_DOXYGEN
+/**
+ * Initialize NMT callback function.
+ *
+ * Function initializes optional callback function, which is called after
+ * NMT State change has occured. Function may wake up external task which
+ * handles NMT events.
+ * The first call is made immediately to give the consumer the current NMT state.
+ *
+ * @remark Be aware that the callback function is run inside the CAN receive
+ * function context. Depending on the driver, this might be inside an interrupt!
+ *
+ * @param NMT This object.
+ * @param pFunctNMT Pointer to the callback function. Not called if NULL.
+ */
+void CO_NMT_initCallbackChanged(
+        CO_NMT_t               *NMT,
+        void                  (*pFunctNMT)(CO_NMT_internalState_t state));
+#endif
+
+
+/**
+ * Process received NMT and produce Heartbeat messages.
+ *
+ * Function must be called cyclically.
+ *
+ * @param NMT This object.
+ * @param timeDifference_us Time difference from previous function call in [microseconds].
+ * @param HBtime_ms _Producer Heartbeat time_ (object dictionary, index 0x1017).
+ * @param NMTstartup _NMT startup behavior_ (object dictionary, index 0x1F80).
+ * @param errorRegister _Error register_ (object dictionary, index 0x1001).
+ * @param errorBehavior pointer to _Error behavior_ array (object dictionary, index 0x1029).
+ *        Object controls, if device should leave NMT operational state.
+ *        Length of array must be 6. If pointer is NULL, no calculation is made.
+ * @param [out] timerNext_us info to OS - see CO_process().
+ *
+ * @return #CO_NMT_reset_cmd_t
+ */
+CO_NMT_reset_cmd_t CO_NMT_process(
+        CO_NMT_t               *NMT,
+        uint32_t                timeDifference_us,
+        uint16_t                HBtime_ms,
+        uint32_t                NMTstartup,
+        uint8_t                 errorRegister,
+        const uint8_t           errorBehavior[],
+        uint32_t               *timerNext_us);
+
+
+/**
+ * Query current NMT state
+ *
+ * @param NMT This object.
+ *
+ * @return @ref CO_NMT_internalState_t
+ */
+static inline CO_NMT_internalState_t CO_NMT_getInternalState(CO_NMT_t *NMT) {
+    return (NMT == NULL) ? CO_NMT_INITIALIZING : NMT->operatingState;
+}
+
+
+/**
+ * Set internal NMT state
+ *
+ * Functions sets state directly, without any checking. @ref CO_NMT_process()
+ * may also switch between states automatically, see @ref CO_NMT_control_t.
+ *
+ * @param NMT This object.
+ * @param state New state.
+ */
+static inline void CO_NMT_setInternalState(CO_NMT_t *NMT,
+                                           CO_NMT_internalState_t state)
+{
+    if (NMT != NULL) NMT->operatingState = state;
+}
+
+
+#if ((CO_CONFIG_NMT) & CO_CONFIG_NMT_MASTER) || defined CO_DOXYGEN
+/**
+ * Send NMT master command.
+ *
+ * This functionality can only be used from NMT master. There is one exception
+ * where application from slave node may send NMT master command: If CANopen
+ * object 0x1F80 has value of **0x2**, then NMT slave shall execute the NMT
+ * service start remote node (CO_NMT_ENTER_OPERATIONAL) with nodeID set to 0.
+ *
+ * @param NMT This object.
+ * @param command NMT command from CO_NMT_command_t.
+ * @param nodeID Node ID of the remote node. 0 for all nodes including self.
+ *
+ * @return 0: Operation completed successfully.
+ * @return other: same as CO_CANsend().
+ */
+CO_ReturnError_t CO_NMT_sendCommand(CO_NMT_t *NMT,
+                                    CO_NMT_command_t command,
+                                    uint8_t nodeID);
+
+#endif /* (CO_CONFIG_NMT) & CO_CONFIG_NMT_MASTER */
+
+/** @} */ /* CO_NMT_Heartbeat */
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+#endif /* CO_NMT_HEARTBEAT_H */

controller_yy_app/middleware/CANopenNode/301/CO_PDO.c

@@ -0,0 +1,1095 @@
+/*
+ * CANopen Process Data Object.
+ *
+ * @file        CO_PDO.c
+ * @ingroup     CO_PDO
+ * @author      Janez Paternoster
+ * @copyright   2004 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+
+#include <string.h>
+
+#include "301/CO_PDO.h"
+
+/*
+ * Read received message from CAN module.
+ *
+ * Function will be called (by CAN receive interrupt) every time, when CAN
+ * message with correct identifier will be received. For more information and
+ * description of parameters see file CO_driver.h.
+ * If new message arrives and previous message wasn't processed yet, then
+ * previous message will be lost and overwritten by new message. That's OK with PDOs.
+ */
+static void CO_PDO_receive(void *object, void *msg){
+    CO_RPDO_t *RPDO;
+    uint8_t DLC = CO_CANrxMsg_readDLC(msg);
+    uint8_t *data = CO_CANrxMsg_readData(msg);
+
+    RPDO = (CO_RPDO_t*)object;   /* this is the correct pointer type of the first argument */
+
+    if ( (RPDO->valid) &&
+        (*RPDO->operatingState == CO_NMT_OPERATIONAL) &&
+        (DLC >= RPDO->dataLength))
+    {
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+        const size_t index = RPDO->SYNC && RPDO->synchronous && RPDO->SYNC->CANrxToggle;
+#else
+        const size_t index = 0;
+#endif
+
+        /* copy data into appropriate buffer and set 'new message' flag */
+        memcpy(RPDO->CANrxData[index], data, sizeof(RPDO->CANrxData[index]));
+        CO_FLAG_SET(RPDO->CANrxNew[index]);
+
+#if (CO_CONFIG_PDO) & CO_CONFIG_FLAG_CALLBACK_PRE
+        /* Optional signal to RTOS, which can resume task, which handles RPDO. */
+        if (RPDO->pFunctSignalPre != NULL) {
+            RPDO->pFunctSignalPre(RPDO->functSignalObjectPre);
+        }
+#endif
+    }
+}
+
+
+/*
+ * Configure RPDO Communication parameter.
+ *
+ * Function is called from commuincation reset or when parameter changes.
+ *
+ * Function configures following variable from CO_RPDO_t: _valid_. It also
+ * configures CAN rx buffer. If configuration fails, emergency message is send
+ * and device is not able to enter NMT operational.
+ *
+ * @param RPDO RPDO object.
+ * @param COB_IDUsedByRPDO _RPDO communication parameter_, _COB-ID for PDO_ variable
+ * from Object dictionary (index 0x1400+, subindex 1).
+ */
+static void CO_RPDOconfigCom(CO_RPDO_t* RPDO, uint32_t COB_IDUsedByRPDO){
+    uint16_t ID;
+    CO_ReturnError_t r;
+
+    ID = (uint16_t)COB_IDUsedByRPDO;
+
+    /* is RPDO used? */
+    if ((COB_IDUsedByRPDO & 0xBFFFF800L) == 0 && RPDO->dataLength && ID){
+        /* is used default COB-ID? */
+        if (ID == RPDO->defaultCOB_ID) ID += RPDO->nodeId;
+        RPDO->valid = true;
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+        RPDO->synchronous = (RPDO->RPDOCommPar->transmissionType <= 240) ? true : false;
+#endif
+    }
+    else{
+        ID = 0;
+        RPDO->valid = false;
+        CO_FLAG_CLEAR(RPDO->CANrxNew[0]);
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+        CO_FLAG_CLEAR(RPDO->CANrxNew[1]);
+#endif
+    }
+    r = CO_CANrxBufferInit(
+            RPDO->CANdevRx,         /* CAN device */
+            RPDO->CANdevRxIdx,      /* rx buffer index */
+            ID,                     /* CAN identifier */
+            0x7FF,                  /* mask */
+            0,                      /* rtr */
+            (void*)RPDO,            /* object passed to receive function */
+            CO_PDO_receive);        /* this function will process received message */
+    if (r != CO_ERROR_NO){
+        RPDO->valid = false;
+        CO_FLAG_CLEAR(RPDO->CANrxNew[0]);
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+        CO_FLAG_CLEAR(RPDO->CANrxNew[1]);
+#endif
+    }
+}
+
+
+/*
+ * Configure TPDO Communication parameter.
+ *
+ * Function is called from commuincation reset or when parameter changes.
+ *
+ * Function configures following variable from CO_TPDO_t: _valid_. It also
+ * configures CAN tx buffer. If configuration fails, emergency message is send
+ * and device is not able to enter NMT operational.
+ *
+ * @param TPDO TPDO object.
+ * @param COB_IDUsedByTPDO _TPDO communication parameter_, _COB-ID for PDO_ variable
+ * from Object dictionary (index 0x1400+, subindex 1).
+ * @param syncFlag Indicate, if TPDO is synchronous.
+ */
+static void CO_TPDOconfigCom(CO_TPDO_t* TPDO, uint32_t COB_IDUsedByTPDO, uint8_t syncFlag){
+    uint16_t ID;
+
+    ID = (uint16_t)COB_IDUsedByTPDO;
+
+    /* is TPDO used? */
+    if ((COB_IDUsedByTPDO & 0xBFFFF800L) == 0 && TPDO->dataLength && ID){
+        /* is used default COB-ID? */
+        if (ID == TPDO->defaultCOB_ID) ID += TPDO->nodeId;
+        TPDO->valid = true;
+    }
+    else{
+        ID = 0;
+        TPDO->valid = false;
+    }
+
+    TPDO->CANtxBuff = CO_CANtxBufferInit(
+            TPDO->CANdevTx,            /* CAN device */
+            TPDO->CANdevTxIdx,         /* index of specific buffer inside CAN module */
+            ID,                        /* CAN identifier */
+            0,                         /* rtr */
+            TPDO->dataLength,          /* number of data bytes */
+            syncFlag);                 /* synchronous message flag bit */
+
+    if (TPDO->CANtxBuff == 0){
+        TPDO->valid = false;
+    }
+}
+
+/*
+ * Find mapped variable in Object Dictionary.
+ *
+ * Function is called from CO_R(T)PDOconfigMap or when mapping parameter changes.
+ *
+ * @param SDO SDO object.
+ * @param map PDO mapping parameter.
+ * @param R_T 0 for RPDO map, 1 for TPDO map.
+ * @param ppData Pointer to returning parameter: pointer to data of mapped variable.
+ * @param pLength Pointer to returning parameter: *add* length of mapped variable.
+ * @param pSendIfCOSFlags Pointer to returning parameter: sendIfCOSFlags variable.
+ * @param pIsMultibyteVar Pointer to returning parameter: true for multibyte variable.
+ *
+ * @return 0 on success, otherwise SDO abort code.
+ */
+static uint32_t CO_PDOfindMap(
+        CO_SDO_t               *SDO,
+        uint32_t                map,
+        uint8_t                 R_T,
+        uint8_t               **ppData,
+        uint8_t                *pLength,
+        uint8_t                *pSendIfCOSFlags,
+        uint8_t                *pIsMultibyteVar)
+{
+    uint16_t entryNo;
+    uint16_t index;
+    uint8_t subIndex;
+    uint8_t dataLen;
+    uint8_t objectLen;
+    uint8_t attr;
+
+    index = (uint16_t)(map>>16);
+    subIndex = (uint8_t)(map>>8);
+    dataLen = (uint8_t) map;   /* data length in bits */
+
+    /* data length must be byte aligned */
+    if (dataLen&0x07) return CO_SDO_AB_NO_MAP;   /* Object cannot be mapped to the PDO. */
+
+    dataLen >>= 3;    /* new data length is in bytes */
+    *pLength += dataLen;
+
+    /* total PDO length can not be more than 8 bytes */
+    if (*pLength > 8) return CO_SDO_AB_MAP_LEN;  /* The number and length of the objects to be mapped would exceed PDO length. */
+
+    /* is there a reference to dummy entries */
+    if (index <=7 && subIndex == 0){
+        static uint32_t dummyTX = 0;
+        static uint32_t dummyRX;
+        uint8_t dummySize = 4;
+
+        if (index<2) dummySize = 0;
+        else if (index==2 || index==5) dummySize = 1;
+        else if (index==3 || index==6) dummySize = 2;
+
+        /* is size of variable big enough for map */
+        if (dummySize < dataLen) return CO_SDO_AB_NO_MAP;   /* Object cannot be mapped to the PDO. */
+
+        /* Data and ODE pointer */
+        if (R_T == 0) *ppData = (uint8_t*) &dummyRX;
+        else         *ppData = (uint8_t*) &dummyTX;
+
+        return 0;
+    }
+
+    /* find object in Object Dictionary */
+    entryNo = CO_OD_find(SDO, index);
+
+    /* Does object exist in OD? */
+    if (entryNo == 0xFFFF || subIndex > SDO->OD[entryNo].maxSubIndex)
+        return CO_SDO_AB_NOT_EXIST;   /* Object does not exist in the object dictionary. */
+
+    attr = CO_OD_getAttribute(SDO, entryNo, subIndex);
+    /* Is object Mappable for RPDO? */
+    if (R_T==0 && !((attr&CO_ODA_RPDO_MAPABLE) && (attr&CO_ODA_WRITEABLE))) return CO_SDO_AB_NO_MAP;   /* Object cannot be mapped to the PDO. */
+    /* Is object Mappable for TPDO? */
+    if (R_T!=0 && !((attr&CO_ODA_TPDO_MAPABLE) && (attr&CO_ODA_READABLE))) return CO_SDO_AB_NO_MAP;   /* Object cannot be mapped to the PDO. */
+
+    /* is size of variable big enough for map */
+    objectLen = CO_OD_getLength(SDO, entryNo, subIndex);
+    if (objectLen < dataLen) return CO_SDO_AB_NO_MAP;   /* Object cannot be mapped to the PDO. */
+
+    /* mark multibyte variable */
+    *pIsMultibyteVar = (attr&CO_ODA_MB_VALUE) ? 1 : 0;
+
+    /* pointer to data */
+    *ppData = (uint8_t*) CO_OD_getDataPointer(SDO, entryNo, subIndex);
+#ifdef CO_BIG_ENDIAN
+    /* skip unused MSB bytes */
+    if (*pIsMultibyteVar){
+        *ppData += objectLen - dataLen;
+    }
+#endif
+
+    /* setup change of state flags */
+    if (attr&CO_ODA_TPDO_DETECT_COS){
+        int16_t i;
+        for(i=*pLength-dataLen; i<*pLength; i++){
+            *pSendIfCOSFlags |= 1<<i;
+        }
+    }
+
+    return 0;
+}
+
+
+/*
+ * Configure RPDO Mapping parameter.
+ *
+ * Function is called from communication reset or when parameter changes.
+ *
+ * Function configures following variables from CO_RPDO_t: _dataLength_ and
+ * _mapPointer_.
+ *
+ * @param RPDO RPDO object.
+ * @param noOfMappedObjects Number of mapped object (from OD).
+ *
+ * @return 0 on success, otherwise SDO abort code.
+ */
+static uint32_t CO_RPDOconfigMap(CO_RPDO_t* RPDO, uint8_t noOfMappedObjects){
+    int16_t i;
+    uint8_t length = 0;
+    uint32_t ret = 0;
+    const uint32_t* pMap = &RPDO->RPDOMapPar->mappedObject1;
+
+    for(i=noOfMappedObjects; i>0; i--){
+        int16_t j;
+        uint8_t* pData;
+        uint8_t dummy = 0;
+        uint8_t prevLength = length;
+        uint8_t MBvar;
+        uint32_t map = *(pMap++);
+
+        /* function do much checking of errors in map */
+        ret = CO_PDOfindMap(
+                RPDO->SDO,
+                map,
+                0,
+                &pData,
+                &length,
+                &dummy,
+                &MBvar);
+        if (ret){
+            length = 0;
+            CO_errorReport(RPDO->em, CO_EM_PDO_WRONG_MAPPING, CO_EMC_PROTOCOL_ERROR, map);
+            break;
+        }
+
+        /* write PDO data pointers */
+#ifdef CO_BIG_ENDIAN
+        if (MBvar){
+            for(j=length-1; j>=prevLength; j--)
+                RPDO->mapPointer[j] = pData++;
+        }
+        else{
+            for(j=prevLength; j<length; j++)
+                RPDO->mapPointer[j] = pData++;
+        }
+#else
+        for(j=prevLength; j<length; j++){
+            RPDO->mapPointer[j] = pData++;
+        }
+#endif
+
+    }
+
+    RPDO->dataLength = length;
+
+    return ret;
+}
+
+uint32_t debug_map =0;
+/*
+ * Configure TPDO Mapping parameter.
+ *
+ * Function is called from communication reset or when parameter changes.
+ *
+ * Function configures following variables from CO_TPDO_t: _dataLength_,
+ * _mapPointer_ and _sendIfCOSFlags_.
+ *
+ * @param TPDO TPDO object.
+ * @param noOfMappedObjects Number of mapped object (from OD).
+ *
+ * @return 0 on success, otherwise SDO abort code.
+ */
+static uint32_t CO_TPDOconfigMap(CO_TPDO_t* TPDO, uint8_t noOfMappedObjects){
+    int16_t i;
+    uint8_t length = 0;
+    uint32_t ret = 0;
+    const uint32_t* pMap = &TPDO->TPDOMapPar->mappedObject1;
+
+    TPDO->sendIfCOSFlags = 0;
+
+    for(i=noOfMappedObjects; i>0; i--){
+        int16_t j;
+        uint8_t* pData;
+        uint8_t prevLength = length;
+        uint8_t MBvar;
+        uint32_t map = *(pMap++);
+        debug_map = map;
+
+        /* function do much checking of errors in map */
+        ret = CO_PDOfindMap(
+                TPDO->SDO,
+                map,
+                1,
+                &pData,
+                &length,
+                &TPDO->sendIfCOSFlags,
+                &MBvar);
+        if (ret){
+            length = 0;
+            CO_errorReport(TPDO->em, CO_EM_PDO_WRONG_MAPPING, CO_EMC_PROTOCOL_ERROR, map);
+            break;
+        }
+
+        /* write PDO data pointers */
+#ifdef CO_BIG_ENDIAN
+        if (MBvar){
+            for(j=length-1; j>=prevLength; j--)
+                TPDO->mapPointer[j] = pData++;
+        }
+        else{
+            for(j=prevLength; j<length; j++)
+                TPDO->mapPointer[j] = pData++;
+        }
+#else
+        for(j=prevLength; j<length; j++){
+            TPDO->mapPointer[j] = pData++;
+        }
+#endif
+
+    }
+
+    TPDO->dataLength = length;
+
+    return ret;
+}
+
+
+/*
+ * Function for accessing _RPDO communication parameter_ (index 0x1400+) from SDO server.
+ *
+ * For more information see file CO_SDOserver.h.
+ */
+static CO_SDO_abortCode_t CO_ODF_RPDOcom(CO_ODF_arg_t *ODF_arg){
+    CO_RPDO_t *RPDO;
+
+    RPDO = (CO_RPDO_t*) ODF_arg->object;
+
+    /* Reading Object Dictionary variable */
+    if (ODF_arg->reading){
+        if (ODF_arg->subIndex == 1){
+            uint32_t value = CO_getUint32(ODF_arg->data);
+
+            /* if default COB ID is used, write default value here */
+            if (((value)&0xFFFF) == RPDO->defaultCOB_ID && RPDO->defaultCOB_ID)
+                value += RPDO->nodeId;
+
+            /* If PDO is not valid, set bit 31 */
+            if (!RPDO->valid) value |= 0x80000000L;
+
+            CO_setUint32(ODF_arg->data, value);
+        }
+        return CO_SDO_AB_NONE;
+    }
+
+    /* Writing Object Dictionary variable */
+    if (RPDO->restrictionFlags & 0x04)
+        return CO_SDO_AB_READONLY;  /* Attempt to write a read only object. */
+    if (*RPDO->operatingState == CO_NMT_OPERATIONAL && (RPDO->restrictionFlags & 0x01))
+        return CO_SDO_AB_DATA_DEV_STATE;   /* Data cannot be transferred or stored to the application because of the present device state. */
+
+    if (ODF_arg->subIndex == 1){   /* COB_ID */
+        uint32_t value = CO_getUint32(ODF_arg->data);
+
+        /* bits 11...29 must be zero */
+        if (value & 0x3FFF8000L)
+            return CO_SDO_AB_INVALID_VALUE;  /* Invalid value for parameter (download only). */
+
+        /* if default COB-ID is being written, write defaultCOB_ID without nodeId */
+        if (((value)&0xFFFF) == (RPDO->defaultCOB_ID + RPDO->nodeId)){
+            value &= 0xC0000000L;
+            value += RPDO->defaultCOB_ID;
+            CO_setUint32(ODF_arg->data, value);
+        }
+
+        /* if PDO is valid, bits 0..29 can not be changed */
+        if (RPDO->valid && ((value ^ RPDO->RPDOCommPar->COB_IDUsedByRPDO) & 0x3FFFFFFFL))
+            return CO_SDO_AB_INVALID_VALUE;  /* Invalid value for parameter (download only). */
+
+        /* configure RPDO */
+        CO_RPDOconfigCom(RPDO, value);
+    }
+    else if (ODF_arg->subIndex == 2){   /* Transmission_type */
+        uint8_t *value = (uint8_t*) ODF_arg->data;
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+        bool_t synchronousPrev = RPDO->synchronous;
+
+        /* values from 241...253 are not valid */
+        if (*value >= 241 && *value <= 253)
+            return CO_SDO_AB_INVALID_VALUE;  /* Invalid value for parameter (download only). */
+
+        RPDO->synchronous = (*value <= 240) ? true : false;
+
+        /* Remove old message from second buffer. */
+        if (RPDO->synchronous != synchronousPrev) {
+            CO_FLAG_CLEAR(RPDO->CANrxNew[1]);
+        }
+#else
+        /* values from 0...253 are not valid */
+        if (*value <= 253)
+            return CO_SDO_AB_INVALID_VALUE;  /* Invalid value for parameter (download only). */
+#endif
+    }
+
+    return CO_SDO_AB_NONE;
+}
+
+
+/*
+ * Function for accessing _TPDO communication parameter_ (index 0x1800+) from SDO server.
+ *
+ * For more information see file CO_SDOserver.h.
+ */
+static CO_SDO_abortCode_t CO_ODF_TPDOcom(CO_ODF_arg_t *ODF_arg){
+    CO_TPDO_t *TPDO;
+
+    TPDO = (CO_TPDO_t*) ODF_arg->object;
+
+    if (ODF_arg->subIndex == 4) return CO_SDO_AB_SUB_UNKNOWN;  /* Sub-index does not exist. */
+
+    /* Reading Object Dictionary variable */
+    if (ODF_arg->reading){
+        if (ODF_arg->subIndex == 1){   /* COB_ID */
+            uint32_t value = CO_getUint32(ODF_arg->data);
+
+            /* if default COB ID is used, write default value here */
+            if (((value)&0xFFFF) == TPDO->defaultCOB_ID && TPDO->defaultCOB_ID)
+                value += TPDO->nodeId;
+
+            /* If PDO is not valid, set bit 31 */
+            if (!TPDO->valid) value |= 0x80000000L;
+
+            CO_setUint32(ODF_arg->data, value);
+        }
+        return CO_SDO_AB_NONE;
+    }
+
+    /* Writing Object Dictionary variable */
+    if (TPDO->restrictionFlags & 0x04)
+        return CO_SDO_AB_READONLY;  /* Attempt to write a read only object. */
+    if (*TPDO->operatingState == CO_NMT_OPERATIONAL && (TPDO->restrictionFlags & 0x01))
+        return CO_SDO_AB_DATA_DEV_STATE;   /* Data cannot be transferred or stored to the application because of the present device state. */
+
+    if (ODF_arg->subIndex == 1){   /* COB_ID */
+        uint32_t value = CO_getUint32(ODF_arg->data);
+
+        /* bits 11...29 must be zero */
+        if (value & 0x3FFF8000L)
+            return CO_SDO_AB_INVALID_VALUE;  /* Invalid value for parameter (download only). */
+
+        /* if default COB-ID is being written, write defaultCOB_ID without nodeId */
+        if (((value)&0xFFFF) == (TPDO->defaultCOB_ID + TPDO->nodeId)){
+            value &= 0xC0000000L;
+            value += TPDO->defaultCOB_ID;
+
+            CO_setUint32(ODF_arg->data, value);
+        }
+
+        /* if PDO is valid, bits 0..29 can not be changed */
+        if (TPDO->valid && ((value ^ TPDO->TPDOCommPar->COB_IDUsedByTPDO) & 0x3FFFFFFFL))
+            return CO_SDO_AB_INVALID_VALUE;  /* Invalid value for parameter (download only). */
+
+        /* configure TPDO */
+        CO_TPDOconfigCom(TPDO, value, TPDO->CANtxBuff->syncFlag);
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+        TPDO->syncCounter = 255;
+#endif
+    }
+    else if (ODF_arg->subIndex == 2){   /* Transmission_type */
+        uint8_t *value = (uint8_t*) ODF_arg->data;
+
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+        /* values from 241...253 are not valid */
+        if (*value >= 241 && *value <= 253)
+            return CO_SDO_AB_INVALID_VALUE;  /* Invalid value for parameter (download only). */
+        TPDO->CANtxBuff->syncFlag = (*value <= 240) ? 1 : 0;
+        TPDO->syncCounter = 255;
+#else
+        /* values from 0...253 are not valid */
+        if (*value <= 253)
+            return CO_SDO_AB_INVALID_VALUE;  /* Invalid value for parameter (download only). */
+#endif
+    }
+    else if (ODF_arg->subIndex == 3){   /* Inhibit_Time */
+        /* if PDO is valid, value can not be changed */
+        if (TPDO->valid)
+            return CO_SDO_AB_INVALID_VALUE;  /* Invalid value for parameter (download only). */
+
+        TPDO->inhibitTimer = 0;
+    }
+    else if (ODF_arg->subIndex == 5){   /* Event_Timer */
+        uint16_t value = CO_getUint16(ODF_arg->data);
+
+        TPDO->eventTimer = ((uint32_t) value) * 1000;
+    }
+    else if (ODF_arg->subIndex == 6){   /* SYNC start value */
+        uint8_t *value = (uint8_t*) ODF_arg->data;
+
+        /* if PDO is valid, value can not be changed */
+        if (TPDO->valid)
+            return CO_SDO_AB_INVALID_VALUE;  /* Invalid value for parameter (download only). */
+
+        /* values from 240...255 are not valid */
+        if (*value > 240)
+            return CO_SDO_AB_INVALID_VALUE;  /* Invalid value for parameter (download only). */
+    }
+
+    return CO_SDO_AB_NONE;
+}
+
+
+/*
+ * Function for accessing _RPDO mapping parameter_ (index 0x1600+) from SDO server.
+ *
+ * For more information see file CO_SDOserver.h.
+ */
+static CO_SDO_abortCode_t CO_ODF_RPDOmap(CO_ODF_arg_t *ODF_arg){
+    CO_RPDO_t *RPDO;
+
+    RPDO = (CO_RPDO_t*) ODF_arg->object;
+
+    /* Reading Object Dictionary variable */
+    if (ODF_arg->reading){
+        uint8_t *value = (uint8_t*) ODF_arg->data;
+
+        if (ODF_arg->subIndex == 0){
+            /* If there is error in mapping, dataLength is 0, so numberOfMappedObjects is 0. */
+            if (!RPDO->dataLength) *value = 0;
+        }
+        return CO_SDO_AB_NONE;
+    }
+
+    /* Writing Object Dictionary variable */
+    if (RPDO->restrictionFlags & 0x08)
+        return CO_SDO_AB_READONLY;  /* Attempt to write a read only object. */
+    if (*RPDO->operatingState == CO_NMT_OPERATIONAL && (RPDO->restrictionFlags & 0x02))
+        return CO_SDO_AB_DATA_DEV_STATE;   /* Data cannot be transferred or stored to the application because of the present device state. */
+    if (RPDO->valid)
+        return CO_SDO_AB_UNSUPPORTED_ACCESS;  /* Unsupported access to an object. */
+
+    /* numberOfMappedObjects */
+    if (ODF_arg->subIndex == 0){
+        uint8_t *value = (uint8_t*) ODF_arg->data;
+
+        if (*value > 8)
+            return CO_SDO_AB_MAP_LEN;  /* Number and length of object to be mapped exceeds PDO length. */
+
+        /* configure mapping */
+        return (CO_SDO_abortCode_t) CO_RPDOconfigMap(RPDO, *value);
+    }
+
+    /* mappedObject */
+    else{
+        uint32_t value = CO_getUint32(ODF_arg->data);
+        uint8_t* pData;
+        uint8_t length = 0;
+        uint8_t dummy = 0;
+        uint8_t MBvar;
+
+        if (RPDO->dataLength)
+            return CO_SDO_AB_UNSUPPORTED_ACCESS;  /* Unsupported access to an object. */
+
+        /* verify if mapping is correct */
+        return (CO_SDO_abortCode_t) CO_PDOfindMap(
+                RPDO->SDO,
+                value,
+                0,
+               &pData,
+               &length,
+               &dummy,
+               &MBvar);
+    }
+
+    return CO_SDO_AB_NONE;
+}
+
+
+/*
+ * Function for accessing _TPDO mapping parameter_ (index 0x1A00+) from SDO server.
+ *
+ * For more information see file CO_SDOserver.h.
+ */
+static CO_SDO_abortCode_t CO_ODF_TPDOmap(CO_ODF_arg_t *ODF_arg){
+    CO_TPDO_t *TPDO;
+
+    TPDO = (CO_TPDO_t*) ODF_arg->object;
+
+    /* Reading Object Dictionary variable */
+    if (ODF_arg->reading){
+        uint8_t *value = (uint8_t*) ODF_arg->data;
+
+        if (ODF_arg->subIndex == 0){
+            /* If there is error in mapping, dataLength is 0, so numberOfMappedObjects is 0. */
+            if (!TPDO->dataLength) *value = 0;
+        }
+        return CO_SDO_AB_NONE;
+    }
+
+    /* Writing Object Dictionary variable */
+    if (TPDO->restrictionFlags & 0x08)
+        return CO_SDO_AB_READONLY;  /* Attempt to write a read only object. */
+    if (*TPDO->operatingState == CO_NMT_OPERATIONAL && (TPDO->restrictionFlags & 0x02))
+        return CO_SDO_AB_DATA_DEV_STATE;   /* Data cannot be transferred or stored to the application because of the present device state. */
+    if (TPDO->valid)
+        return CO_SDO_AB_UNSUPPORTED_ACCESS;  /* Unsupported access to an object. */
+
+    /* numberOfMappedObjects */
+    if (ODF_arg->subIndex == 0){
+        uint8_t *value = (uint8_t*) ODF_arg->data;
+
+        if (*value > 8)
+            return CO_SDO_AB_MAP_LEN;  /* Number and length of object to be mapped exceeds PDO length. */
+
+        /* configure mapping */
+        return (CO_SDO_abortCode_t) CO_TPDOconfigMap(TPDO, *value);
+    }
+
+    /* mappedObject */
+    else{
+        uint32_t value = CO_getUint32(ODF_arg->data);
+        uint8_t* pData;
+        uint8_t length = 0;
+        uint8_t dummy = 0;
+        uint8_t MBvar;
+
+        if (TPDO->dataLength)
+            return CO_SDO_AB_UNSUPPORTED_ACCESS;  /* Unsupported access to an object. */
+
+        /* verify if mapping is correct */
+        return (CO_SDO_abortCode_t) CO_PDOfindMap(
+                TPDO->SDO,
+                value,
+                1,
+               &pData,
+               &length,
+               &dummy,
+               &MBvar);
+    }
+
+    return CO_SDO_AB_NONE;
+}
+
+
+/******************************************************************************/
+CO_ReturnError_t CO_RPDO_init(
+        CO_RPDO_t              *RPDO,
+        CO_EM_t                *em,
+        CO_SDO_t               *SDO,
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+        CO_SYNC_t              *SYNC,
+#endif
+        CO_NMT_internalState_t *operatingState,
+        uint8_t                 nodeId,
+        uint16_t                defaultCOB_ID,
+        uint8_t                 restrictionFlags,
+        const CO_RPDOCommPar_t *RPDOCommPar,
+        const CO_RPDOMapPar_t  *RPDOMapPar,
+        uint16_t                idx_RPDOCommPar,
+        uint16_t                idx_RPDOMapPar,
+        CO_CANmodule_t         *CANdevRx,
+        uint16_t                CANdevRxIdx)
+{
+    /* verify arguments */
+    if (RPDO==NULL || em==NULL || SDO==NULL || operatingState==NULL ||
+        RPDOCommPar==NULL || RPDOMapPar==NULL || CANdevRx==NULL){
+        return CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    /* Configure object variables */
+    RPDO->em = em;
+    RPDO->SDO = SDO;
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+    RPDO->SYNC = SYNC;
+#endif
+    RPDO->RPDOCommPar = RPDOCommPar;
+    RPDO->RPDOMapPar = RPDOMapPar;
+    RPDO->operatingState = operatingState;
+    RPDO->nodeId = nodeId;
+    RPDO->defaultCOB_ID = defaultCOB_ID;
+    RPDO->restrictionFlags = restrictionFlags;
+#if (CO_CONFIG_PDO) & CO_CONFIG_FLAG_CALLBACK_PRE
+    RPDO->pFunctSignalPre = NULL;
+    RPDO->functSignalObjectPre = NULL;
+#endif
+
+    /* Configure Object dictionary entry at index 0x1400+ and 0x1600+ */
+    CO_OD_configure(SDO, idx_RPDOCommPar, CO_ODF_RPDOcom, (void*)RPDO, 0, 0);
+    CO_OD_configure(SDO, idx_RPDOMapPar, CO_ODF_RPDOmap, (void*)RPDO, 0, 0);
+
+    /* configure communication and mapping */
+    CO_FLAG_CLEAR(RPDO->CANrxNew[0]);
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+    CO_FLAG_CLEAR(RPDO->CANrxNew[1]);
+#endif
+    RPDO->CANdevRx = CANdevRx;
+    RPDO->CANdevRxIdx = CANdevRxIdx;
+
+    CO_RPDOconfigMap(RPDO, RPDOMapPar->numberOfMappedObjects);
+    CO_RPDOconfigCom(RPDO, RPDOCommPar->COB_IDUsedByRPDO);
+
+    return CO_ERROR_NO;
+}
+
+
+#if (CO_CONFIG_PDO) & CO_CONFIG_FLAG_CALLBACK_PRE
+/******************************************************************************/
+void CO_RPDO_initCallbackPre(
+        CO_RPDO_t              *RPDO,
+        void                   *object,
+        void                  (*pFunctSignalPre)(void *object))
+{
+    if (RPDO != NULL){
+        RPDO->functSignalObjectPre = object;
+        RPDO->pFunctSignalPre = pFunctSignalPre;
+    }
+}
+#endif
+
+
+/******************************************************************************/
+CO_ReturnError_t CO_TPDO_init(
+        CO_TPDO_t              *TPDO,
+        CO_EM_t                *em,
+        CO_SDO_t               *SDO,
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+        CO_SYNC_t              *SYNC,
+#endif
+        CO_NMT_internalState_t *operatingState,
+        uint8_t                 nodeId,
+        uint16_t                defaultCOB_ID,
+        uint8_t                 restrictionFlags,
+        const CO_TPDOCommPar_t *TPDOCommPar,
+        const CO_TPDOMapPar_t  *TPDOMapPar,
+        uint16_t                idx_TPDOCommPar,
+        uint16_t                idx_TPDOMapPar,
+        CO_CANmodule_t         *CANdevTx,
+        uint16_t                CANdevTxIdx)
+{
+    /* verify arguments */
+    if (TPDO==NULL || em==NULL || SDO==NULL || operatingState==NULL ||
+        TPDOCommPar==NULL || TPDOMapPar==NULL || CANdevTx==NULL){
+        return CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    /* Configure object variables */
+    TPDO->em = em;
+    TPDO->SDO = SDO;
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+    TPDO->SYNC = SYNC;
+#endif
+    TPDO->TPDOCommPar = TPDOCommPar;
+    TPDO->TPDOMapPar = TPDOMapPar;
+    TPDO->operatingState = operatingState;
+    TPDO->nodeId = nodeId;
+    TPDO->defaultCOB_ID = defaultCOB_ID;
+    TPDO->restrictionFlags = restrictionFlags;
+
+    /* Configure Object dictionary entry at index 0x1800+ and 0x1A00+ */
+    CO_OD_configure(SDO, idx_TPDOCommPar, CO_ODF_TPDOcom, (void*)TPDO, 0, 0);
+    CO_OD_configure(SDO, idx_TPDOMapPar, CO_ODF_TPDOmap, (void*)TPDO, 0, 0);
+
+    /* configure communication and mapping */
+    TPDO->CANdevTx = CANdevTx;
+    TPDO->CANdevTxIdx = CANdevTxIdx;
+    TPDO->inhibitTimer = 0;
+    TPDO->eventTimer = ((uint32_t) TPDOCommPar->eventTimer) * 1000;
+    if (TPDOCommPar->transmissionType>=254) TPDO->sendRequest = 1;
+
+    CO_TPDOconfigMap(TPDO, TPDOMapPar->numberOfMappedObjects);
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+    TPDO->syncCounter = 255;
+    CO_TPDOconfigCom(TPDO, TPDOCommPar->COB_IDUsedByTPDO, ((TPDOCommPar->transmissionType<=240) ? 1 : 0));
+
+    if ((TPDOCommPar->transmissionType>240 &&
+         TPDOCommPar->transmissionType<254) ||
+         TPDOCommPar->SYNCStartValue>240){
+            TPDO->valid = false;
+    }
+#else
+    CO_TPDOconfigCom(TPDO, TPDOCommPar->COB_IDUsedByTPDO, 0);
+    if (TPDOCommPar->transmissionType<254)
+        TPDO->valid = false;
+#endif
+
+    return CO_ERROR_NO;
+}
+
+
+/******************************************************************************/
+uint8_t CO_TPDOisCOS(CO_TPDO_t *TPDO){
+
+    /* Prepare TPDO data automatically from Object Dictionary variables */
+    uint8_t* pPDOdataByte;
+    uint8_t** ppODdataByte;
+
+    pPDOdataByte = &TPDO->CANtxBuff->data[TPDO->dataLength];
+    ppODdataByte = &TPDO->mapPointer[TPDO->dataLength];
+
+    switch(TPDO->dataLength){
+        case 8: if (*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x80)) return 1; // fallthrough
+        case 7: if (*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x40)) return 1; // fallthrough
+        case 6: if (*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x20)) return 1; // fallthrough
+        case 5: if (*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x10)) return 1; // fallthrough
+        case 4: if (*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x08)) return 1; // fallthrough
+        case 3: if (*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x04)) return 1; // fallthrough
+        case 2: if (*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x02)) return 1; // fallthrough
+        case 1: if (*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x01)) return 1; // fallthrough
+    }
+
+    return 0;
+}
+
+/******************************************************************************/
+CO_ReturnError_t CO_TPDOsend(CO_TPDO_t *TPDO){
+    int16_t i;
+    uint8_t* pPDOdataByte;
+    uint8_t** ppODdataByte;
+
+#if (CO_CONFIG_PDO) & CO_CONFIG_TPDO_CALLS_EXTENSION
+    if (TPDO->SDO->ODExtensions){
+        /* for each mapped OD, check mapping to see if an OD extension is available, and call it if it is */
+        const uint32_t* pMap = &TPDO->TPDOMapPar->mappedObject1;
+        CO_SDO_t *pSDO = TPDO->SDO;
+
+        for(i=TPDO->TPDOMapPar->numberOfMappedObjects; i>0; i--){
+            uint32_t map = *(pMap++);
+            uint16_t index = (uint16_t)(map>>16);
+            uint8_t subIndex = (uint8_t)(map>>8);
+            uint16_t entryNo = CO_OD_find(pSDO, index);
+            if ( entryNo == 0xFFFF ) continue;
+            CO_OD_extension_t *ext = &pSDO->ODExtensions[entryNo];
+            if ( ext->pODFunc == NULL) continue;
+            CO_ODF_arg_t ODF_arg;
+            memset((void*)&ODF_arg, 0, sizeof(CO_ODF_arg_t));
+            ODF_arg.reading = true;
+            ODF_arg.index = index;
+            ODF_arg.subIndex = subIndex;
+            ODF_arg.object = ext->object;
+            ODF_arg.attribute = CO_OD_getAttribute(pSDO, entryNo, subIndex);
+            ODF_arg.pFlags = CO_OD_getFlagsPointer(pSDO, entryNo, subIndex);
+            ODF_arg.data = CO_OD_getDataPointer(pSDO, entryNo, subIndex); //https://github.com/CANopenNode/CANopenNode/issues/100
+            ODF_arg.dataLength = CO_OD_getLength(pSDO, entryNo, subIndex);
+            ext->pODFunc(&ODF_arg);
+        }
+    }
+#endif
+    i = TPDO->dataLength;
+    pPDOdataByte = &TPDO->CANtxBuff->data[0];
+    ppODdataByte = &TPDO->mapPointer[0];
+
+    /* Copy data from Object dictionary. */
+    for(; i>0; i--) {
+        *(pPDOdataByte++) = **(ppODdataByte++);
+    }
+
+    TPDO->sendRequest = 0;
+
+    return CO_CANsend(TPDO->CANdevTx, TPDO->CANtxBuff);
+}
+
+/******************************************************************************/
+void CO_RPDO_process(CO_RPDO_t *RPDO, bool_t syncWas){
+    bool_t process_rpdo = true;
+
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+    if (RPDO->synchronous && !syncWas)
+        process_rpdo = false;
+#endif
+
+    if (!RPDO->valid || !(*RPDO->operatingState == CO_NMT_OPERATIONAL))
+    {
+        CO_FLAG_CLEAR(RPDO->CANrxNew[0]);
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+        CO_FLAG_CLEAR(RPDO->CANrxNew[1]);
+#endif
+    }
+    else if (process_rpdo)
+    {
+#if (CO_CONFIG_PDO) & CO_CONFIG_RPDO_CALLS_EXTENSION
+        bool_t update = false;
+#endif
+
+        uint8_t bufNo = 0;
+
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+        /* Determine, which of the two rx buffers, contains relevant message. */
+        if (RPDO->SYNC && RPDO->synchronous && !RPDO->SYNC->CANrxToggle) {
+            bufNo = 1;
+        }
+#endif
+
+        while(CO_FLAG_READ(RPDO->CANrxNew[bufNo])){
+            int16_t i;
+            uint8_t* pPDOdataByte;
+            uint8_t** ppODdataByte;
+
+            i = RPDO->dataLength;
+            pPDOdataByte = &RPDO->CANrxData[bufNo][0];
+            ppODdataByte = &RPDO->mapPointer[0];
+
+            /* Copy data to Object dictionary. If between the copy operation CANrxNew
+             * is set to true by receive thread, then copy the latest data again. */
+            CO_FLAG_CLEAR(RPDO->CANrxNew[bufNo]);
+            for(; i>0; i--) {
+                **(ppODdataByte++) = *(pPDOdataByte++);
+            }
+#if (CO_CONFIG_PDO) & CO_CONFIG_RPDO_CALLS_EXTENSION
+            update = true;
+#endif
+        }
+#if (CO_CONFIG_PDO) & CO_CONFIG_RPDO_CALLS_EXTENSION
+        if (update && RPDO->SDO->ODExtensions){
+            int16_t i;
+            /* for each mapped OD, check mapping to see if an OD extension is available, and call it if it is */
+            const uint32_t* pMap = &RPDO->RPDOMapPar->mappedObject1;
+            CO_SDO_t *pSDO = RPDO->SDO;
+
+            for(i=RPDO->RPDOMapPar->numberOfMappedObjects; i>0; i--){
+                uint32_t map = *(pMap++);
+                uint16_t index = (uint16_t)(map>>16);
+                uint8_t subIndex = (uint8_t)(map>>8);
+                uint16_t entryNo = CO_OD_find(pSDO, index);
+                if ( entryNo == 0xFFFF ) continue;
+                CO_OD_extension_t *ext = &pSDO->ODExtensions[entryNo];
+                if ( ext->pODFunc == NULL) continue;
+                CO_ODF_arg_t ODF_arg;
+                memset((void*)&ODF_arg, 0, sizeof(CO_ODF_arg_t));
+                ODF_arg.reading = false;
+                ODF_arg.index = index;
+                ODF_arg.subIndex = subIndex;
+                ODF_arg.object = ext->object;
+                ODF_arg.attribute = CO_OD_getAttribute(pSDO, entryNo, subIndex);
+                ODF_arg.pFlags = CO_OD_getFlagsPointer(pSDO, entryNo, subIndex);
+                ODF_arg.data = CO_OD_getDataPointer(pSDO, entryNo, subIndex); //https://github.com/CANopenNode/CANopenNode/issues/100
+                ODF_arg.dataLength = CO_OD_getLength(pSDO, entryNo, subIndex);
+                ext->pODFunc(&ODF_arg);
+            }
+        }
+#endif
+    }
+}
+
+
+/******************************************************************************/
+void CO_TPDO_process(
+        CO_TPDO_t              *TPDO,
+        bool_t                  syncWas,
+        uint32_t                timeDifference_us,
+        uint32_t               *timerNext_us)
+{
+    (void)timerNext_us; /* may be unused */
+
+    /* update timers */
+    TPDO->inhibitTimer = (TPDO->inhibitTimer > timeDifference_us) ? (TPDO->inhibitTimer - timeDifference_us) : 0;
+    TPDO->eventTimer = (TPDO->eventTimer > timeDifference_us) ? (TPDO->eventTimer - timeDifference_us) : 0;
+
+    if (TPDO->valid && *TPDO->operatingState == CO_NMT_OPERATIONAL){
+
+        /* Send PDO by application request or by Event timer */
+        if (TPDO->TPDOCommPar->transmissionType >= 253){
+            if (TPDO->inhibitTimer == 0 && (TPDO->sendRequest || (TPDO->TPDOCommPar->eventTimer && TPDO->eventTimer == 0))){
+                if (CO_TPDOsend(TPDO) == CO_ERROR_NO){
+                    /* successfully sent */
+                    TPDO->inhibitTimer = ((uint32_t) TPDO->TPDOCommPar->inhibitTime) * 100;
+                    TPDO->eventTimer = ((uint32_t) TPDO->TPDOCommPar->eventTimer) * 1000;
+                }
+            }
+#if (CO_CONFIG_PDO) & CO_CONFIG_FLAG_TIMERNEXT
+            if (timerNext_us != NULL){
+                if (TPDO->sendRequest && *timerNext_us > TPDO->inhibitTimer){
+                    *timerNext_us = TPDO->inhibitTimer; /* Schedule for just beyond inhibit window */
+                }else if (TPDO->TPDOCommPar->eventTimer && *timerNext_us > TPDO->eventTimer){
+                    *timerNext_us = TPDO->eventTimer; /* Schedule for next maximum event time */
+                }
+            }
+#endif
+        }
+
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+        /* Synchronous PDOs */
+        else if (TPDO->SYNC && syncWas){
+            /* send synchronous acyclic PDO */
+            if (TPDO->TPDOCommPar->transmissionType == 0){
+                if (TPDO->sendRequest) CO_TPDOsend(TPDO);
+            }
+            /* send synchronous cyclic PDO */
+            else{
+                /* is the start of synchronous TPDO transmission */
+                if (TPDO->syncCounter == 255){
+                    if (TPDO->SYNC->counterOverflowValue && TPDO->TPDOCommPar->SYNCStartValue)
+                        TPDO->syncCounter = 254;   /* SYNCStartValue is in use */
+                    else
+                        TPDO->syncCounter = TPDO->TPDOCommPar->transmissionType;
+                }
+                /* if the SYNCStartValue is in use, start first TPDO after SYNC with matched SYNCStartValue. */
+                if (TPDO->syncCounter == 254){
+                    if (TPDO->SYNC->counter == TPDO->TPDOCommPar->SYNCStartValue){
+                        TPDO->syncCounter = TPDO->TPDOCommPar->transmissionType;
+                        CO_TPDOsend(TPDO);
+                    }
+                }
+                /* Send PDO after every N-th Sync */
+                else if (--TPDO->syncCounter == 0){
+                    TPDO->syncCounter = TPDO->TPDOCommPar->transmissionType;
+                    CO_TPDOsend(TPDO);
+                }
+            }
+        }
+#endif
+
+    }
+    else{
+        /* Not operational or valid. Force TPDO first send after operational or valid. */
+        if (TPDO->TPDOCommPar->transmissionType>=254) TPDO->sendRequest = 1;
+        else                                         TPDO->sendRequest = 0;
+    }
+}

controller_yy_app/middleware/CANopenNode/301/CO_PDO.h

@@ -0,0 +1,439 @@
+/**
+ * CANopen Process Data Object protocol.
+ *
+ * @file        CO_PDO.h
+ * @ingroup     CO_PDO
+ * @author      Janez Paternoster
+ * @copyright   2004 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CO_PDO_H
+#define CO_PDO_H
+
+#include "301/CO_driver.h"
+#include "301/CO_SDOserver.h"
+#include "301/CO_Emergency.h"
+#include "301/CO_NMT_Heartbeat.h"
+#include "301/CO_SYNC.h"
+
+/* default configuration, see CO_config.h */
+#ifndef CO_CONFIG_PDO
+#define CO_CONFIG_PDO (CO_CONFIG_RPDO_ENABLE | \
+                       CO_CONFIG_TPDO_ENABLE | \
+                       CO_CONFIG_PDO_SYNC_ENABLE)
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_PDO PDO
+ * @ingroup CO_CANopen_301
+ * @{
+ *
+ * CANopen Process Data Object protocol.
+ *
+ * Process data objects are used for real-time data transfer with no protocol
+ * overhead.
+ *
+ * TPDO with specific identifier is transmitted by one device and recieved by
+ * zero or more devices as RPDO. PDO communication parameters(COB-ID,
+ * transmission type, etc.) are in Object Dictionary at index 0x1400+ and
+ * 0x1800+. PDO mapping parameters (size and contents of the PDO) are in Object
+ * Dictionary at index 0x1600+ and 0x1A00+.
+ *
+ * Features of the PDO as implemented here, in CANopenNode:
+ *  - Dynamic PDO mapping.
+ *  - Map granularity of one byte.
+ *  - After RPDO is received from CAN bus, its data are copied to buffer.
+ *    Function CO_RPDO_process() (called by application) copies data to
+ *    mapped objects in Object Dictionary. Synchronous RPDOs are processed AFTER
+ *    reception of the next SYNC message.
+ *  - Function CO_TPDO_process() (called by application) sends TPDO if
+ *    necessary. There are possible different transmission types, including
+ *    automatic detection of Change of State of specific variable.
+ */
+
+
+/**
+ * RPDO communication parameter. The same as record from Object dictionary (index 0x1400+).
+ */
+typedef struct{
+    uint8_t             maxSubIndex;    /**< Equal to 2 */
+    /** Communication object identifier for message received. Meaning of the specific bits:
+        - Bit  0-10: COB-ID for PDO, to change it bit 31 must be set.
+        - Bit 11-29: set to 0 for 11 bit COB-ID.
+        - Bit 30:    If true, rtr are NOT allowed for PDO.
+        - Bit 31:    If true, node does NOT use the PDO. */
+    uint32_t            COB_IDUsedByRPDO;
+    /** Transmission type. Values:
+        - 0-240:   Reciving is synchronous, process after next reception of the SYNC object.
+        - 241-253: Not used.
+        - 254:     Manufacturer specific.
+        - 255:     Asynchronous. */
+    uint8_t             transmissionType;
+}CO_RPDOCommPar_t;
+
+
+/**
+ * RPDO mapping parameter. The same as record from Object dictionary (index 0x1600+).
+ */
+typedef struct{
+    /** Actual number of mapped objects from 0 to 8. To change mapped object,
+    this value must be 0. */
+    uint8_t             numberOfMappedObjects;
+    /** Location and size of the mapped object. Bit meanings `0xIIIISSLL`:
+        - Bit  0-7:  Data Length in bits.
+        - Bit 8-15:  Subindex from object distionary.
+        - Bit 16-31: Index from object distionary. */
+    uint32_t            mappedObject1;
+    uint32_t            mappedObject2;  /**< Same */
+    uint32_t            mappedObject3;  /**< Same */
+    uint32_t            mappedObject4;  /**< Same */
+    uint32_t            mappedObject5;  /**< Same */
+    uint32_t            mappedObject6;  /**< Same */
+    uint32_t            mappedObject7;  /**< Same */
+    uint32_t            mappedObject8;  /**< Same */
+}CO_RPDOMapPar_t;
+
+
+/**
+ * TPDO communication parameter. The same as record from Object dictionary (index 0x1800+).
+ */
+typedef struct{
+    uint8_t             maxSubIndex;    /**< Equal to 6 */
+    /** Communication object identifier for transmitting message. Meaning of the specific bits:
+        - Bit  0-10: COB-ID for PDO, to change it bit 31 must be set.
+        - Bit 11-29: set to 0 for 11 bit COB-ID.
+        - Bit 30:    If true, rtr are NOT allowed for PDO.
+        - Bit 31:    If true, node does NOT use the PDO. */
+    uint32_t            COB_IDUsedByTPDO;
+    /** Transmission type. Values:
+        - 0:       Transmiting is synchronous, specification in device profile.
+        - 1-240:   Transmiting is synchronous after every N-th SYNC object.
+        - 241-251: Not used.
+        - 252-253: Transmited only on reception of Remote Transmission Request.
+        - 254:     Manufacturer specific.
+        - 255:     Asinchronous, specification in device profile. */
+    uint8_t             transmissionType;
+    /** Minimum time between transmissions of the PDO in 100micro seconds.
+    Zero disables functionality. */
+    uint16_t            inhibitTime;
+    /** Not used */
+    uint8_t             compatibilityEntry;
+    /** Time between periodic transmissions of the PDO in milliseconds.
+    Zero disables functionality. */
+    uint16_t            eventTimer;
+    /** Used with numbered SYNC messages. Values:
+        - 0:       Counter of the SYNC message shall not be processed.
+        - 1-240:   The SYNC message with the counter value equal to this value
+                   shall be regarded as the first received SYNC message. */
+    uint8_t             SYNCStartValue;
+}CO_TPDOCommPar_t;
+
+
+/**
+ * TPDO mapping parameter. The same as record from Object dictionary (index 0x1A00+).
+ */
+typedef struct{
+    /** Actual number of mapped objects from 0 to 8. To change mapped object,
+    this value must be 0. */
+    uint8_t             numberOfMappedObjects;
+    /** Location and size of the mapped object. Bit meanings `0xIIIISSLL`:
+        - Bit  0-7:  Data Length in bits.
+        - Bit 8-15:  Subindex from object distionary.
+        - Bit 16-31: Index from object distionary. */
+    uint32_t            mappedObject1;
+    uint32_t            mappedObject2;  /**< Same */
+    uint32_t            mappedObject3;  /**< Same */
+    uint32_t            mappedObject4;  /**< Same */
+    uint32_t            mappedObject5;  /**< Same */
+    uint32_t            mappedObject6;  /**< Same */
+    uint32_t            mappedObject7;  /**< Same */
+    uint32_t            mappedObject8;  /**< Same */
+}CO_TPDOMapPar_t;
+
+
+/**
+ * RPDO object.
+ */
+typedef struct{
+    CO_EM_t            *em;             /**< From CO_RPDO_init() */
+    CO_SDO_t           *SDO;            /**< From CO_RPDO_init() */
+    const CO_RPDOCommPar_t *RPDOCommPar;/**< From CO_RPDO_init() */
+    const CO_RPDOMapPar_t  *RPDOMapPar; /**< From CO_RPDO_init() */
+    CO_NMT_internalState_t *operatingState; /**< From CO_RPDO_init() */
+    uint8_t             nodeId;         /**< From CO_RPDO_init() */
+    uint16_t            defaultCOB_ID;  /**< From CO_RPDO_init() */
+    uint8_t             restrictionFlags;/**< From CO_RPDO_init() */
+    /** True, if PDO is enabled and valid */
+    bool_t              valid;
+    /** Data length of the received PDO message. Calculated from mapping */
+    uint8_t             dataLength;
+    /** Pointers to 8 data objects, where PDO will be copied */
+    uint8_t            *mapPointer[8];
+#if ((CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE) || defined CO_DOXYGEN
+    CO_SYNC_t          *SYNC;           /**< From CO_RPDO_init() */
+    /** True, if PDO synchronous (transmissionType <= 240) */
+    bool_t              synchronous;
+    /** Variable indicates, if new PDO message received from CAN bus. */
+    volatile void      *CANrxNew[2];
+    /** 8 data bytes of the received message. */
+    uint8_t             CANrxData[2][8];
+#else
+    volatile void      *CANrxNew[1];
+    uint8_t             CANrxData[1][8];
+#endif
+#if ((CO_CONFIG_PDO) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+    /** From CO_RPDO_initCallbackPre() or NULL */
+    void              (*pFunctSignalPre)(void *object);
+    /** From CO_RPDO_initCallbackPre() or NULL */
+    void               *functSignalObjectPre;
+#endif
+    CO_CANmodule_t     *CANdevRx;       /**< From CO_RPDO_init() */
+    uint16_t            CANdevRxIdx;    /**< From CO_RPDO_init() */
+}CO_RPDO_t;
+
+
+/**
+ * TPDO object.
+ */
+typedef struct{
+    CO_EM_t            *em;             /**< From CO_TPDO_init() */
+    CO_SDO_t           *SDO;            /**< From CO_TPDO_init() */
+    const CO_TPDOCommPar_t *TPDOCommPar;/**< From CO_TPDO_init() */
+    const CO_TPDOMapPar_t  *TPDOMapPar; /**< From CO_TPDO_init() */
+    CO_NMT_internalState_t *operatingState; /**< From CO_TPDO_init() */
+    uint8_t             nodeId;         /**< From CO_TPDO_init() */
+    uint16_t            defaultCOB_ID;  /**< From CO_TPDO_init() */
+    uint8_t             restrictionFlags;/**< From CO_TPDO_init() */
+    bool_t              valid;          /**< True, if PDO is enabled and valid */
+    /** Data length of the transmitting PDO message. Calculated from mapping */
+    uint8_t             dataLength;
+    /** If application set this flag, PDO will be later sent by
+    function CO_TPDO_process(). Depends on transmission type. */
+    uint8_t             sendRequest;
+    /** Pointers to 8 data objects, where PDO will be copied */
+    uint8_t            *mapPointer[8];
+    /** Inhibit timer used for inhibit PDO sending translated to microseconds */
+    uint32_t            inhibitTimer;
+    /** Event timer used for PDO sending translated to microseconds */
+    uint32_t            eventTimer;
+    /** Each flag bit is connected with one mapPointer. If flag bit
+    is true, CO_TPDO_process() functiuon will send PDO if
+    Change of State is detected on value pointed by that mapPointer */
+    uint8_t             sendIfCOSFlags;
+#if ((CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE) || defined CO_DOXYGEN
+    /** SYNC counter used for PDO sending */
+    uint8_t             syncCounter;
+    CO_SYNC_t          *SYNC;           /**< From CO_TPDO_init() */
+#endif
+    CO_CANmodule_t     *CANdevTx;       /**< From CO_TPDO_init() */
+    CO_CANtx_t         *CANtxBuff;      /**< CAN transmit buffer inside CANdev */
+    uint16_t            CANdevTxIdx;    /**< From CO_TPDO_init() */
+}CO_TPDO_t;
+
+
+/**
+ * Initialize RPDO object.
+ *
+ * Function must be called in the communication reset section.
+ *
+ * @param RPDO This object will be initialized.
+ * @param em Emergency object.
+ * @param SDO SDO server object.
+ * @param SYNC void pointer to SYNC object or NULL.
+ * @param operatingState Pointer to variable indicating CANopen device NMT internal state.
+ * @param nodeId CANopen Node ID of this device. If default COB_ID is used, value will be added.
+ * @param defaultCOB_ID Default COB ID for this PDO (without NodeId).
+ * See #CO_Default_CAN_ID_t
+ * @param restrictionFlags Flag bits indicates, how PDO communication
+ * and mapping parameters are handled:
+ *  - Bit1: If true, communication parameters are writeable only in pre-operational NMT state.
+ *  - Bit2: If true, mapping parameters are writeable only in pre-operational NMT state.
+ *  - Bit3: If true, communication parameters are read-only.
+ *  - Bit4: If true, mapping parameters are read-only.
+ * @param RPDOCommPar Pointer to _RPDO communication parameter_ record from Object
+ * dictionary (index 0x1400+).
+ * @param RPDOMapPar Pointer to _RPDO mapping parameter_ record from Object
+ * dictionary (index 0x1600+).
+ * @param idx_RPDOCommPar Index in Object Dictionary.
+ * @param idx_RPDOMapPar Index in Object Dictionary.
+ * @param CANdevRx CAN device for PDO reception.
+ * @param CANdevRxIdx Index of receive buffer in the above CAN device.
+ *
+ * @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ */
+CO_ReturnError_t CO_RPDO_init(
+        CO_RPDO_t              *RPDO,
+        CO_EM_t                *em,
+        CO_SDO_t               *SDO,
+#if ((CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE) || defined CO_DOXYGEN
+        CO_SYNC_t              *SYNC,
+#endif
+        CO_NMT_internalState_t *operatingState,
+        uint8_t                 nodeId,
+        uint16_t                defaultCOB_ID,
+        uint8_t                 restrictionFlags,
+        const CO_RPDOCommPar_t *RPDOCommPar,
+        const CO_RPDOMapPar_t  *RPDOMapPar,
+        uint16_t                idx_RPDOCommPar,
+        uint16_t                idx_RPDOMapPar,
+        CO_CANmodule_t         *CANdevRx,
+        uint16_t                CANdevRxIdx);
+
+
+#if ((CO_CONFIG_PDO) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+/**
+ * Initialize RPDO callback function.
+ *
+ * Function initializes optional callback function, which should immediately
+ * start processing of CO_RPDO_process() function.
+ * Callback is called after RPDO message is received from the CAN bus.
+ *
+ * @param RPDO This object.
+ * @param object Pointer to object, which will be passed to pFunctSignalPre(). Can be NULL
+ * @param pFunctSignalPre Pointer to the callback function. Not called if NULL.
+ */
+void CO_RPDO_initCallbackPre(
+        CO_RPDO_t              *RPDO,
+        void                   *object,
+        void                  (*pFunctSignalPre)(void *object));
+#endif
+
+
+/**
+ * Initialize TPDO object.
+ *
+ * Function must be called in the communication reset section.
+ *
+ * @param TPDO This object will be initialized.
+ * @param em Emergency object.
+ * @param SDO SDO object.
+ * @param SYNC void pointer to SYNC object or NULL.
+ * @param operatingState Pointer to variable indicating CANopen device NMT internal state.
+ * @param nodeId CANopen Node ID of this device. If default COB_ID is used, value will be added.
+ * @param defaultCOB_ID Default COB ID for this PDO (without NodeId).
+ * See #CO_Default_CAN_ID_t
+ * @param restrictionFlags Flag bits indicates, how PDO communication
+ * and mapping parameters are handled:
+ *  - Bit1: If true, communication parameters are writeable only in pre-operational NMT state.
+ *  - Bit2: If true, mapping parameters are writeable only in pre-operational NMT state.
+ *  - Bit3: If true, communication parameters are read-only.
+ *  - Bit4: If true, mapping parameters are read-only.
+ * @param TPDOCommPar Pointer to _TPDO communication parameter_ record from Object
+ * dictionary (index 0x1400+).
+ * @param TPDOMapPar Pointer to _TPDO mapping parameter_ record from Object
+ * dictionary (index 0x1600+).
+ * @param idx_TPDOCommPar Index in Object Dictionary.
+ * @param idx_TPDOMapPar Index in Object Dictionary.
+ * @param CANdevTx CAN device used for PDO transmission.
+ * @param CANdevTxIdx Index of transmit buffer in the above CAN device.
+ *
+ * @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ */
+CO_ReturnError_t CO_TPDO_init(
+        CO_TPDO_t              *TPDO,
+        CO_EM_t                *em,
+        CO_SDO_t               *SDO,
+#if ((CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE) || defined CO_DOXYGEN
+        CO_SYNC_t              *SYNC,
+#endif
+        CO_NMT_internalState_t *operatingState,
+        uint8_t                 nodeId,
+        uint16_t                defaultCOB_ID,
+        uint8_t                 restrictionFlags,
+        const CO_TPDOCommPar_t *TPDOCommPar,
+        const CO_TPDOMapPar_t  *TPDOMapPar,
+        uint16_t                idx_TPDOCommPar,
+        uint16_t                idx_TPDOMapPar,
+        CO_CANmodule_t         *CANdevTx,
+        uint16_t                CANdevTxIdx);
+
+
+/**
+ * Verify Change of State of the PDO.
+ *
+ * Function verifies if variable mapped to TPDO has changed its value. Verified
+ * are only variables, which has set attribute _CO_ODA_TPDO_DETECT_COS_ in
+ * #CO_SDO_OD_attributes_t.
+ *
+ * Function may be called by application just before CO_TPDO_process() function,
+ * for example: `TPDOx->sendRequest = CO_TPDOisCOS(TPDOx); CO_TPDO_process(TPDOx, ....`
+ *
+ * @param TPDO TPDO object.
+ *
+ * @return True if COS was detected.
+ */
+uint8_t CO_TPDOisCOS(CO_TPDO_t *TPDO);
+
+
+/**
+ * Send TPDO message.
+ *
+ * Function prepares TPDO data from Object Dictionary variables. It should not
+ * be called by application, it is called from CO_TPDO_process().
+ *
+ *
+ * @param TPDO TPDO object.
+ *
+ * @return Same as CO_CANsend().
+ */
+CO_ReturnError_t CO_TPDOsend(CO_TPDO_t *TPDO);
+
+
+/**
+ * Process received PDO messages.
+ *
+ * Function must be called cyclically in any NMT state. It copies data from RPDO
+ * to Object Dictionary variables if: new PDO receives and PDO is valid and NMT
+ * operating state is operational. It does not verify _transmission type_.
+ *
+ * @param RPDO This object.
+ * @param syncWas True, if CANopen SYNC message was just received or transmitted.
+ */
+void CO_RPDO_process(CO_RPDO_t *RPDO, bool_t syncWas);
+
+
+/**
+ * Process transmitting PDO messages.
+ *
+ * Function must be called cyclically in any NMT state. It prepares and sends
+ * TPDO if necessary. If Change of State needs to be detected, function
+ * CO_TPDOisCOS() must be called before.
+ *
+ * @param TPDO This object.
+ * @param syncWas True, if CANopen SYNC message was just received or transmitted.
+ * @param timeDifference_us Time difference from previous function call in [microseconds].
+ * @param [out] timerNext_us info to OS - see CO_process_SYNC_PDO().
+ */
+void CO_TPDO_process(
+        CO_TPDO_t              *TPDO,
+        bool_t                  syncWas,
+        uint32_t                timeDifference_us,
+        uint32_t               *timerNext_us);
+
+/** @} */ /* CO_PDO */
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+#endif /* CO_PDO_H */

controller_yy_app/middleware/CANopenNode/301/CO_SDOclient.c

@@ -0,0 +1,1545 @@
+/*
+ * CANopen Service Data Object - client.
+ *
+ * @file        CO_SDOclient.c
+ * @ingroup     CO_SDOclient
+ * @author      Janez Paternoster
+ * @author      Matej Severkar
+ * @copyright   2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "301/CO_SDOclient.h"
+
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_ENABLE
+
+/* verify configuration */
+#if CO_CONFIG_SDO_CLI_BUFFER_SIZE < 7
+ #error CO_CONFIG_SDO_CLI_BUFFER_SIZE must be set to 7 or more.
+#endif
+#if !((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ENABLE)
+ #error CO_CONFIG_FIFO_ENABLE must be enabled.
+#endif
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
+ #if !((CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED)
+  #error CO_CONFIG_SDO_CLI_SEGMENTED must be enabled.
+ #endif
+ #if !((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ALT_READ)
+  #error CO_CONFIG_FIFO_ALT_READ must be enabled.
+ #endif
+ #if !((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_CRC16_CCITT)
+  #error CO_CONFIG_FIFO_CRC16_CCITT must be enabled.
+ #endif
+#endif
+
+/* default 'protocol switch threshold' size for block transfer */
+#ifndef CO_CONFIG_SDO_CLI_PST
+#define CO_CONFIG_SDO_CLI_PST 21
+#endif
+
+
+/*
+ * Read received message from CAN module.
+ *
+ * Function will be called (by CAN receive interrupt) every time, when CAN
+ * message with correct identifier will be received. For more information and
+ * description of parameters see file CO_driver.h.
+ */
+static void CO_SDOclient_receive(void *object, void *msg) {
+    CO_SDOclient_t *SDO_C = (CO_SDOclient_t*)object;
+    uint8_t DLC = CO_CANrxMsg_readDLC(msg);
+    uint8_t *data = CO_CANrxMsg_readData(msg);
+
+    /* Ignore messages in idle state and messages with wrong length. Ignore
+     * message also if previous message was not processed yet and not abort */
+    if (SDO_C->state != CO_SDO_ST_IDLE && DLC == 8U
+        && (!CO_FLAG_READ(SDO_C->CANrxNew) || data[0] == 0x80)
+    ) {
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
+        if (data[0] == 0x80 /* abort from server */
+            || (SDO_C->state != CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ
+                && SDO_C->state != CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP)
+        ) {
+#endif
+            /* copy data and set 'new message' flag */
+            memcpy((void *)&SDO_C->CANrxData[0], (const void *)&data[0], 8);
+            CO_FLAG_SET(SDO_C->CANrxNew);
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_CALLBACK_PRE
+            /* Optional signal to RTOS, which can resume task, which handles
+            * SDO client processing. */
+            if (SDO_C->pFunctSignal != NULL) {
+                SDO_C->pFunctSignal(SDO_C->functSignalObject);
+            }
+#endif
+
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
+        }
+        else if (SDO_C->state == CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ) {
+            /* block upload, copy data directly */
+            CO_SDO_state_t state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ;
+            uint8_t seqno = data[0] & 0x7F;
+            SDO_C->timeoutTimer = 0;
+            SDO_C->block_timeoutTimer = 0;
+
+            /* verify if sequence number is correct */
+            if (seqno <= SDO_C->block_blksize
+                && seqno == (SDO_C->block_seqno + 1)
+            ) {
+                SDO_C->block_seqno = seqno;
+
+                /* is this the last segment? */
+                if ((data[0] & 0x80) != 0) {
+                    /* copy data to temporary buffer, because we don't know the
+                     * number of bytes not containing data */
+                    memcpy((void *)&SDO_C->block_dataUploadLast[0],
+                           (const void *)&data[1], 7);
+                    SDO_C->finished = true;
+                    state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP;
+                }
+                else {
+                    /* Copy data. There is always enough space in fifo buffer,
+                     * because block_blksize was calculated before */
+                    CO_fifo_write(&SDO_C->bufFifo,
+                                  (const char *)&data[1],
+                                  7, &SDO_C->block_crc);
+                    SDO_C->sizeTran += 7;
+                    /* all segments in sub-block has been transferred */
+                    if (seqno == SDO_C->block_blksize) {
+                        state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP;
+                    }
+                }
+            }
+            /* If message is duplicate or sequence didn't start yet, ignore
+             * it. Otherwise seqno is wrong, so break sub-block. Data after
+             * last good seqno will be re-transmitted. */
+            else if (seqno != SDO_C->block_seqno && SDO_C->block_seqno != 0U) {
+                state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP;
+#ifdef CO_DEBUG_SDO_CLIENT
+                char msg[80];
+                sprintf(msg,
+                        "sub-block, rx WRONG: sequno=%02X, previous=%02X",
+                        seqno, SDO_C->block_seqno);
+                CO_DEBUG_SDO_CLIENT(msg);
+#endif
+            }
+#ifdef CO_DEBUG_SDO_CLIENT
+            else {
+                char msg[80];
+                sprintf(msg,
+                        "sub-block, rx ignored: sequno=%02X, expected=%02X",
+                        seqno, SDO_C->block_seqno + 1);
+                CO_DEBUG_SDO_CLIENT(msg);
+            }
+#endif
+
+            /* Is exit from sub-block receive state? */
+            if (state != CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ) {
+                /* Processing will continue in another thread, so make memory
+                 * barrier here with CO_FLAG_CLEAR() call. */
+                CO_FLAG_CLEAR(SDO_C->CANrxNew);
+                SDO_C->state = state;
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_CALLBACK_PRE
+                /* Optional signal to RTOS, which can resume task, which handles
+                * SDO client processing. */
+                if (SDO_C->pFunctSignal != NULL) {
+                    SDO_C->pFunctSignal(SDO_C->functSignalObject);
+                }
+#endif
+            }
+        }
+#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK */
+    }
+}
+
+
+/******************************************************************************/
+CO_ReturnError_t CO_SDOclient_init(CO_SDOclient_t *SDO_C,
+                                   void *SDO,
+                                   CO_SDOclientPar_t *SDOClientPar,
+                                   CO_CANmodule_t *CANdevRx,
+                                   uint16_t CANdevRxIdx,
+                                   CO_CANmodule_t *CANdevTx,
+                                   uint16_t CANdevTxIdx)
+{
+    /* verify arguments */
+    if (SDO_C == NULL || SDOClientPar == NULL || SDOClientPar->maxSubIndex != 3
+        || CANdevRx==NULL || CANdevTx==NULL
+    ) {
+        return CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    /* Configure object variables */
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_LOCAL
+    SDO_C->SDO = (CO_SDO_t *)SDO;
+#endif
+    SDO_C->SDOClientPar = SDOClientPar;
+    SDO_C->CANdevRx = CANdevRx;
+    SDO_C->CANdevRxIdx = CANdevRxIdx;
+    SDO_C->CANdevTx = CANdevTx;
+    SDO_C->CANdevTxIdx = CANdevTxIdx;
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_CALLBACK_PRE
+    SDO_C->pFunctSignal = NULL;
+    SDO_C->functSignalObject = NULL;
+#endif
+
+    /* prepare circular fifo buffer */
+    CO_fifo_init(&SDO_C->bufFifo, SDO_C->buf,
+                 CO_CONFIG_SDO_CLI_BUFFER_SIZE + 1);
+
+    SDO_C->state = CO_SDO_ST_IDLE;
+    CO_FLAG_CLEAR(SDO_C->CANrxNew);
+
+    SDO_C->COB_IDClientToServerPrev = 0;
+    SDO_C->COB_IDServerToClientPrev = 0;
+    CO_SDOclient_setup(SDO_C,
+                       SDO_C->SDOClientPar->COB_IDClientToServer,
+                       SDO_C->SDOClientPar->COB_IDServerToClient,
+                       SDO_C->SDOClientPar->nodeIDOfTheSDOServer);
+
+    return CO_ERROR_NO;
+}
+
+
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_CALLBACK_PRE
+/******************************************************************************/
+void CO_SDOclient_initCallbackPre(CO_SDOclient_t *SDOclient,
+                                  void *object,
+                                  void (*pFunctSignal)(void *object))
+{
+    if (SDOclient != NULL) {
+        SDOclient->functSignalObject = object;
+        SDOclient->pFunctSignal = pFunctSignal;
+    }
+}
+#endif
+
+
+/******************************************************************************/
+CO_SDO_return_t CO_SDOclient_setup(CO_SDOclient_t *SDO_C,
+                                   uint32_t COB_IDClientToServer,
+                                   uint32_t COB_IDServerToClient,
+                                   uint8_t nodeIDOfTheSDOServer)
+{
+    uint32_t idCtoS, idStoC;
+    uint8_t idNode;
+
+    /* verify parameters */
+    if (SDO_C == NULL
+        || (COB_IDClientToServer & 0x7FFFF800L) != 0
+        || (COB_IDServerToClient & 0x7FFFF800L) != 0
+        || nodeIDOfTheSDOServer > 127
+    ) {
+        return CO_SDO_RT_wrongArguments;
+    }
+
+    /* Configure object variables */
+    SDO_C->state = CO_SDO_ST_IDLE;
+    CO_FLAG_CLEAR(SDO_C->CANrxNew);
+
+    /* setup Object Dictionary variables */
+    if ((COB_IDClientToServer & 0x80000000L) != 0
+        || (COB_IDServerToClient & 0x80000000L) != 0
+        || nodeIDOfTheSDOServer == 0
+    ) {
+        /* SDO is NOT used */
+        idCtoS = 0x80000000L;
+        idStoC = 0x80000000L;
+        idNode = 0;
+    }
+    else {
+        if (COB_IDClientToServer == 0 || COB_IDServerToClient == 0) {
+            idCtoS = 0x600 + nodeIDOfTheSDOServer;
+            idStoC = 0x580 + nodeIDOfTheSDOServer;
+        }
+        else {
+            idCtoS = COB_IDClientToServer;
+            idStoC = COB_IDServerToClient;
+        }
+        idNode = nodeIDOfTheSDOServer;
+    }
+
+    SDO_C->SDOClientPar->COB_IDClientToServer = idCtoS;
+    SDO_C->SDOClientPar->COB_IDServerToClient = idStoC;
+    SDO_C->SDOClientPar->nodeIDOfTheSDOServer = idNode;
+
+    /* configure SDO client CAN reception, if differs. */
+    if (SDO_C->COB_IDClientToServerPrev != idCtoS
+        || SDO_C->COB_IDServerToClientPrev != idStoC
+    ) {
+        CO_ReturnError_t ret = CO_CANrxBufferInit(
+                SDO_C->CANdevRx,        /* CAN device */
+                SDO_C->CANdevRxIdx,     /* rx buffer index */
+                (uint16_t)idStoC,       /* CAN identifier */
+                0x7FF,                  /* mask */
+                0,                      /* rtr */
+                (void*)SDO_C,           /* object passed to receive function */
+                CO_SDOclient_receive);  /* this function will process rx msg */
+
+        /* configure SDO client CAN transmission */
+        SDO_C->CANtxBuff = CO_CANtxBufferInit(
+                SDO_C->CANdevTx,        /* CAN device */
+                SDO_C->CANdevTxIdx,     /* index of buffer inside CAN module */
+                (uint16_t)idCtoS,       /* CAN identifier */
+                0,                      /* rtr */
+                8,                      /* number of data bytes */
+                0);                     /* synchronous message flag bit */
+
+        SDO_C->COB_IDClientToServerPrev = idCtoS;
+        SDO_C->COB_IDServerToClientPrev = idStoC;
+
+        if (ret != CO_ERROR_NO || SDO_C->CANtxBuff == NULL) {
+            return CO_SDO_RT_wrongArguments;
+        }
+    }
+
+    return CO_SDO_RT_ok_communicationEnd;
+}
+
+
+/******************************************************************************
+ * DOWNLOAD                                                                   *
+ ******************************************************************************/
+CO_SDO_return_t CO_SDOclientDownloadInitiate(CO_SDOclient_t *SDO_C,
+                                             uint16_t index,
+                                             uint8_t subIndex,
+                                             size_t sizeIndicated,
+                                             uint16_t SDOtimeoutTime_ms,
+                                             bool_t blockEnable)
+{
+    /* verify parameters */
+    if (SDO_C == NULL) {
+        return CO_SDO_RT_wrongArguments;
+    }
+
+    /* save parameters */
+    SDO_C->index = index;
+    SDO_C->subIndex = subIndex;
+    SDO_C->sizeInd = sizeIndicated;
+    SDO_C->sizeTran = 0;
+    SDO_C->finished = false;
+    SDO_C->SDOtimeoutTime_us = (uint32_t)SDOtimeoutTime_ms * 1000;
+    SDO_C->timeoutTimer = 0;
+    CO_fifo_reset(&SDO_C->bufFifo);
+
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_LOCAL
+    /* if node-ID of the SDO server is the same as node-ID of this node, then
+     * transfer data within this node */
+    if (SDO_C->SDO != NULL
+        && SDO_C->SDOClientPar->nodeIDOfTheSDOServer == SDO_C->SDO->nodeId
+    ) {
+        SDO_C->state = CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER;
+    }
+    else
+#endif
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
+    if (blockEnable && (sizeIndicated == 0 ||
+                        sizeIndicated > CO_CONFIG_SDO_CLI_PST)
+    ) {
+        SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ;
+    }
+    else
+#endif
+    {
+        SDO_C->state = CO_SDO_ST_DOWNLOAD_INITIATE_REQ;
+    }
+
+    CO_FLAG_CLEAR(SDO_C->CANrxNew);
+
+    return CO_SDO_RT_ok_communicationEnd;
+}
+
+
+void CO_SDOclientDownloadInitiateSize(CO_SDOclient_t *SDO_C,
+                                      size_t sizeIndicated)
+{
+    if (SDO_C != NULL) {
+        SDO_C->sizeInd = sizeIndicated;
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
+        if (SDO_C->state == CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ
+            && sizeIndicated > 0 && sizeIndicated <= CO_CONFIG_SDO_CLI_PST
+        ) {
+            SDO_C->state = CO_SDO_ST_DOWNLOAD_INITIATE_REQ;
+        }
+#endif
+    }
+}
+
+
+/******************************************************************************/
+size_t CO_SDOclientDownloadBufWrite(CO_SDOclient_t *SDO_C,
+                                    const char *buf,
+                                    size_t count)
+{
+    size_t ret = 0;
+    if (SDO_C != NULL && buf != NULL) {
+        ret = CO_fifo_write(&SDO_C->bufFifo, buf, count, NULL);
+    }
+    return ret;
+}
+
+
+/******************************************************************************/
+CO_SDO_return_t CO_SDOclientDownload(CO_SDOclient_t *SDO_C,
+                                     uint32_t timeDifference_us,
+                                     bool_t abort,
+                                     bool_t bufferPartial,
+                                     CO_SDO_abortCode_t *SDOabortCode,
+                                     size_t *sizeTransferred,
+                                     uint32_t *timerNext_us)
+{
+    (void)timerNext_us; (void) bufferPartial; /* may be unused */
+
+    CO_SDO_return_t ret = CO_SDO_RT_waitingResponse;
+    CO_SDO_abortCode_t abortCode = CO_SDO_AB_NONE;
+
+    if (SDO_C == NULL) {
+        abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
+        ret = CO_SDO_RT_wrongArguments;
+    }
+    else if (SDO_C->state == CO_SDO_ST_IDLE) {
+        ret = CO_SDO_RT_ok_communicationEnd;
+    }
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_LOCAL
+    /* Transfer data locally **************************************************/
+    else if (SDO_C->state == CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER && !abort) {
+        if (SDO_C->SDO->state != CO_SDO_ST_IDLE) {
+            abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
+            ret = CO_SDO_RT_endedWithClientAbort;
+        }
+        else {
+            /* Max data size is limited to fifo size (for now). */
+            size_t count = CO_fifo_getOccupied(&SDO_C->bufFifo);
+
+            if (SDO_C->sizeInd > 0 && SDO_C->sizeInd != count) {
+                abortCode = CO_SDO_AB_TYPE_MISMATCH;
+                ret = CO_SDO_RT_endedWithClientAbort;
+            }
+            else {
+                /* init ODF_arg */
+                abortCode = CO_SDO_initTransfer(SDO_C->SDO, SDO_C->index,
+                                                SDO_C->subIndex);
+                if (abortCode == CO_SDO_AB_NONE) {
+                    /* set buffer and write data to the Object dictionary */
+                    SDO_C->SDO->ODF_arg.data = (uint8_t *)SDO_C->buf;
+                    abortCode = CO_SDO_writeOD(SDO_C->SDO, count);
+                }
+                if (abortCode == CO_SDO_AB_NONE) {
+                    SDO_C->sizeTran = count;
+                    ret = CO_SDO_RT_ok_communicationEnd;
+                }
+                else {
+                    ret = CO_SDO_RT_endedWithServerAbort;
+                }
+            }
+        }
+        SDO_C->state = CO_SDO_ST_IDLE;
+    }
+#endif /* CO_CONFIG_SDO_CLI_LOCAL */
+    /* CAN data received ******************************************************/
+    else if (CO_FLAG_READ(SDO_C->CANrxNew)) {
+        /* is SDO abort */
+        if (SDO_C->CANrxData[0] == 0x80) {
+            uint32_t code;
+            memcpy(&code, &SDO_C->CANrxData[4], sizeof(code));
+            abortCode = (CO_SDO_abortCode_t)CO_SWAP_32(code);
+            SDO_C->state = CO_SDO_ST_IDLE;
+            ret = CO_SDO_RT_endedWithServerAbort;
+        }
+        else if (abort) {
+            abortCode = (SDOabortCode != NULL)
+                      ? *SDOabortCode : CO_SDO_AB_DEVICE_INCOMPAT;
+            SDO_C->state = CO_SDO_ST_ABORT;
+        }
+        else switch (SDO_C->state) {
+            case CO_SDO_ST_DOWNLOAD_INITIATE_RSP: {
+                if (SDO_C->CANrxData[0] == 0x60) {
+                    /* verify index and subindex */
+                    uint16_t index;
+                    uint8_t subindex;
+                    index = ((uint16_t) SDO_C->CANrxData[2]) << 8;
+                    index |= SDO_C->CANrxData[1];
+                    subindex = SDO_C->CANrxData[3];
+                    if (index != SDO_C->index || subindex != SDO_C->subIndex) {
+                        abortCode = CO_SDO_AB_PRAM_INCOMPAT;
+                        SDO_C->state = CO_SDO_ST_ABORT;
+                        break;
+                    }
+
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED
+                    if (SDO_C->finished) {
+                        /* expedited transfer */
+                        SDO_C->state = CO_SDO_ST_IDLE;
+                        ret = CO_SDO_RT_ok_communicationEnd;
+                    }
+                    else {
+                        /* segmented transfer - prepare the first segment */
+                        SDO_C->toggle = 0x00;
+                        SDO_C->state = CO_SDO_ST_DOWNLOAD_SEGMENT_REQ;
+                    }
+#else
+                    /* expedited transfer */
+                    SDO_C->state = CO_SDO_ST_IDLE;
+                    ret = CO_SDO_RT_ok_communicationEnd;
+#endif
+                }
+                else {
+                    abortCode = CO_SDO_AB_CMD;
+                    SDO_C->state = CO_SDO_ST_ABORT;
+                }
+                break;
+            }
+
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED
+            case CO_SDO_ST_DOWNLOAD_SEGMENT_RSP: {
+                if ((SDO_C->CANrxData[0] & 0xEF) == 0x20) {
+                    /* verify and alternate toggle bit */
+                    uint8_t toggle = SDO_C->CANrxData[0] & 0x10;
+                    if (toggle != SDO_C->toggle) {
+                        abortCode = CO_SDO_AB_TOGGLE_BIT;
+                        SDO_C->state = CO_SDO_ST_ABORT;
+                        break;
+                    }
+                    SDO_C->toggle = (toggle == 0x00) ? 0x10 : 0x00;
+
+                    /* is end of transfer? */
+                    if (SDO_C->finished) {
+                        SDO_C->state = CO_SDO_ST_IDLE;
+                        ret = CO_SDO_RT_ok_communicationEnd;
+                    }
+                    else {
+                        SDO_C->state = CO_SDO_ST_DOWNLOAD_SEGMENT_REQ;
+                    }
+                }
+                else {
+                    abortCode = CO_SDO_AB_CMD;
+                    SDO_C->state = CO_SDO_ST_ABORT;
+                }
+                break;
+            }
+#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED */
+
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
+            case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP: {
+                if ((SDO_C->CANrxData[0] & 0xFB) == 0xA0) {
+                    /* verify index and subindex */
+                    uint16_t index;
+                    uint8_t subindex;
+                    index = ((uint16_t) SDO_C->CANrxData[2]) << 8;
+                    index |= SDO_C->CANrxData[1];
+                    subindex = SDO_C->CANrxData[3];
+                    if (index != SDO_C->index || subindex != SDO_C->subIndex) {
+                        abortCode = CO_SDO_AB_PRAM_INCOMPAT;
+                        SDO_C->state = CO_SDO_ST_ABORT;
+                        break;
+                    }
+
+                    SDO_C->block_crc = 0;
+                    SDO_C->block_blksize = SDO_C->CANrxData[4];
+                    if (SDO_C->block_blksize < 1 || SDO_C->block_blksize > 127)
+                        SDO_C->block_blksize = 127;
+                    SDO_C->block_seqno = 0;
+                    CO_fifo_altBegin(&SDO_C->bufFifo, 0);
+                    SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ;
+                }
+                else {
+                    abortCode = CO_SDO_AB_CMD;
+                    SDO_C->state = CO_SDO_ST_ABORT;
+                }
+                break;
+            }
+
+            case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ:
+            case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP: {
+                if (SDO_C->CANrxData[0] == 0xA2) {
+                    /* check number of segments */
+                    if (SDO_C->CANrxData[1] < SDO_C->block_seqno) {
+                        /* NOT all segments transferred successfully.
+                         * Re-transmit data after erroneous segment. */
+                        CO_fifo_altBegin(&SDO_C->bufFifo,
+                                         (size_t)SDO_C->CANrxData[1] * 7);
+                        SDO_C->finished = false;
+                    }
+                    else if (SDO_C->CANrxData[1] > SDO_C->block_seqno) {
+                        /* something strange from server, break transmission */
+                        abortCode = CO_SDO_AB_CMD;
+                        SDO_C->state = CO_SDO_ST_ABORT;
+                        break;
+                    }
+
+                    /* confirm successfully transmitted data */
+                    CO_fifo_altFinish(&SDO_C->bufFifo, &SDO_C->block_crc);
+
+                    if (SDO_C->finished) {
+                        SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_END_REQ;
+                    } else {
+                        SDO_C->block_blksize = SDO_C->CANrxData[2];
+                        SDO_C->block_seqno = 0;
+                        CO_fifo_altBegin(&SDO_C->bufFifo, 0);
+                        SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ;
+                    }
+                }
+                else {
+                    abortCode = CO_SDO_AB_CMD;
+                    SDO_C->state = CO_SDO_ST_ABORT;
+                }
+                break;
+            }
+
+            case CO_SDO_ST_DOWNLOAD_BLK_END_RSP: {
+                if (SDO_C->CANrxData[0] == 0xA1) {
+                    /*  SDO block download successfully transferred */
+                    SDO_C->state = CO_SDO_ST_IDLE;
+                    ret = CO_SDO_RT_ok_communicationEnd;
+                }
+                else {
+                    abortCode = CO_SDO_AB_CMD;
+                    SDO_C->state = CO_SDO_ST_ABORT;
+                }
+                break;
+            }
+#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK */
+
+            default: {
+                abortCode = CO_SDO_AB_CMD;
+                SDO_C->state = CO_SDO_ST_ABORT;
+                break;
+            }
+        }
+        SDO_C->timeoutTimer = 0;
+        timeDifference_us = 0;
+        CO_FLAG_CLEAR(SDO_C->CANrxNew);
+    }
+    else if (abort) {
+        abortCode = (SDOabortCode != NULL)
+                  ? *SDOabortCode : CO_SDO_AB_DEVICE_INCOMPAT;
+        SDO_C->state = CO_SDO_ST_ABORT;
+    }
+
+    /* Timeout timers *********************************************************/
+    if (ret == CO_SDO_RT_waitingResponse) {
+        if (SDO_C->timeoutTimer < SDO_C->SDOtimeoutTime_us) {
+            SDO_C->timeoutTimer += timeDifference_us;
+        }
+        if (SDO_C->timeoutTimer >= SDO_C->SDOtimeoutTime_us) {
+            abortCode = CO_SDO_AB_TIMEOUT;
+            SDO_C->state = CO_SDO_ST_ABORT;
+        }
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_TIMERNEXT
+        else if (timerNext_us != NULL) {
+            /* check again after timeout time elapsed */
+            uint32_t diff = SDO_C->SDOtimeoutTime_us - SDO_C->timeoutTimer;
+            if (*timerNext_us > diff) {
+                *timerNext_us = diff;
+            }
+        }
+#endif
+        if (SDO_C->CANtxBuff->bufferFull) {
+            ret = CO_SDO_RT_transmittBufferFull;
+        }
+    }
+
+    /* Transmit CAN data ******************************************************/
+    if (ret == CO_SDO_RT_waitingResponse) {
+        size_t count;
+        memset((void *)&SDO_C->CANtxBuff->data[0], 0, 8);
+
+        switch (SDO_C->state) {
+        case CO_SDO_ST_DOWNLOAD_INITIATE_REQ: {
+            SDO_C->CANtxBuff->data[0] = 0x20;
+            SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->index;
+            SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->index >> 8);
+            SDO_C->CANtxBuff->data[3] = SDO_C->subIndex;
+
+            /* get count of data bytes to transfer */
+            count = CO_fifo_getOccupied(&SDO_C->bufFifo);
+
+            /* is expedited transfer, <= 4bytes of data */
+            if ((SDO_C->sizeInd == 0 && count <= 4)
+                || (SDO_C->sizeInd > 0 && SDO_C->sizeInd <= 4)
+            ) {
+                SDO_C->CANtxBuff->data[0] |= 0x02;
+
+                /* verify length, indicate data size */
+                if (count == 0 || (SDO_C->sizeInd > 0 &&
+                                   SDO_C->sizeInd != count)
+                ) {
+                    SDO_C->state = CO_SDO_ST_IDLE;
+                    abortCode = CO_SDO_AB_TYPE_MISMATCH;
+                    ret = CO_SDO_RT_endedWithClientAbort;
+                    break;
+                }
+                if (SDO_C->sizeInd > 0) {
+                    SDO_C->CANtxBuff->data[0] |= 0x01 | ((4 - count) << 2);
+                }
+
+                /* copy data */
+                CO_fifo_read(&SDO_C->bufFifo,
+                             (char *)&SDO_C->CANtxBuff->data[4], count, NULL);
+                SDO_C->sizeTran = count;
+                SDO_C->finished = true;
+            }
+            else {
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED
+                /* segmented transfer, indicate data size */
+                if (SDO_C->sizeInd > 0) {
+                    uint32_t size = CO_SWAP_32(SDO_C->sizeInd);
+                    SDO_C->CANtxBuff->data[0] |= 0x01;
+                    memcpy(&SDO_C->CANtxBuff->data[4], &size, sizeof(size));
+                }
+#else
+                SDO_C->state = CO_SDO_ST_IDLE;
+                abortCode = CO_SDO_AB_UNSUPPORTED_ACCESS;
+                ret = CO_SDO_RT_endedWithClientAbort;
+                break;
+#endif
+            }
+
+            /* reset timeout timer and send message */
+            SDO_C->timeoutTimer = 0;
+            CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
+            SDO_C->state = CO_SDO_ST_DOWNLOAD_INITIATE_RSP;
+            break;
+        }
+
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED
+        case CO_SDO_ST_DOWNLOAD_SEGMENT_REQ: {
+            /* fill data bytes */
+            count = CO_fifo_read(&SDO_C->bufFifo,
+                                 (char *)&SDO_C->CANtxBuff->data[1],
+                                 7, NULL);
+
+            /* verify if sizeTran is too large */
+            SDO_C->sizeTran += count;
+            if (SDO_C->sizeInd > 0 && SDO_C->sizeTran > SDO_C->sizeInd) {
+                SDO_C->sizeTran -= count;
+                abortCode = CO_SDO_AB_DATA_LONG;
+                SDO_C->state = CO_SDO_ST_ABORT;
+                break;
+            }
+
+            /* SDO command specifier */
+            SDO_C->CANtxBuff->data[0] = SDO_C->toggle | ((7 - count) << 1);
+
+            /* is end of transfer? Verify also sizeTran */
+            if (CO_fifo_getOccupied(&SDO_C->bufFifo) == 0 && !bufferPartial) {
+                if (SDO_C->sizeInd > 0 && SDO_C->sizeTran < SDO_C->sizeInd) {
+                    abortCode = CO_SDO_AB_DATA_SHORT;
+                    SDO_C->state = CO_SDO_ST_ABORT;
+                    break;
+                }
+                SDO_C->CANtxBuff->data[0] |= 0x01;
+                SDO_C->finished = true;
+            }
+
+            /* reset timeout timer and send message */
+            SDO_C->timeoutTimer = 0;
+            CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
+            SDO_C->state = CO_SDO_ST_DOWNLOAD_SEGMENT_RSP;
+            break;
+        }
+#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED */
+
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
+        case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ: {
+            SDO_C->CANtxBuff->data[0] = 0xC4;
+            SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->index;
+            SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->index >> 8);
+            SDO_C->CANtxBuff->data[3] = SDO_C->subIndex;
+
+            /* indicate data size */
+            if (SDO_C->sizeInd > 0) {
+                uint32_t size = CO_SWAP_32(SDO_C->sizeInd);
+                SDO_C->CANtxBuff->data[0] |= 0x02;
+                memcpy(&SDO_C->CANtxBuff->data[4], &size, sizeof(size));
+            }
+
+            /* reset timeout timer and send message */
+            SDO_C->timeoutTimer = 0;
+            CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
+            SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP;
+            break;
+        }
+
+        case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ: {
+            if (CO_fifo_altGetOccupied(&SDO_C->bufFifo) < 7 && bufferPartial) {
+                /* wait until data are refilled */
+                break;
+            }
+            SDO_C->CANtxBuff->data[0] = ++SDO_C->block_seqno;
+
+            /* get up to 7 data bytes */
+            count = CO_fifo_altRead(&SDO_C->bufFifo,
+                                    (char *)&SDO_C->CANtxBuff->data[1], 7);
+            SDO_C->block_noData = 7 - count;
+
+            /* verify if sizeTran is too large */
+            SDO_C->sizeTran += count;
+            if (SDO_C->sizeInd > 0 && SDO_C->sizeTran > SDO_C->sizeInd) {
+                SDO_C->sizeTran -= count;
+                abortCode = CO_SDO_AB_DATA_LONG;
+                SDO_C->state = CO_SDO_ST_ABORT;
+                break;
+            }
+
+            /* is end of transfer? Verify also sizeTran */
+            if (CO_fifo_altGetOccupied(&SDO_C->bufFifo) == 0 && !bufferPartial){
+                if (SDO_C->sizeInd > 0 && SDO_C->sizeTran < SDO_C->sizeInd) {
+                    abortCode = CO_SDO_AB_DATA_SHORT;
+                    SDO_C->state = CO_SDO_ST_ABORT;
+                    break;
+                }
+                SDO_C->CANtxBuff->data[0] |= 0x80;
+                SDO_C->finished = true;
+                SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP;
+            }
+            /* are all segments in current block transferred? */
+            else if (SDO_C->block_seqno >= SDO_C->block_blksize) {
+                SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP;
+            }
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_TIMERNEXT
+            else {
+                /* Inform OS to call this function again without delay. */
+                if (timerNext_us != NULL) {
+                    *timerNext_us = 0;
+                }
+            }
+#endif
+            /* reset timeout timer and send message */
+            SDO_C->timeoutTimer = 0;
+            CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
+            break;
+        }
+
+        case CO_SDO_ST_DOWNLOAD_BLK_END_REQ: {
+            SDO_C->CANtxBuff->data[0] = 0xC1 | (SDO_C->block_noData << 2);
+            SDO_C->CANtxBuff->data[1] = (uint8_t) SDO_C->block_crc;
+            SDO_C->CANtxBuff->data[2] = (uint8_t) (SDO_C->block_crc >> 8);
+
+            /* reset timeout timer and send message */
+            SDO_C->timeoutTimer = 0;
+            CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
+            SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_END_RSP;
+            break;
+        }
+#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK */
+
+        default: {
+            break;
+        }
+        }
+    }
+
+    if (ret == CO_SDO_RT_waitingResponse) {
+        if (SDO_C->state == CO_SDO_ST_ABORT) {
+            uint32_t code = CO_SWAP_32((uint32_t)abortCode);
+            /* Send SDO abort message */
+            SDO_C->CANtxBuff->data[0] = 0x80;
+            SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->index;
+            SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->index >> 8);
+            SDO_C->CANtxBuff->data[3] = SDO_C->subIndex;
+
+            memcpy(&SDO_C->CANtxBuff->data[4], &code, sizeof(code));
+            CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
+            SDO_C->state = CO_SDO_ST_IDLE;
+            ret = CO_SDO_RT_endedWithClientAbort;
+        }
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
+        else if (SDO_C->state == CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ) {
+            ret = CO_SDO_RT_blockDownldInProgress;
+        }
+#endif
+    }
+
+    if (sizeTransferred != NULL) {
+        *sizeTransferred = SDO_C->sizeTran;
+    }
+    if (SDOabortCode != NULL) {
+        *SDOabortCode = abortCode;
+    }
+
+    return ret;
+}
+
+
+/******************************************************************************
+ * UPLOAD                                                                     *
+ ******************************************************************************/
+CO_SDO_return_t CO_SDOclientUploadInitiate(CO_SDOclient_t *SDO_C,
+                                           uint16_t index,
+                                           uint8_t subIndex,
+                                           uint16_t SDOtimeoutTime_ms,
+                                           bool_t blockEnable)
+{
+    /* verify parameters */
+    if (SDO_C == NULL) {
+        return CO_SDO_RT_wrongArguments;
+    }
+
+    /* save parameters */
+    SDO_C->index = index;
+    SDO_C->subIndex = subIndex;
+    SDO_C->sizeInd = 0;
+    SDO_C->sizeTran = 0;
+    SDO_C->finished = false;
+    CO_fifo_reset(&SDO_C->bufFifo);
+    SDO_C->SDOtimeoutTime_us = (uint32_t)SDOtimeoutTime_ms * 1000;
+    SDO_C->timeoutTimer = 0;
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
+    SDO_C->block_SDOtimeoutTime_us = (uint32_t)SDOtimeoutTime_ms * 700;
+#endif
+
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_LOCAL
+    /* if node-ID of the SDO server is the same as node-ID of this node, then
+     * transfer data within this node */
+    if (SDO_C->SDO != NULL
+        && SDO_C->SDOClientPar->nodeIDOfTheSDOServer == SDO_C->SDO->nodeId
+    ) {
+        SDO_C->state = CO_SDO_ST_UPLOAD_LOCAL_TRANSFER;
+    }
+    else
+#endif
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
+    if (blockEnable) {
+        SDO_C->state = CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ;
+    }
+    else
+#endif
+    {
+        SDO_C->state = CO_SDO_ST_UPLOAD_INITIATE_REQ;
+    }
+
+    CO_FLAG_CLEAR(SDO_C->CANrxNew);
+
+    return CO_SDO_RT_ok_communicationEnd;
+}
+
+
+/******************************************************************************/
+CO_SDO_return_t CO_SDOclientUpload(CO_SDOclient_t *SDO_C,
+                                   uint32_t timeDifference_us,
+                                   bool_t abort,
+                                   CO_SDO_abortCode_t *SDOabortCode,
+                                   size_t *sizeIndicated,
+                                   size_t *sizeTransferred,
+                                   uint32_t *timerNext_us)
+{
+    (void)timerNext_us; /* may be unused */
+
+    CO_SDO_return_t ret = CO_SDO_RT_waitingResponse;
+    CO_SDO_abortCode_t abortCode = CO_SDO_AB_NONE;
+
+    if (SDO_C == NULL) {
+        abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
+        ret = CO_SDO_RT_wrongArguments;
+    }
+    else if (SDO_C->state == CO_SDO_ST_IDLE) {
+        ret = CO_SDO_RT_ok_communicationEnd;
+    }
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_LOCAL
+    /* Transfer data locally **************************************************/
+    else if (SDO_C->state == CO_SDO_ST_UPLOAD_LOCAL_TRANSFER && !abort) {
+        if (SDO_C->SDO->state != 0) {
+            abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
+            ret = CO_SDO_RT_endedWithClientAbort;
+        }
+        else {
+            /* Max data size is limited to fifo size (for now). */
+            size_t count = CO_fifo_getSpace(&SDO_C->bufFifo);
+
+            /* init ODF_arg */
+            abortCode = CO_SDO_initTransfer(SDO_C->SDO, SDO_C->index,
+                                            SDO_C->subIndex);
+            if (abortCode == CO_SDO_AB_NONE) {
+                /* set buffer and read data from the Object dictionary */
+                SDO_C->SDO->ODF_arg.data = (uint8_t *)SDO_C->buf;
+                if (SDO_C->SDO->ODF_arg.ODdataStorage == 0) {
+                    /* set length if domain */
+                    SDO_C->SDO->ODF_arg.dataLength = count;
+                }
+                abortCode = CO_SDO_readOD(SDO_C->SDO, count);
+            }
+            if (abortCode == CO_SDO_AB_NONE) {
+                /* is SDO buffer too small */
+                if (SDO_C->SDO->ODF_arg.lastSegment == 0) {
+                    abortCode = CO_SDO_AB_OUT_OF_MEM;  /* Out of memory */
+                    ret = CO_SDO_RT_endedWithServerAbort;
+                }
+                else {
+                    SDO_C->sizeTran = (size_t)SDO_C->SDO->ODF_arg.dataLength;
+                    /* fifo was written directly, indicate data size manually */
+                    SDO_C->bufFifo.writePtr = SDO_C->sizeTran;
+                    ret = CO_SDO_RT_ok_communicationEnd;
+                }
+            }
+            else {
+                ret = CO_SDO_RT_endedWithServerAbort;
+            }
+        }
+        SDO_C->state = CO_SDO_ST_IDLE;
+    }
+#endif /* CO_CONFIG_SDO_CLI_LOCAL */
+    /* CAN data received ******************************************************/
+    else if (CO_FLAG_READ(SDO_C->CANrxNew)) {
+        /* is SDO abort */
+        if (SDO_C->CANrxData[0] == 0x80) {
+            uint32_t code;
+            memcpy(&code, &SDO_C->CANrxData[4], sizeof(code));
+            abortCode = (CO_SDO_abortCode_t)CO_SWAP_32(code);
+            SDO_C->state = CO_SDO_ST_IDLE;
+            ret = CO_SDO_RT_endedWithServerAbort;
+        }
+        else if (abort) {
+            abortCode = (SDOabortCode != NULL)
+                      ? *SDOabortCode : CO_SDO_AB_DEVICE_INCOMPAT;
+            SDO_C->state = CO_SDO_ST_ABORT;
+        }
+        else switch (SDO_C->state) {
+            case CO_SDO_ST_UPLOAD_INITIATE_RSP: {
+                if ((SDO_C->CANrxData[0] & 0xF0) == 0x40) {
+                    /* verify index and subindex */
+                    uint16_t index;
+                    uint8_t subindex;
+                    index = ((uint16_t) SDO_C->CANrxData[2]) << 8;
+                    index |= SDO_C->CANrxData[1];
+                    subindex = SDO_C->CANrxData[3];
+                    if (index != SDO_C->index || subindex != SDO_C->subIndex) {
+                        abortCode = CO_SDO_AB_PRAM_INCOMPAT;
+                        SDO_C->state = CO_SDO_ST_ABORT;
+                        break;
+                    }
+
+                    if (SDO_C->CANrxData[0] & 0x02) {
+                        /* Expedited transfer */
+                        size_t count = 4;
+                        /* is size indicated? */
+                        if (SDO_C->CANrxData[0] & 0x01) {
+                            count -= (SDO_C->CANrxData[0] >> 2) & 0x03;
+                        }
+                        /* copy data, indicate size and finish */
+                        CO_fifo_write(&SDO_C->bufFifo,
+                                      (const char *)&SDO_C->CANrxData[4],
+                                      count, NULL);
+                        SDO_C->sizeTran = count;
+                        SDO_C->state = CO_SDO_ST_IDLE;
+                        ret = CO_SDO_RT_ok_communicationEnd;
+                    }
+                    else {
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED
+                        /* segmented transfer, is size indicated? */
+                        if (SDO_C->CANrxData[0] & 0x01) {
+                            uint32_t size;
+                            memcpy(&size, &SDO_C->CANrxData[4], sizeof(size));
+                            SDO_C->sizeInd = CO_SWAP_32(size);
+                        }
+                        SDO_C->toggle = 0x00;
+                        SDO_C->state = CO_SDO_ST_UPLOAD_SEGMENT_REQ;
+#else
+                        abortCode = CO_SDO_AB_UNSUPPORTED_ACCESS;
+                        SDO_C->state = CO_SDO_ST_ABORT;
+#endif
+                    }
+                }
+                else {
+                    abortCode = CO_SDO_AB_CMD;
+                    SDO_C->state = CO_SDO_ST_ABORT;
+                }
+                break;
+            }
+
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED
+            case CO_SDO_ST_UPLOAD_SEGMENT_RSP: {
+                if ((SDO_C->CANrxData[0] & 0xE0) == 0x00) {
+                    size_t count, countWr;
+
+                    /* verify and alternate toggle bit */
+                    uint8_t toggle = SDO_C->CANrxData[0] & 0x10;
+                    if (toggle != SDO_C->toggle) {
+                        abortCode = CO_SDO_AB_TOGGLE_BIT;
+                        SDO_C->state = CO_SDO_ST_ABORT;
+                        break;
+                    }
+                    SDO_C->toggle = (toggle == 0x00) ? 0x10 : 0x00;
+
+                    /* get data size and write data to the buffer */
+                    count = 7 - ((SDO_C->CANrxData[0] >> 1) & 0x07);
+                    countWr = CO_fifo_write(&SDO_C->bufFifo,
+                                            (const char *)&SDO_C->CANrxData[1],
+                                            count, NULL);
+                    SDO_C->sizeTran += countWr;
+
+                    /* verify, if there was not enough space in fifo buffer */
+                    if (countWr != count) {
+                        abortCode = CO_SDO_AB_OUT_OF_MEM;
+                        SDO_C->state = CO_SDO_ST_ABORT;
+                        break;
+                    }
+
+                    /* verify if size of data uploaded is too large */
+                    if (SDO_C->sizeInd > 0
+                        && SDO_C->sizeTran > SDO_C->sizeInd
+                    ) {
+                        abortCode = CO_SDO_AB_DATA_LONG;
+                        SDO_C->state = CO_SDO_ST_ABORT;
+                        break;
+                    }
+
+                    /* If no more segments to be upload, finish */
+                    if (SDO_C->CANrxData[0] & 0x01) {
+                        /* verify size of data uploaded */
+                        if (SDO_C->sizeInd > 0
+                            && SDO_C->sizeTran < SDO_C->sizeInd
+                        ) {
+                            abortCode = CO_SDO_AB_DATA_SHORT;
+                            SDO_C->state = CO_SDO_ST_ABORT;
+                        } else {
+                            SDO_C->state = CO_SDO_ST_IDLE;
+                            ret = CO_SDO_RT_ok_communicationEnd;
+                        }
+                    }
+                    else {
+                        SDO_C->state = CO_SDO_ST_UPLOAD_SEGMENT_REQ;
+                    }
+                }
+                else {
+                    abortCode = CO_SDO_AB_CMD;
+                    SDO_C->state = CO_SDO_ST_ABORT;
+                }
+                break;
+            }
+#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED */
+
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
+            case CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP: {
+                if ((SDO_C->CANrxData[0] & 0xF9) == 0xC0) {
+                    uint16_t index;
+                    uint8_t subindex;
+
+                    /* get server CRC support info and data size */
+                    if ((SDO_C->CANrxData[0] & 0x04) != 0) {
+                        SDO_C->block_crcEnabled = true;
+                    } else {
+                        SDO_C->block_crcEnabled = false;
+                    }
+                    if (SDO_C->CANrxData[0] & 0x02) {
+                        uint32_t size;
+                        memcpy(&size, &SDO_C->CANrxData[4], sizeof(size));
+                        SDO_C->sizeInd = CO_SWAP_32(size);
+                    }
+
+                    /* verify index and subindex */
+                    index = ((uint16_t) SDO_C->CANrxData[2]) << 8;
+                    index |= SDO_C->CANrxData[1];
+                    subindex = SDO_C->CANrxData[3];
+                    if (index != SDO_C->index || subindex != SDO_C->subIndex) {
+                        abortCode = CO_SDO_AB_PRAM_INCOMPAT;
+                        SDO_C->state = CO_SDO_ST_ABORT;
+                    }
+                    else {
+                        SDO_C->state = CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2;
+                    }
+                }
+                /* switch to regular transfer, CO_SDO_ST_UPLOAD_INITIATE_RSP */
+                else if ((SDO_C->CANrxData[0] & 0xF0) == 0x40) {
+                    /* verify index and subindex */
+                    uint16_t index;
+                    uint8_t subindex;
+                    index = ((uint16_t) SDO_C->CANrxData[2]) << 8;
+                    index |= SDO_C->CANrxData[1];
+                    subindex = SDO_C->CANrxData[3];
+                    if (index != SDO_C->index || subindex != SDO_C->subIndex) {
+                        abortCode = CO_SDO_AB_PRAM_INCOMPAT;
+                        SDO_C->state = CO_SDO_ST_ABORT;
+                        break;
+                    }
+
+                    if (SDO_C->CANrxData[0] & 0x02) {
+                        /* Expedited transfer */
+                        size_t count = 4;
+                        /* is size indicated? */
+                        if (SDO_C->CANrxData[0] & 0x01) {
+                            count -= (SDO_C->CANrxData[0] >> 2) & 0x03;
+                        }
+                        /* copy data, indicate size and finish */
+                        CO_fifo_write(&SDO_C->bufFifo,
+                                      (const char *)&SDO_C->CANrxData[4],
+                                      count, NULL);
+                        SDO_C->sizeTran = count;
+                        SDO_C->state = CO_SDO_ST_IDLE;
+                        ret = CO_SDO_RT_ok_communicationEnd;
+                    }
+                    else {
+                        /* segmented transfer, is size indicated? */
+                        if (SDO_C->CANrxData[0] & 0x01) {
+                            uint32_t size;
+                            memcpy(&size, &SDO_C->CANrxData[4], sizeof(size));
+                            SDO_C->sizeInd = CO_SWAP_32(size);
+                        }
+                        SDO_C->toggle = 0x00;
+                        SDO_C->state = CO_SDO_ST_UPLOAD_SEGMENT_REQ;
+                    }
+                }
+                else {
+                    abortCode = CO_SDO_AB_CMD;
+                    SDO_C->state = CO_SDO_ST_ABORT;
+                }
+                break;
+            }
+
+            case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ: {
+                /* data are copied directly in the receive function */
+                break;
+            }
+
+            case CO_SDO_ST_UPLOAD_BLK_END_SREQ: {
+                if ((SDO_C->CANrxData[0] & 0xE3) == 0xC1) {
+                    /* Get number of data bytes in last segment, that do not
+                     * contain data. Then copy remaining data into fifo */
+                    uint8_t noData = ((SDO_C->CANrxData[0] >> 2) & 0x07);
+                    CO_fifo_write(&SDO_C->bufFifo,
+                                  (const char *)&SDO_C->block_dataUploadLast[0],
+                                  7 - noData,
+                                  &SDO_C->block_crc);
+                    SDO_C->sizeTran += 7 - noData;
+
+                    /* verify length */
+                    if (SDO_C->sizeInd > 0
+                        && SDO_C->sizeTran != SDO_C->sizeInd
+                    ) {
+                        abortCode = (SDO_C->sizeTran > SDO_C->sizeInd) ?
+                                    CO_SDO_AB_DATA_LONG : CO_SDO_AB_DATA_SHORT;
+                        SDO_C->state = CO_SDO_ST_ABORT;
+                        break;
+                    }
+
+                    /* verify CRC */
+                    if (SDO_C->block_crcEnabled) {
+                        uint16_t crcServer;
+                        crcServer = ((uint16_t) SDO_C->CANrxData[2]) << 8;
+                        crcServer |= SDO_C->CANrxData[1];
+                        if (crcServer != SDO_C->block_crc) {
+                            abortCode = CO_SDO_AB_CRC;
+                            SDO_C->state = CO_SDO_ST_ABORT;
+                            break;
+                        }
+                    }
+                    SDO_C->state = CO_SDO_ST_UPLOAD_BLK_END_CRSP;
+                }
+                else {
+                    abortCode = CO_SDO_AB_CMD;
+                    SDO_C->state = CO_SDO_ST_ABORT;
+                }
+                break;
+            }
+#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK */
+
+            default: {
+                abortCode = CO_SDO_AB_CMD;
+                SDO_C->state = CO_SDO_ST_ABORT;
+                break;
+            }
+        }
+        SDO_C->timeoutTimer = 0;
+        timeDifference_us = 0;
+        CO_FLAG_CLEAR(SDO_C->CANrxNew);
+    }
+    else if (abort) {
+        abortCode = (SDOabortCode != NULL)
+                  ? *SDOabortCode : CO_SDO_AB_DEVICE_INCOMPAT;
+        SDO_C->state = CO_SDO_ST_ABORT;
+    }
+
+    /* Timeout timers *********************************************************/
+    if (ret == CO_SDO_RT_waitingResponse) {
+        if (SDO_C->timeoutTimer < SDO_C->SDOtimeoutTime_us) {
+            SDO_C->timeoutTimer += timeDifference_us;
+        }
+        if (SDO_C->timeoutTimer >= SDO_C->SDOtimeoutTime_us) {
+            if (SDO_C->state == CO_SDO_ST_UPLOAD_SEGMENT_REQ ||
+                SDO_C->state == CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP
+            ) {
+                /* application didn't empty buffer */
+                abortCode = CO_SDO_AB_GENERAL;
+            } else {
+                abortCode = CO_SDO_AB_TIMEOUT;
+            }
+            SDO_C->state = CO_SDO_ST_ABORT;
+        }
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_TIMERNEXT
+        else if (timerNext_us != NULL) {
+            /* check again after timeout time elapsed */
+            uint32_t diff = SDO_C->SDOtimeoutTime_us - SDO_C->timeoutTimer;
+            if (*timerNext_us > diff) {
+                *timerNext_us = diff;
+            }
+        }
+#endif
+
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
+        /* Timeout for sub-block reception */
+        if (SDO_C->state == CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ) {
+            if (SDO_C->block_timeoutTimer < SDO_C->block_SDOtimeoutTime_us) {
+                SDO_C->block_timeoutTimer += timeDifference_us;
+            }
+            if (SDO_C->block_timeoutTimer >= SDO_C->block_SDOtimeoutTime_us) {
+                /* SDO_C->state will change, processing will continue in this
+                 * thread. Make memory barrier here with CO_FLAG_CLEAR() call.*/
+                SDO_C->state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP;
+                CO_FLAG_CLEAR(SDO_C->CANrxNew);
+            }
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_TIMERNEXT
+            else if (timerNext_us != NULL) {
+                /* check again after timeout time elapsed */
+                uint32_t diff = SDO_C->block_SDOtimeoutTime_us -
+                                SDO_C->block_timeoutTimer;
+                if (*timerNext_us > diff) {
+                    *timerNext_us = diff;
+                }
+            }
+#endif
+        }
+#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK */
+
+        if (SDO_C->CANtxBuff->bufferFull) {
+            ret = CO_SDO_RT_transmittBufferFull;
+        }
+    }
+
+    /* Transmit CAN data ******************************************************/
+    if (ret == CO_SDO_RT_waitingResponse) {
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
+        size_t count;
+#endif
+        memset((void *)&SDO_C->CANtxBuff->data[0], 0, 8);
+
+        switch (SDO_C->state) {
+        case CO_SDO_ST_UPLOAD_INITIATE_REQ: {
+            SDO_C->CANtxBuff->data[0] = 0x40;
+            SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->index;
+            SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->index >> 8);
+            SDO_C->CANtxBuff->data[3] = SDO_C->subIndex;
+
+            /* reset timeout timer and send message */
+            SDO_C->timeoutTimer = 0;
+            CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
+            SDO_C->state = CO_SDO_ST_UPLOAD_INITIATE_RSP;
+            break;
+        }
+
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED
+        case CO_SDO_ST_UPLOAD_SEGMENT_REQ: {
+            /* verify, if there is enough space in data buffer */
+            if (CO_fifo_getSpace(&SDO_C->bufFifo) < 7) {
+                ret = CO_SDO_RT_uploadDataBufferFull;
+                break;
+            }
+            SDO_C->CANtxBuff->data[0] = 0x60 | SDO_C->toggle;
+
+            /* reset timeout timer and send message */
+            SDO_C->timeoutTimer = 0;
+            CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
+            SDO_C->state = CO_SDO_ST_UPLOAD_SEGMENT_RSP;
+            break;
+        }
+#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED */
+
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
+        case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ: {
+            SDO_C->CANtxBuff->data[0] = 0xA4;
+            SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->index;
+            SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->index >> 8);
+            SDO_C->CANtxBuff->data[3] = SDO_C->subIndex;
+
+            /* calculate number of block segments from free buffer space */
+            count = CO_fifo_getSpace(&SDO_C->bufFifo) / 7;
+            if (count > 127) {
+                count = 127;
+            }
+            else if (count == 0) {
+                abortCode = CO_SDO_AB_OUT_OF_MEM;
+                SDO_C->state = CO_SDO_ST_ABORT;
+                break;
+            }
+            SDO_C->block_blksize = (uint8_t)count;
+            SDO_C->CANtxBuff->data[4] = SDO_C->block_blksize;
+            SDO_C->CANtxBuff->data[5] = CO_CONFIG_SDO_CLI_PST;
+
+            /* reset timeout timer and send message */
+            SDO_C->timeoutTimer = 0;
+            CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
+            SDO_C->state = CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP;
+            break;
+        }
+
+        case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2: {
+            SDO_C->CANtxBuff->data[0] = 0xA3;
+
+            /* reset timeout timers, seqno and send message */
+            SDO_C->timeoutTimer = 0;
+            SDO_C->block_timeoutTimer = 0;
+            SDO_C->block_seqno = 0;
+            SDO_C->block_crc = 0;
+            /* Block segments will be received in different thread. Make memory
+             * barrier here with CO_FLAG_CLEAR() call. */
+            SDO_C->state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ;
+            CO_FLAG_CLEAR(SDO_C->CANrxNew);
+            CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
+            break;
+        }
+
+        case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP: {
+            SDO_C->CANtxBuff->data[0] = 0xA2;
+            SDO_C->CANtxBuff->data[1] = SDO_C->block_seqno;
+#ifdef CO_DEBUG_SDO_CLIENT
+            bool_t transferShort = SDO_C->block_seqno != SDO_C->block_blksize;
+            uint8_t seqnoStart = SDO_C->block_seqno;
+#endif
+
+            /* Is last segment? */
+            if (SDO_C->finished) {
+                SDO_C->state = CO_SDO_ST_UPLOAD_BLK_END_SREQ;
+            }
+            else {
+                /* verify if size of data uploaded is too large */
+                if (SDO_C->sizeInd > 0 && SDO_C->sizeTran > SDO_C->sizeInd) {
+                    abortCode = CO_SDO_AB_DATA_LONG;
+                    SDO_C->state = CO_SDO_ST_ABORT;
+                    break;
+                }
+
+                /* calculate number of block segments from free buffer space */
+                count = CO_fifo_getSpace(&SDO_C->bufFifo) / 7;
+                if (count >= 127) {
+                    count = 127;
+                }
+                else if (CO_fifo_getOccupied(&SDO_C->bufFifo) > 0) {
+                    /* application must empty data buffer first */
+                    ret = CO_SDO_RT_uploadDataBufferFull;
+#ifdef CO_DEBUG_SDO_CLIENT
+                    if (transferShort) {
+                        char msg[80];
+                        sprintf(msg,
+                                "sub-block, uploadDataBufferFull: sequno=%02X",
+                                seqnoStart);
+                        CO_DEBUG_SDO_CLIENT(msg);
+                    }
+#endif
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_TIMERNEXT
+                    /* Inform OS to call this function again without delay. */
+                    if (timerNext_us != NULL) {
+                        *timerNext_us = 0;
+                    }
+#endif
+                    break;
+                }
+                SDO_C->block_blksize = (uint8_t)count;
+                SDO_C->block_seqno = 0;
+                /* Block segments will be received in different thread. Make
+                 * memory barrier here with CO_FLAG_CLEAR() call. */
+                SDO_C->state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ;
+                CO_FLAG_CLEAR(SDO_C->CANrxNew);
+            }
+
+            SDO_C->CANtxBuff->data[2] = SDO_C->block_blksize;
+
+            /* reset block_timeoutTimer, but not SDO_C->timeoutTimer */
+            SDO_C->block_timeoutTimer = 0;
+            CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
+#ifdef CO_DEBUG_SDO_CLIENT
+            if (transferShort && !SDO_C->finished) {
+                char msg[80];
+                sprintf(msg,
+                        "sub-block restarted: sequnoPrev=%02X, blksize=%02X",
+                        seqnoStart, SDO_C->block_blksize);
+                CO_DEBUG_SDO_CLIENT(msg);
+            }
+#endif
+            break;
+        }
+
+        case CO_SDO_ST_UPLOAD_BLK_END_CRSP: {
+            SDO_C->CANtxBuff->data[0] = 0xA1;
+
+            CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
+            SDO_C->state = CO_SDO_ST_IDLE;
+            ret = CO_SDO_RT_ok_communicationEnd;
+            break;
+        }
+#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK */
+
+        default: {
+            break;
+        }
+        }
+    }
+
+    if (ret == CO_SDO_RT_waitingResponse) {
+        if (SDO_C->state == CO_SDO_ST_ABORT) {
+            uint32_t code = CO_SWAP_32((uint32_t)abortCode);
+            /* Send SDO abort message */
+            SDO_C->CANtxBuff->data[0] = 0x80;
+            SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->index;
+            SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->index >> 8);
+            SDO_C->CANtxBuff->data[3] = SDO_C->subIndex;
+
+            memcpy(&SDO_C->CANtxBuff->data[4], &code, sizeof(code));
+            CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
+            SDO_C->state = CO_SDO_ST_IDLE;
+            ret = CO_SDO_RT_endedWithClientAbort;
+        }
+#if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
+        else if (SDO_C->state == CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ) {
+            ret = CO_SDO_RT_blockUploadInProgress;
+        }
+#endif
+    }
+
+    if (sizeIndicated != NULL) {
+        *sizeIndicated = SDO_C->sizeInd;
+    }
+    if (sizeTransferred != NULL) {
+        *sizeTransferred = SDO_C->sizeTran;
+    }
+    if (SDOabortCode != NULL) {
+        *SDOabortCode = abortCode;
+    }
+
+    return ret;
+}
+
+
+/******************************************************************************/
+size_t CO_SDOclientUploadBufRead(CO_SDOclient_t *SDO_C,
+                                 char *buf,
+                                 size_t count)
+{
+    size_t ret = 0;
+    if (SDO_C != NULL && buf != NULL) {
+        ret = CO_fifo_read(&SDO_C->bufFifo, buf, count, NULL);
+    }
+    return ret;
+}
+
+
+/******************************************************************************/
+void CO_SDOclientClose(CO_SDOclient_t *SDO_C) {
+    if (SDO_C != NULL) {
+        SDO_C->state = CO_SDO_ST_IDLE;
+    }
+}
+
+#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_ENABLE */

controller_yy_app/middleware/CANopenNode/301/CO_SDOclient.h

@@ -0,0 +1,456 @@
+/**
+ * CANopen Service Data Object - client protocol.
+ *
+ * @file        CO_SDOclient.h
+ * @ingroup     CO_SDOclient
+ * @author      Janez Paternoster
+ * @author      Matej Severkar
+ * @copyright   2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CO_SDO_CLIENT_H
+#define CO_SDO_CLIENT_H
+
+#include "301/CO_driver.h"
+#include "301/CO_SDOserver.h"
+#include "301/CO_fifo.h"
+
+/* default configuration, see CO_config.h */
+#ifndef CO_CONFIG_SDO_CLI
+#define CO_CONFIG_SDO_CLI (0)
+#endif
+#ifndef CO_CONFIG_SDO_CLI_BUFFER_SIZE
+ #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
+  #define CO_CONFIG_SDO_CLI_BUFFER_SIZE 1000
+ #else
+  #define CO_CONFIG_SDO_CLI_BUFFER_SIZE 32
+ #endif
+#endif
+
+#if ((CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_ENABLE) || defined CO_DOXYGEN
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_SDOclient SDO client
+ * @ingroup CO_CANopen_301
+ * @{
+ *
+ * CANopen Service Data Object - client protocol (master functionality).
+ *
+ * @see @ref CO_SDOserver
+ */
+
+
+/**
+ * SDO Client Parameter. The same as record from Object dictionary
+ * (index 0x1280+).
+ */
+typedef struct {
+    /** Equal to 3 */
+    uint8_t             maxSubIndex;
+    /** Communication object identifier for client transmission.
+     * Meaning of the specific bits:
+        - Bit 0...10: 11-bit CAN identifier.
+        - Bit 11..30: reserved, set to 0.
+        - Bit 31: if 1, SDO client object is not used. */
+    uint32_t            COB_IDClientToServer;
+    /** Communication object identifier for message received from server.
+     * Meaning of the specific bits:
+        - Bit 0...10: 11-bit CAN identifier.
+        - Bit 11..30: reserved, set to 0.
+        - Bit 31: if 1, SDO client object is not used. */
+    uint32_t            COB_IDServerToClient;
+    /** Node-ID of the SDO server */
+    uint8_t             nodeIDOfTheSDOServer;
+} CO_SDOclientPar_t;
+
+
+/**
+ * SDO client object
+ */
+typedef struct {
+#if ((CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_LOCAL) || defined CO_DOXYGEN
+    /** From CO_SDOclient_init() */
+    CO_SDO_t *SDO;
+#endif
+    /** From CO_SDOclient_init() */
+    CO_SDOclientPar_t *SDOClientPar;
+    /** From CO_SDOclient_init() */
+    CO_CANmodule_t *CANdevRx;
+    /** From CO_SDOclient_init() */
+    uint16_t CANdevRxIdx;
+    /** From CO_SDOclient_init() */
+    CO_CANmodule_t *CANdevTx;
+    /** From CO_SDOclient_init() */
+    uint16_t CANdevTxIdx;
+    /** CAN transmit buffer inside CANdevTx for CAN tx message */
+    CO_CANtx_t *CANtxBuff;
+    /** Index of current object in Object Dictionary */
+    uint16_t index;
+    /** Subindex of current object in Object Dictionary */
+    uint8_t subIndex;
+    /* If true, then data transfer is finished */
+    bool_t finished;
+    /** Size of data, which will be transferred. It is optionally indicated by
+     * client in case of download or by server in case of upload. */
+    size_t sizeInd;
+    /** Size of data which is actually transferred. */
+    size_t sizeTran;
+    /** Internal state of the SDO client */
+    volatile CO_SDO_state_t state;
+    /** Maximum timeout time between request and response in microseconds */
+    uint32_t SDOtimeoutTime_us;
+    /** Timeout timer for SDO communication */
+    uint32_t timeoutTimer;
+    /** CO_fifo_t object for data buffer (not pointer) */
+    CO_fifo_t bufFifo;
+    /** Data buffer of usable size @ref CO_CONFIG_SDO_CLI_BUFFER_SIZE, used
+     * inside bufFifo. Must be one byte larger for fifo usage. */
+    char buf[CO_CONFIG_SDO_CLI_BUFFER_SIZE + 1];
+    /** Indicates, if new SDO message received from CAN bus. It is not cleared,
+     * until received message is completely processed. */
+    volatile void *CANrxNew;
+    /** 8 data bytes of the received message */
+    uint8_t CANrxData[8];
+    /** Previous value of the COB_IDClientToServer */
+    uint32_t COB_IDClientToServerPrev;
+    /** Previous value of the COB_IDServerToClient */
+    uint32_t COB_IDServerToClientPrev;
+#if ((CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+    /** From CO_SDOclient_initCallbackPre() or NULL */
+    void (*pFunctSignal)(void *object);
+    /** From CO_SDOclient_initCallbackPre() or NULL */
+    void *functSignalObject;
+#endif
+#if ((CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED) || defined CO_DOXYGEN
+    /** Toggle bit toggled in each segment in segmented transfer */
+    uint8_t toggle;
+#endif
+#if ((CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK) || defined CO_DOXYGEN
+    /** Timeout time for SDO sub-block upload, half of #SDOtimeoutTime_us */
+    uint32_t block_SDOtimeoutTime_us;
+    /** Timeout timer for SDO sub-block upload */
+    uint32_t block_timeoutTimer;
+    /** Sequence number of segment in block, 1..127 */
+    uint8_t block_seqno;
+    /** Number of segments per block, 1..127 */
+    uint8_t block_blksize;
+    /** Number of bytes in last segment that do not contain data */
+    uint8_t block_noData;
+    /** Server CRC support in block transfer */
+    bool_t block_crcEnabled;
+    /** Last 7 bytes of data at block upload */
+    uint8_t block_dataUploadLast[7];
+    /** Calculated CRC checksum */
+    uint16_t block_crc;
+#endif
+} CO_SDOclient_t;
+
+
+/**
+ * Initialize SDO client object.
+ *
+ * Function must be called in the communication reset section.
+ *
+ * @param SDO_C This object will be initialized.
+ * @param SDO SDO server object. It is used in case, if client is accessing
+ * object dictionary from its own device. If NULL, it will be ignored.
+ * @param SDOClientPar Pointer to _SDO Client Parameter_ record from Object
+ * dictionary (index 0x1280+). Will be written.
+ * @param CANdevRx CAN device for SDO client reception.
+ * @param CANdevRxIdx Index of receive buffer in the above CAN device.
+ * @param CANdevTx CAN device for SDO client transmission.
+ * @param CANdevTxIdx Index of transmit buffer in the above CAN device.
+ *
+ * @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ */
+CO_ReturnError_t CO_SDOclient_init(CO_SDOclient_t *SDO_C,
+                                   void *SDO,
+                                   CO_SDOclientPar_t *SDOClientPar,
+                                   CO_CANmodule_t *CANdevRx,
+                                   uint16_t CANdevRxIdx,
+                                   CO_CANmodule_t *CANdevTx,
+                                   uint16_t CANdevTxIdx);
+
+
+#if ((CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+/**
+ * Initialize SDOclient callback function.
+ *
+ * Function initializes optional callback function, which should immediately
+ * start processing of CO_SDOclientDownload() or CO_SDOclientUpload() function.
+ * Callback is called after SDOclient message is received from the CAN bus or
+ * when new call without delay is necessary (exchange data with own SDO server
+ * or SDO block transfer is in progress).
+ *
+ * @param SDOclient This object.
+ * @param object Pointer to object, which will be passed to pFunctSignal(). Can
+ * be NULL.
+ * @param pFunctSignal Pointer to the callback function. Not called if NULL.
+ */
+void CO_SDOclient_initCallbackPre(CO_SDOclient_t *SDOclient,
+                                  void *object,
+                                  void (*pFunctSignal)(void *object));
+#endif
+
+
+/**
+ * Setup SDO client object.
+ *
+ * Function must be called before new SDO communication. If previous SDO
+ * communication was with the same node, function does not need to be called.
+ *
+ * @remark If configuring SDO client from network is required, this function
+ * should be set as callback for the corresponding SDO client parameter OD
+ * entry.
+ *
+ * @param SDO_C This object.
+ * @param COB_IDClientToServer See CO_SDOclientPar_t. If zero, then
+ * nodeIDOfTheSDOServer is used with default COB-ID.
+ * @param COB_IDServerToClient See CO_SDOclientPar_t. If zero, then
+ * nodeIDOfTheSDOServer is used with default COB-ID.
+ * @param nodeIDOfTheSDOServer Node-ID of the SDO server. If zero, SDO client
+ * object is not used. If it is the same as node-ID of this node, then data will
+ * be exchanged with this node (without CAN communication).
+ *
+ * @return #CO_SDO_return_t
+ */
+CO_SDO_return_t CO_SDOclient_setup(CO_SDOclient_t *SDO_C,
+                                   uint32_t COB_IDClientToServer,
+                                   uint32_t COB_IDServerToClient,
+                                   uint8_t nodeIDOfTheSDOServer);
+
+
+/**
+ * Initiate SDO download communication.
+ *
+ * Function initiates SDO download communication with server specified in
+ * CO_SDOclient_init() function. Data will be written to remote node.
+ * Function is non-blocking.
+ *
+ * @param SDO_C This object.
+ * @param index Index of object in object dictionary in remote node.
+ * @param subIndex Subindex of object in object dictionary in remote node.
+ * @param sizeIndicated Optionally indicate size of data to be downloaded.
+ * Actual data are written with one or multiple CO_SDOclientDownloadBufWrite()
+ * calls.
+ * - If sizeIndicated is different than 0, then total number of data
+ * written by CO_SDOclientDownloadBufWrite() will be compared against
+ * sizeIndicated. Also sizeIndicated info will be passed to the server, which
+ * will compare actual data size downloaded. In case of mismatch, SDO abort
+ * message will be generated.
+ * - If sizeIndicated is 0, then actual data size will not be verified.
+ * @param SDOtimeoutTime_ms Timeout time for SDO communication in milliseconds.
+ * @param blockEnable Try to initiate block transfer.
+ *
+ * @return #CO_SDO_return_t
+ */
+CO_SDO_return_t CO_SDOclientDownloadInitiate(CO_SDOclient_t *SDO_C,
+                                             uint16_t index,
+                                             uint8_t subIndex,
+                                             size_t sizeIndicated,
+                                             uint16_t SDOtimeoutTime_ms,
+                                             bool_t blockEnable);
+
+
+/**
+ * Initiate SDO download communication - update size.
+ *
+ * This is optional function, which updates sizeIndicated, if it was not known
+ * in the CO_SDOclientDownloadInitiate() function call. This function can be
+ * used after CO_SDOclientDownloadBufWrite(), but must be used before
+ * CO_SDOclientDownload().
+ *
+ * @param SDO_C This object.
+ * @param sizeIndicated Same as in CO_SDOclientDownloadInitiate().
+ */
+void CO_SDOclientDownloadInitiateSize(CO_SDOclient_t *SDO_C,
+                                      size_t sizeIndicated);
+
+
+/**
+ * Write data into SDO client buffer
+ *
+ * This function copies data from buf into internal SDO client fifo buffer.
+ * Function returns number of bytes successfully copied. If there is not enough
+ * space in destination, not all bytes will be copied. Additional data can be
+ * copied in next cycles. If there is enough space in destination and
+ * sizeIndicated is different than zero, then all data must be written at once.
+ *
+ * This function is basically a wrapper for CO_fifo_write() function. As
+ * alternative, other functions from CO_fifo can be used directly, for example
+ * CO_fifo_cpyTok2U8() or similar.
+ *
+ * @param SDO_C This object.
+ * @param buf Buffer which will be copied
+ * @param count Number of bytes in buf
+ *
+ * @return number of bytes actually written.
+ */
+size_t CO_SDOclientDownloadBufWrite(CO_SDOclient_t *SDO_C,
+                                    const char *buf,
+                                    size_t count);
+
+
+/**
+ * Process SDO download communication.
+ *
+ * Function must be called cyclically until it returns <=0. It Proceeds SDO
+ * download communication initiated with CO_SDOclientDownloadInitiate().
+ * Function is non-blocking.
+ *
+ * If function returns #CO_SDO_RT_blockDownldInProgress and OS has buffer for
+ * CAN tx messages, then this function may be called multiple times within own
+ * loop. This can speed-up SDO block transfer.
+ *
+ * @param SDO_C This object.
+ * @param timeDifference_us Time difference from previous function call in
+ * [microseconds].
+ * @param abort If true, SDO client will send abort message from SDOabortCode
+ * and transmission will be aborted.
+ * @param bufferPartial True indicates, not all data were copied to internal
+ * buffer yet. Buffer will be refilled later with #CO_SDOclientDownloadBufWrite.
+ * @param [out] SDOabortCode In case of error in communication, SDO abort code
+ * contains reason of error. Ignored if NULL.
+ * @param [out] sizeTransferred Actual size of data transferred. Ignored if NULL
+ * @param [out] timerNext_us info to OS - see CO_process(). Ignored if NULL.
+ *
+ * @return #CO_SDO_return_t. If less than 0, then error occurred,
+ * SDOabortCode contains reason and state becomes idle. If 0, communication
+ * ends successfully and state becomes idle. If greater than 0, then
+ * communication is in progress.
+ */
+CO_SDO_return_t CO_SDOclientDownload(CO_SDOclient_t *SDO_C,
+                                     uint32_t timeDifference_us,
+                                     bool_t abort,
+                                     bool_t bufferPartial,
+                                     CO_SDO_abortCode_t *SDOabortCode,
+                                     size_t *sizeTransferred,
+                                     uint32_t *timerNext_us);
+
+
+/**
+ * Initiate SDO upload communication.
+ *
+ * Function initiates SDO upload communication with server specified in
+ * CO_SDOclient_init() function. Data will be read from remote node.
+ * Function is non-blocking.
+ *
+ * @param SDO_C This object.
+ * @param index Index of object in object dictionary in remote node.
+ * @param subIndex Subindex of object in object dictionary in remote node.
+ * @param SDOtimeoutTime_ms Timeout time for SDO communication in milliseconds.
+ * @param blockEnable Try to initiate block transfer.
+ *
+ * @return #CO_SDO_return_t
+ */
+CO_SDO_return_t CO_SDOclientUploadInitiate(CO_SDOclient_t *SDO_C,
+                                           uint16_t index,
+                                           uint8_t subIndex,
+                                           uint16_t SDOtimeoutTime_ms,
+                                           bool_t blockEnable);
+
+
+/**
+ * Process SDO upload communication.
+ *
+ * Function must be called cyclically until it returns <=0. It Proceeds SDO
+ * upload communication initiated with CO_SDOclientUploadInitiate().
+ * Function is non-blocking.
+ *
+ * If this function returns #CO_SDO_RT_uploadDataBufferFull, then data must be
+ * read from fifo buffer to make it empty. This function can then be called
+ * once again immediately to speed-up block transfer. Note also, that remaining
+ * data must be read after function returns #CO_SDO_RT_ok_communicationEnd.
+ * Data must not be read, if function returns #CO_SDO_RT_blockUploadInProgress.
+ *
+ * @param SDO_C This object.
+ * @param timeDifference_us Time difference from previous function call in
+ * [microseconds].
+ * @param abort If true, SDO client will send abort message from SDOabortCode
+ * and reception will be aborted.
+ * @param [out] SDOabortCode In case of error in communication, SDO abort code
+ * contains reason of error. Ignored if NULL.
+ * @param [out] sizeIndicated If larger than 0, then SDO server has indicated
+ * size of data transfer. Ignored if NULL.
+ * @param [out] sizeTransferred Actual size of data transferred. Ignored if NULL
+ * @param [out] timerNext_us info to OS - see CO_process(). Ignored if NULL.
+ *
+ * @return #CO_SDO_return_t. If less than 0, then error occurred,
+ * SDOabortCode contains reason and state becomes idle. If 0, communication
+ * ends successfully and state becomes idle. If greater than 0, then
+ * communication is in progress.
+ */
+CO_SDO_return_t CO_SDOclientUpload(CO_SDOclient_t *SDO_C,
+                                   uint32_t timeDifference_us,
+                                   bool_t abort,
+                                   CO_SDO_abortCode_t *SDOabortCode,
+                                   size_t *sizeIndicated,
+                                   size_t *sizeTransferred,
+                                   uint32_t *timerNext_us);
+
+
+/**
+ * Read data from SDO client buffer.
+ *
+ * This function copies data from internal fifo buffer of SDO client into buf.
+ * Function returns number of bytes successfully copied. It can be called in
+ * multiple cycles, if data length is large.
+ *
+ * This function is basically a wrapper for CO_fifo_read() function. As
+ * alternative, other functions from CO_fifo can be used directly, for example
+ * CO_fifo_readU82a() or similar.
+ *
+ * @warning This function (or similar) must NOT be called when
+ * CO_SDOclientUpload() returns #CO_SDO_RT_blockUploadInProgress!
+ *
+ * @param SDO_C This object.
+ * @param buf Buffer into which data will be copied
+ * @param count Copy up to count bytes into buffer
+ *
+ * @return number of bytes actually read.
+ */
+size_t CO_SDOclientUploadBufRead(CO_SDOclient_t *SDO_C,
+                                 char *buf,
+                                 size_t count);
+
+
+/**
+ * Close SDO communication temporary.
+ *
+ * Function must be called after finish of each SDO client communication cycle.
+ * It disables reception of SDO client CAN messages. It is necessary, because
+ * CO_SDOclient_receive function may otherwise write into undefined SDO buffer.
+ *
+ * @param SDO_C This object.
+ */
+void CO_SDOclientClose(CO_SDOclient_t *SDO_C);
+
+/** @} */ /* CO_SDOclient */
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_ENABLE */
+
+#endif /* CO_SDO_CLIENT_H */

controller_yy_app/middleware/CANopenNode/301/CO_SDOserver.c

@@ -0,0 +1,1478 @@
+/*
+ * CANopen Service Data Object - server.
+ *
+ * @file        CO_SDOserver.c
+ * @ingroup     CO_SDOserver
+ * @author      Janez Paternoster
+ * @copyright   2004 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+
+#include "301/CO_driver.h"
+#include "301/CO_SDOserver.h"
+#include "301/crc16-ccitt.h"
+
+
+/* Client command specifier, see DS301 */
+#define CCS_DOWNLOAD_INITIATE          1U
+#define CCS_DOWNLOAD_SEGMENT           0U
+#define CCS_UPLOAD_INITIATE            2U
+#define CCS_UPLOAD_SEGMENT             3U
+#define CCS_DOWNLOAD_BLOCK             6U
+#define CCS_UPLOAD_BLOCK               5U
+#define CCS_ABORT                      0x80U
+
+
+#if CO_CONFIG_SDO_BUFFER_SIZE < 7
+    #error CO_CONFIG_SDO_BUFFER_SIZE must be greater than 7
+#endif
+
+static void CO_SDO_receive_done(CO_SDO_t *SDO){
+#if CO_SDO_RX_DATA_SIZE > 1
+    uint8_t rcv = SDO->CANrxRcv;
+    uint8_t newRcv = rcv;
+
+    if (++newRcv >= CO_SDO_RX_DATA_SIZE)
+        newRcv = 0;
+    SDO->CANrxRcv = newRcv;
+    CO_FLAG_SET(SDO->CANrxNew[rcv]);
+#else
+    CO_FLAG_SET(SDO->CANrxNew[0]);
+#endif
+}
+
+/*
+ * Read received message from CAN module.
+ *
+ * Function will be called (by CAN receive interrupt) every time, when CAN
+ * message with correct identifier will be received. For more information and
+ * description of parameters see file CO_driver.h.
+ */
+static void CO_SDO_receive(void *object, void *msg);
+static void CO_SDO_receive(void *object, void *msg){
+    CO_SDO_t *SDO;
+    uint8_t rcv, *CANrxData;
+    uint8_t DLC = CO_CANrxMsg_readDLC(msg);
+    uint8_t *data = CO_CANrxMsg_readData(msg);
+
+    SDO = (CO_SDO_t*)object;   /* this is the correct pointer type of the first argument */
+    rcv = SDO->CANrxRcv;
+    CANrxData = SDO->CANrxData[rcv];
+
+    /* verify message length and message queue overflow (if previous messages were not processed yet) */
+    if ((DLC == 8U) && (!CO_FLAG_READ(SDO->CANrxNew[rcv]))){
+        if (SDO->state != CO_SDO_ST_DOWNLOAD_BL_SUBBLOCK) {
+            /* copy data and set 'new message' flag */
+            memcpy(CANrxData, data, DLC);
+            CO_SDO_receive_done(SDO);
+        }
+        else {
+            /* block download, copy data directly */
+            uint8_t seqno;
+
+            CANrxData[0] = data[0];
+            seqno = CANrxData[0] & 0x7fU;
+            SDO->timeoutTimer = 0;
+            /* clear timeout in sub-block transfer indication if set before */
+            if (SDO->timeoutSubblockDownolad) {
+                SDO->timeoutSubblockDownolad = false;
+            }
+
+            /* check correct sequence number. */
+            if (seqno == (SDO->sequence + 1U)) {
+                /* sequence is correct */
+
+                /* check if buffer can store whole message just in case */
+                if (CO_CONFIG_SDO_BUFFER_SIZE - SDO->bufferOffset >= 7) {
+                    uint8_t i;
+
+                    SDO->sequence++;
+
+                    /* copy data */
+                    for(i=1; i<8; i++) {
+                        SDO->ODF_arg.data[SDO->bufferOffset++] = data[i]; //SDO->ODF_arg.data is equal as SDO->databuffer
+                    }
+
+                    /* break reception if last segment, block ends or block sequence is too large */
+                    if (((CANrxData[0] & 0x80U) == 0x80U) || (SDO->sequence >= SDO->blksize)) {
+                        SDO->state = CO_SDO_ST_DOWNLOAD_BL_SUB_RESP;
+                        CO_SDO_receive_done(SDO);
+                    }
+                } else {
+                    /* buffer is full, ignore this segment, send response without resetting sequence */
+                    SDO->state = CO_SDO_ST_DOWNLOAD_BL_SUB_RESP_2;
+                    CO_SDO_receive_done(SDO);
+                }
+            }
+            else if ((seqno == SDO->sequence) || (SDO->sequence == 0U)) {
+                /* Ignore message, if it is duplicate or if sequence didn't started yet. */
+            }
+            else {
+                /* seqno is wrong, send response without resetting sequence */
+                SDO->state = CO_SDO_ST_DOWNLOAD_BL_SUB_RESP_2;
+                CO_SDO_receive_done(SDO);
+            }
+        }
+
+#if (CO_CONFIG_SDO) & CO_CONFIG_FLAG_CALLBACK_PRE
+        /* Optional signal to RTOS, which can resume task, which handles SDO server. */
+        if (CO_FLAG_READ(SDO->CANrxNew[rcv]) && SDO->pFunctSignalPre != NULL) {
+            SDO->pFunctSignalPre(SDO->functSignalObjectPre);
+        }
+#endif
+    }
+}
+
+
+/*
+ * Function for accessing _SDO server parameter_ for default SDO (index 0x1200)
+ * from SDO server.
+ *
+ * For more information see file CO_SDOserver.h.
+ */
+static CO_SDO_abortCode_t CO_ODF_1200(CO_ODF_arg_t *ODF_arg);
+static CO_SDO_abortCode_t CO_ODF_1200(CO_ODF_arg_t *ODF_arg){
+    uint8_t *nodeId;
+    uint32_t value;
+    CO_SDO_abortCode_t ret = CO_SDO_AB_NONE;
+
+    nodeId = (uint8_t*) ODF_arg->object;
+    value = CO_getUint32(ODF_arg->data);
+
+    /* if SDO reading Object dictionary 0x1200, add nodeId to the value */
+    if ((ODF_arg->reading) && (ODF_arg->subIndex > 0U)){
+        CO_setUint32(ODF_arg->data, value + *nodeId);
+    }
+
+    return ret;
+}
+
+
+/******************************************************************************/
+CO_ReturnError_t CO_SDO_init(
+        CO_SDO_t               *SDO,
+        uint32_t                COB_IDClientToServer,
+        uint32_t                COB_IDServerToClient,
+        uint16_t                ObjDictIndex_SDOServerParameter,
+        CO_SDO_t               *parentSDO,
+        const CO_OD_entry_t     OD[],
+        uint16_t                ODSize,
+        CO_OD_extension_t      *ODExtensions,
+        uint8_t                 nodeId,
+        uint16_t                SDOtimeoutTime_ms,
+        CO_CANmodule_t         *CANdevRx,
+        uint16_t                CANdevRxIdx,
+        CO_CANmodule_t         *CANdevTx,
+        uint16_t                CANdevTxIdx)
+{
+    CO_ReturnError_t ret = CO_ERROR_NO;
+
+    /* verify arguments */
+    if (SDO==NULL || CANdevRx==NULL || CANdevTx==NULL){
+        return CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    /* configure own object dictionary */
+    if (parentSDO == NULL){
+        uint16_t i;
+
+        SDO->ownOD = true;
+        SDO->OD = OD;
+        SDO->ODSize = ODSize;
+        SDO->ODExtensions = ODExtensions;
+
+        /* clear pointers in ODExtensions */
+        for(i=0U; i<ODSize; i++){
+            SDO->ODExtensions[i].pODFunc = NULL;
+            SDO->ODExtensions[i].object = NULL;
+            SDO->ODExtensions[i].flags = NULL;
+        }
+    }
+    /* copy object dictionary from parent */
+    else{
+        SDO->ownOD = false;
+        SDO->OD = parentSDO->OD;
+        SDO->ODSize = parentSDO->ODSize;
+        SDO->ODExtensions = parentSDO->ODExtensions;
+    }
+
+    /* Configure object variables */
+    SDO->nodeId = nodeId;
+    SDO->SDOtimeoutTime_us = (uint32_t)SDOtimeoutTime_ms * 1000;
+    SDO->state = CO_SDO_ST_IDLE;
+
+    uint8_t i;
+    for(i=0U; i<CO_SDO_RX_DATA_SIZE; i++){
+        CO_FLAG_CLEAR(SDO->CANrxNew[i]);
+    }
+    SDO->CANrxRcv = 0;
+    SDO->CANrxProc = 0;
+
+#if (CO_CONFIG_SDO) & CO_CONFIG_FLAG_CALLBACK_PRE
+    SDO->pFunctSignalPre = NULL;
+    SDO->functSignalObjectPre = NULL;
+#endif
+
+    /* Configure Object dictionary entry at index 0x1200 */
+    if (ObjDictIndex_SDOServerParameter == OD_H1200_SDO_SERVER_PARAM){
+        CO_OD_configure(SDO, ObjDictIndex_SDOServerParameter, CO_ODF_1200, (void*)&SDO->nodeId, 0U, 0U);
+    }
+
+    if ((COB_IDClientToServer & 0x80000000) != 0 || (COB_IDServerToClient & 0x80000000) != 0 ){
+        // SDO is invalid
+        COB_IDClientToServer = 0;
+        COB_IDServerToClient = 0;
+    }
+    /* configure SDO server CAN reception */
+    ret = CO_CANrxBufferInit(
+            CANdevRx,               /* CAN device */
+            CANdevRxIdx,            /* rx buffer index */
+            COB_IDClientToServer,   /* CAN identifier */
+            0x7FF,                  /* mask */
+            0,                      /* rtr */
+            (void*)SDO,             /* object passed to receive function */
+            CO_SDO_receive);        /* this function will process received message */
+
+    /* configure SDO server CAN transmission */
+    SDO->CANdevTx = CANdevTx;
+    SDO->CANtxBuff = CO_CANtxBufferInit(
+            CANdevTx,               /* CAN device */
+            CANdevTxIdx,            /* index of specific buffer inside CAN module */
+            COB_IDServerToClient,   /* CAN identifier */
+            0,                      /* rtr */
+            8,                      /* number of data bytes */
+            0);                     /* synchronous message flag bit */
+
+    if (SDO->CANtxBuff == NULL) {
+        ret = CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    return ret;
+}
+
+
+#if (CO_CONFIG_SDO) & CO_CONFIG_FLAG_CALLBACK_PRE
+/******************************************************************************/
+void CO_SDO_initCallbackPre(
+        CO_SDO_t               *SDO,
+        void                   *object,
+        void                  (*pFunctSignalPre)(void *object))
+{
+    if (SDO != NULL){
+        SDO->functSignalObjectPre = object;
+        SDO->pFunctSignalPre = pFunctSignalPre;
+    }
+}
+#endif
+
+
+/******************************************************************************/
+void CO_OD_configure(
+        CO_SDO_t               *SDO,
+        uint16_t                index,
+        CO_SDO_abortCode_t    (*pODFunc)(CO_ODF_arg_t *ODF_arg),
+        void                   *object,
+        uint8_t                *flags,
+        uint8_t                 flagsSize)
+{
+    uint16_t entryNo;
+
+    entryNo = CO_OD_find(SDO, index);
+    if (entryNo < 0xFFFFU){
+        CO_OD_extension_t *ext = &SDO->ODExtensions[entryNo];
+        uint8_t maxSubIndex = SDO->OD[entryNo].maxSubIndex;
+
+        ext->pODFunc = pODFunc;
+        ext->object = object;
+        if ((flags != NULL) && (flagsSize != 0U) && (flagsSize == maxSubIndex)){
+            uint16_t i;
+            ext->flags = flags;
+            for(i=0U; i<=maxSubIndex; i++){
+                ext->flags[i] = 0U;
+            }
+        }
+        else{
+            ext->flags = NULL;
+        }
+    }
+}
+
+
+/******************************************************************************/
+uint16_t CO_OD_find(CO_SDO_t *SDO, uint16_t index){
+    /* Fast search in ordered Object Dictionary. If indexes are mixed, this won't work. */
+    /* If Object Dictionary has up to 2^N entries, then N is max number of loop passes. */
+    uint16_t cur, min, max;
+    const CO_OD_entry_t* object;
+
+    min = 0U;
+    max = SDO->ODSize - 1U;
+    while(min < max){
+        cur = (min + max) / 2;
+        object = &SDO->OD[cur];
+        /* Is object matched */
+        if (index == object->index){
+            return cur;
+        }
+        if (index < object->index){
+            max = cur;
+            if (max) max--;
+        }
+        else
+            min = cur + 1U;
+    }
+
+    if (min == max){
+        object = &SDO->OD[min];
+        /* Is object matched */
+        if (index == object->index){
+            return min;
+        }
+    }
+
+    return 0xFFFFU;  /* object does not exist in OD */
+}
+
+
+/******************************************************************************/
+uint16_t CO_OD_getLength(CO_SDO_t *SDO, uint16_t entryNo, uint8_t subIndex){
+    const CO_OD_entry_t* object = &SDO->OD[entryNo];
+
+    if (entryNo == 0xFFFFU){
+        return 0U;
+    }
+
+    if (object->maxSubIndex == 0U){    /* Object type is Var */
+        if (object->pData == 0){ /* data type is domain */
+            return CO_CONFIG_SDO_BUFFER_SIZE;
+        }
+        else{
+            return object->length;
+        }
+    }
+    else if (object->attribute != 0U){ /* Object type is Array */
+        if (subIndex == 0U){
+            return 1U;
+        }
+        else if (object->pData == 0){
+            /* data type is domain */
+            return CO_CONFIG_SDO_BUFFER_SIZE;
+        }
+        else{
+            return object->length;
+        }
+    }
+    else{                            /* Object type is Record */
+        if (((const CO_OD_entryRecord_t*)(object->pData))[subIndex].pData == 0){
+            /* data type is domain */
+            return CO_CONFIG_SDO_BUFFER_SIZE;
+        }
+        else{
+            return ((const CO_OD_entryRecord_t*)(object->pData))[subIndex].length;
+        }
+    }
+}
+
+
+/******************************************************************************/
+uint16_t CO_OD_getAttribute(CO_SDO_t *SDO, uint16_t entryNo, uint8_t subIndex){
+    const CO_OD_entry_t* object = &SDO->OD[entryNo];
+
+    if (entryNo == 0xFFFFU){
+        return 0U;
+    }
+
+    if (object->maxSubIndex == 0U){   /* Object type is Var */
+        return object->attribute;
+    }
+    else if (object->attribute != 0U){/* Object type is Array */
+        bool_t exception_1003 = false;
+        uint16_t attr = object->attribute;
+
+        /* Special exception: Object 1003,00 should be writable */
+        if (object->index == 0x1003 && subIndex == 0) {
+            exception_1003 = true;
+            attr |= CO_ODA_WRITEABLE;
+        }
+
+        if (subIndex == 0U  && !exception_1003){
+            /* First subIndex is readonly */
+            attr &= ~(CO_ODA_WRITEABLE | CO_ODA_RPDO_MAPABLE);
+            attr |= CO_ODA_READABLE;
+        }
+        return attr;
+    }
+    else{                            /* Object type is Record */
+        return ((const CO_OD_entryRecord_t*)(object->pData))[subIndex].attribute;
+    }
+}
+
+
+/******************************************************************************/
+void* CO_OD_getDataPointer(CO_SDO_t *SDO, uint16_t entryNo, uint8_t subIndex){
+    const CO_OD_entry_t* object = &SDO->OD[entryNo];
+
+    if (entryNo == 0xFFFFU){
+        return 0;
+    }
+
+    if (object->maxSubIndex == 0U){   /* Object type is Var */
+        return object->pData;
+    }
+    else if (object->maxSubIndex < subIndex){
+        /* Object type Array/Record, request is out of bounds */
+        return 0;
+    }
+    else if (object->attribute != 0U){/* Object type is Array */
+        if (subIndex==0){
+            /* this is the data, for the subIndex 0 in the array */
+            return (void*) &object->maxSubIndex;
+        }
+        else if (object->pData == 0){
+            /* data type is domain */
+            return 0;
+        }
+        else{
+            return (void*)(((int8_t*)object->pData) + ((subIndex-1) * object->length));
+        }
+    }
+    else{                            /* Object Type is Record */
+        return ((const CO_OD_entryRecord_t*)(object->pData))[subIndex].pData;
+    }
+}
+
+
+/******************************************************************************/
+uint8_t* CO_OD_getFlagsPointer(CO_SDO_t *SDO, uint16_t entryNo, uint8_t subIndex){
+    if (entryNo == 0xFFFF || SDO->ODExtensions == NULL){
+        return NULL;
+    }
+
+    CO_OD_extension_t* ext = &SDO->ODExtensions[entryNo];
+    if (ext->flags == NULL){
+        return NULL;
+    }
+
+    return &ext->flags[subIndex];
+}
+
+
+/******************************************************************************/
+uint32_t CO_SDO_initTransfer(CO_SDO_t *SDO, uint16_t index, uint8_t subIndex){
+
+    SDO->ODF_arg.index = index;
+    SDO->ODF_arg.subIndex = subIndex;
+
+    /* find object in Object Dictionary */
+    SDO->entryNo = CO_OD_find(SDO, index);
+    if (SDO->entryNo == 0xFFFFU){
+        return CO_SDO_AB_NOT_EXIST ;     /* object does not exist in OD */
+    }
+
+    /* verify existance of subIndex */
+    if (subIndex > SDO->OD[SDO->entryNo].maxSubIndex &&
+            SDO->OD[SDO->entryNo].pData != NULL)
+    {
+        return CO_SDO_AB_SUB_UNKNOWN;     /* Sub-index does not exist. */
+    }
+
+    /* pointer to data in Object dictionary */
+    SDO->ODF_arg.ODdataStorage = CO_OD_getDataPointer(SDO, SDO->entryNo, subIndex);
+
+    /* fill ODF_arg */
+    SDO->ODF_arg.object = NULL;
+    if (SDO->ODExtensions){
+        CO_OD_extension_t *ext = &SDO->ODExtensions[SDO->entryNo];
+        SDO->ODF_arg.object = ext->object;
+    }
+    SDO->ODF_arg.data = SDO->databuffer;
+    SDO->ODF_arg.dataLength = CO_OD_getLength(SDO, SDO->entryNo, subIndex);
+    SDO->ODF_arg.attribute = CO_OD_getAttribute(SDO, SDO->entryNo, subIndex);
+    SDO->ODF_arg.pFlags = CO_OD_getFlagsPointer(SDO, SDO->entryNo, subIndex);
+
+    SDO->ODF_arg.firstSegment = true;
+    SDO->ODF_arg.lastSegment = true;
+
+    /* indicate total data length, if not domain */
+    SDO->ODF_arg.dataLengthTotal = (SDO->ODF_arg.ODdataStorage) ? SDO->ODF_arg.dataLength : 0U;
+
+    SDO->ODF_arg.offset = 0U;
+
+    /* verify length */
+    if (SDO->ODF_arg.dataLength > CO_CONFIG_SDO_BUFFER_SIZE){
+        return CO_SDO_AB_DEVICE_INCOMPAT;     /* general internal incompatibility in the device */
+    }
+
+    return 0U;
+}
+
+
+/******************************************************************************/
+uint32_t CO_SDO_readOD(CO_SDO_t *SDO, uint16_t SDOBufferSize){
+    uint8_t *SDObuffer = SDO->ODF_arg.data;
+    uint8_t *ODdata = (uint8_t*)SDO->ODF_arg.ODdataStorage;
+    uint16_t length = SDO->ODF_arg.dataLength;
+    CO_OD_extension_t *ext = NULL;
+
+    /* is object readable? */
+    if ((SDO->ODF_arg.attribute & CO_ODA_READABLE) == 0)
+        return CO_SDO_AB_WRITEONLY;     /* attempt to read a write-only object */
+
+    /* find extension */
+    if (SDO->ODExtensions != NULL){
+        ext = &SDO->ODExtensions[SDO->entryNo];
+    }
+
+    CO_LOCK_OD();
+
+    /* copy data from OD to SDO buffer if not domain */
+    if (ODdata != NULL){
+        while(length--) *(SDObuffer++) = *(ODdata++);
+    }
+    /* if domain, Object dictionary function MUST exist */
+    else{
+        if (ext && ext->pODFunc == NULL){
+            CO_UNLOCK_OD();
+            return CO_SDO_AB_DEVICE_INCOMPAT;     /* general internal incompatibility in the device */
+        }
+    }
+
+    /* call Object dictionary function if registered */
+    SDO->ODF_arg.reading = true;
+    if (ext && ext->pODFunc != NULL){
+        uint32_t abortCode = ext->pODFunc(&SDO->ODF_arg);
+        if (abortCode != 0U){
+            CO_UNLOCK_OD();
+            return abortCode;
+        }
+
+        /* dataLength (upadted by pODFunc) must be inside limits */
+        if ((SDO->ODF_arg.dataLength == 0U) || (SDO->ODF_arg.dataLength > SDOBufferSize)){
+            CO_UNLOCK_OD();
+            return CO_SDO_AB_DEVICE_INCOMPAT;     /* general internal incompatibility in the device */
+        }
+    }
+
+    CO_UNLOCK_OD();
+
+    SDO->ODF_arg.offset += SDO->ODF_arg.dataLength;
+    SDO->ODF_arg.firstSegment = false;
+
+    /* swap data if processor is not little endian (CANopen is) */
+#ifdef CO_BIG_ENDIAN
+    if ((SDO->ODF_arg.attribute & CO_ODA_MB_VALUE) != 0){
+        uint16_t len = SDO->ODF_arg.dataLength;
+        uint8_t *buf1 = SDO->ODF_arg.data;
+        uint8_t *buf2 = buf1 + len - 1;
+
+        len /= 2;
+        while(len--){
+            uint8_t b = *buf1;
+            *(buf1++) = *buf2;
+            *(buf2--) = b;
+        }
+    }
+#endif
+
+    return 0U;
+}
+
+
+/******************************************************************************/
+uint32_t CO_SDO_writeOD(CO_SDO_t *SDO, uint16_t length){
+    uint8_t *SDObuffer = SDO->ODF_arg.data;
+    uint8_t *ODdata = (uint8_t*)SDO->ODF_arg.ODdataStorage;
+    bool_t exception_1003 = false;
+
+    /* is object writeable? */
+    if ((SDO->ODF_arg.attribute & CO_ODA_WRITEABLE) == 0){
+        return CO_SDO_AB_READONLY;     /* attempt to write a read-only object */
+    }
+
+    /* length of domain data is application specific and not verified */
+    if (ODdata == 0){
+        SDO->ODF_arg.dataLength = length;
+    }
+
+    /* verify length except for domain data type */
+    else if (SDO->ODF_arg.dataLength != length){
+        return CO_SDO_AB_TYPE_MISMATCH;     /* Length of service parameter does not match */
+    }
+
+    /* swap data if processor is not little endian (CANopen is) */
+#ifdef CO_BIG_ENDIAN
+    if ((SDO->ODF_arg.attribute & CO_ODA_MB_VALUE) != 0){
+        uint16_t len = SDO->ODF_arg.dataLength;
+        uint8_t *buf1 = SDO->ODF_arg.data;
+        uint8_t *buf2 = buf1 + len - 1;
+
+        len /= 2;
+        while(len--){
+            uint8_t b = *buf1;
+            *(buf1++) = *buf2;
+            *(buf2--) = b;
+        }
+    }
+#endif
+
+    CO_LOCK_OD();
+
+    /* call Object dictionary function if registered */
+    SDO->ODF_arg.reading = false;
+    if (SDO->ODExtensions != NULL){
+        CO_OD_extension_t *ext = &SDO->ODExtensions[SDO->entryNo];
+
+        if (ext->pODFunc != NULL){
+            uint32_t abortCode = ext->pODFunc(&SDO->ODF_arg);
+            if (abortCode != 0U){
+                CO_UNLOCK_OD();
+                return abortCode;
+            }
+        }
+    }
+    SDO->ODF_arg.offset += SDO->ODF_arg.dataLength;
+    SDO->ODF_arg.firstSegment = false;
+
+    /* Special exception: 1003,00 is writable from network, but not in OD  */
+    if (SDO->ODF_arg.index == 0x1003 && SDO->ODF_arg.subIndex == 0) {
+        exception_1003 = true;
+    }
+
+    /* copy data from SDO buffer to OD if not domain */
+    if ((ODdata != NULL) && !exception_1003){
+        while(length--){
+            *(ODdata++) = *(SDObuffer++);
+        }
+    }
+
+    CO_UNLOCK_OD();
+
+    return 0;
+}
+
+/******************************************************************************/
+static void CO_SDO_process_done(CO_SDO_t *SDO, uint32_t *timerNext_us) {
+    (void)timerNext_us; /* may be unused */
+
+#if CO_SDO_RX_DATA_SIZE > 1
+    uint8_t proc = SDO->CANrxProc;
+    uint8_t newProc = proc;
+
+    /* check if buffer needs to be free */
+    if (!CO_FLAG_READ(SDO->CANrxNew[proc])) {
+        return;
+    }
+
+    if (++newProc >= CO_SDO_RX_DATA_SIZE)
+        newProc = 0;
+
+    SDO->CANrxProc = newProc;
+    CO_FLAG_CLEAR(SDO->CANrxNew[proc]);
+
+#if (CO_CONFIG_SDO) & CO_CONFIG_FLAG_TIMERNEXT
+    if ((timerNext_us != NULL) && (CO_FLAG_READ(SDO->CANrxNew[newProc])))
+        timerNext_us = 0; /* Set timerNext_us to 0 to inform OS to call CO_SDO_process function again without delay. */
+#endif
+#else
+    CO_FLAG_CLEAR(SDO->CANrxNew[0]);
+#endif
+}
+
+/******************************************************************************/
+static void CO_SDO_abort(CO_SDO_t *SDO, uint32_t code){
+    SDO->CANtxBuff->data[0] = 0x80;
+    SDO->CANtxBuff->data[1] = SDO->ODF_arg.index & 0xFF;
+    SDO->CANtxBuff->data[2] = (SDO->ODF_arg.index>>8) & 0xFF;
+    SDO->CANtxBuff->data[3] = SDO->ODF_arg.subIndex;
+    CO_memcpySwap4(&SDO->CANtxBuff->data[4], &code);
+    SDO->state = CO_SDO_ST_IDLE;
+    /* skip all received messages in queue if any */
+    while (CO_FLAG_READ(SDO->CANrxNew[SDO->CANrxProc]))
+        CO_SDO_process_done(SDO, NULL);
+    CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff);
+}
+
+
+/******************************************************************************/
+int8_t CO_SDO_process(
+        CO_SDO_t               *SDO,
+        bool_t                  NMTisPreOrOperational,
+        uint32_t                timeDifference_us,
+        uint32_t               *timerNext_us)
+{
+    CO_SDO_state_t state = CO_SDO_ST_IDLE;
+    bool_t sendResponse = false;
+    uint8_t proc, *CANrxData;
+    bool_t isNew;
+
+    proc = SDO->CANrxProc;
+    isNew = CO_FLAG_READ(SDO->CANrxNew[proc]);
+
+    /* return if idle */
+    if ((SDO->state == CO_SDO_ST_IDLE) && (!isNew)){
+        return 0;
+    }
+
+    /* SDO is allowed to work only in operational or pre-operational NMT state */
+    if (!NMTisPreOrOperational){
+        SDO->state = CO_SDO_ST_IDLE;
+        /* free receive buffer if it is not empty */
+        CO_SDO_process_done(SDO, timerNext_us);
+        return 0;
+    }
+
+    CANrxData = SDO->CANrxData[proc];
+
+    /* Is something new to process? */
+    if ((!SDO->CANtxBuff->bufferFull) && (isNew || (SDO->state == CO_SDO_ST_UPLOAD_BL_SUBBLOCK))){
+        /* reset timeout */
+        if (SDO->state != CO_SDO_ST_UPLOAD_BL_SUBBLOCK) {
+            SDO->timeoutTimer = 0;
+            timeDifference_us = 0;
+        }
+
+        /* clear response buffer */
+        memset(SDO->CANtxBuff->data, 0, sizeof(SDO->CANtxBuff->data));
+
+        /* Is abort from client? */
+        if (isNew && (CANrxData[0] == CCS_ABORT)){
+            SDO->state = CO_SDO_ST_IDLE;
+            CO_SDO_process_done(SDO, timerNext_us);
+            return -1;
+        }
+
+        /* continue with previous SDO communication or start new */
+        if (SDO->state != CO_SDO_ST_IDLE){
+            state = SDO->state;
+        }
+        else{
+            uint32_t abortCode;
+            uint16_t index;
+            uint8_t CCS = CANrxData[0] >> 5;   /* Client command specifier */
+
+            /* Is client command specifier valid */
+            if ((CCS != CCS_DOWNLOAD_INITIATE) && (CCS != CCS_UPLOAD_INITIATE) &&
+                (CCS != CCS_DOWNLOAD_BLOCK) && (CCS != CCS_UPLOAD_BLOCK)){
+                CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */
+                return -1;
+            }
+
+            /* init ODF_arg */
+            index = CANrxData[2];
+            index = index << 8 | CANrxData[1];
+            abortCode = CO_SDO_initTransfer(SDO, index, CANrxData[3]);
+            if (abortCode != 0U){
+                CO_SDO_abort(SDO, abortCode);
+                return -1;
+            }
+
+            /* download */
+            if ((CCS == CCS_DOWNLOAD_INITIATE) || (CCS == CCS_DOWNLOAD_BLOCK)){
+                if ((SDO->ODF_arg.attribute & CO_ODA_WRITEABLE) == 0U){
+                    CO_SDO_abort(SDO, CO_SDO_AB_READONLY); /* attempt to write a read-only object */
+                    return -1;
+                }
+
+                /* set state machine to normal or block download */
+                if (CCS == CCS_DOWNLOAD_INITIATE){
+                    state = CO_SDO_ST_DOWNLOAD_INITIATE;
+                }
+                else{
+                    state = CO_SDO_ST_DOWNLOAD_BL_INITIATE;
+                }
+            }
+
+            /* upload */
+            else{
+                abortCode = CO_SDO_readOD(SDO, CO_CONFIG_SDO_BUFFER_SIZE);
+                if (abortCode != 0U){
+                    CO_SDO_abort(SDO, abortCode);
+                    return -1;
+                }
+
+                /* if data size is large enough set state machine to block upload, otherwise set to normal transfer */
+                if ((CCS == CCS_UPLOAD_BLOCK) && (SDO->ODF_arg.dataLength > CANrxData[5])){
+                    state = CO_SDO_ST_UPLOAD_BL_INITIATE;
+                }
+                else{
+                    state = CO_SDO_ST_UPLOAD_INITIATE;
+                }
+            }
+        }
+    }
+
+    /* verify SDO timeout */
+    if (SDO->timeoutTimer < SDO->SDOtimeoutTime_us) {
+        SDO->timeoutTimer += timeDifference_us;
+    }
+    if (SDO->timeoutTimer >= SDO->SDOtimeoutTime_us) {
+        if ((SDO->state == CO_SDO_ST_DOWNLOAD_BL_SUBBLOCK) && (!SDO->timeoutSubblockDownolad) && (!SDO->CANtxBuff->bufferFull)){
+            /* set indication timeout in sub-block transfer and reset timeout */
+            SDO->timeoutSubblockDownolad = true;
+            SDO->timeoutTimer = 0;
+            /* send response without resetting sequence */
+            state = CO_SDO_ST_DOWNLOAD_BL_SUB_RESP_2;
+        }
+        else{
+            CO_SDO_abort(SDO, CO_SDO_AB_TIMEOUT); /* SDO protocol timed out */
+            return -1;
+        }
+    }
+#if (CO_CONFIG_SDO) & CO_CONFIG_FLAG_TIMERNEXT
+    else if (timerNext_us != NULL) {
+        /* check again after timeout time elapsed */
+        uint32_t diff = SDO->SDOtimeoutTime_us - SDO->timeoutTimer;
+        if (*timerNext_us > diff) {
+            *timerNext_us = diff;
+        }
+    }
+#endif
+
+    /* return immediately if still idle */
+    if (state == CO_SDO_ST_IDLE){
+        return 0;
+    }
+
+    /* state machine (buffer is freed with process_done() at the end) */
+    switch(state){
+        uint32_t abortCode;
+        uint16_t len, i;
+        bool_t lastSegmentInSubblock;
+
+        case CO_SDO_ST_DOWNLOAD_INITIATE:{
+            /* default response */
+            SDO->CANtxBuff->data[0] = 0x60;
+            SDO->CANtxBuff->data[1] = CANrxData[1];
+            SDO->CANtxBuff->data[2] = CANrxData[2];
+            SDO->CANtxBuff->data[3] = CANrxData[3];
+
+            /* Expedited transfer */
+            if ((CANrxData[0] & 0x02U) != 0U){
+                /* is size indicated? Get message length */
+                if ((CANrxData[0] & 0x01U) != 0U){
+                    len = 4U - ((CANrxData[0] >> 2U) & 0x03U);
+                }
+                else{
+                    len = SDO->ODF_arg.dataLength;
+                }
+
+                /* copy data to SDO buffer */
+                SDO->ODF_arg.data[0] = CANrxData[4];
+                SDO->ODF_arg.data[1] = CANrxData[5];
+                SDO->ODF_arg.data[2] = CANrxData[6];
+                SDO->ODF_arg.data[3] = CANrxData[7];
+
+                /* write data to the Object dictionary */
+                abortCode = CO_SDO_writeOD(SDO, len);
+                if (abortCode != 0U){
+                    CO_SDO_abort(SDO, abortCode);
+                    return -1;
+                }
+
+                /* finish the communication and run mainline processing again */
+                SDO->state = CO_SDO_ST_IDLE;
+                sendResponse = true;
+#if (CO_CONFIG_SDO) & CO_CONFIG_FLAG_TIMERNEXT
+                if (timerNext_us != NULL)
+                    *timerNext_us = 0;
+#endif
+            }
+
+            /* Segmented transfer */
+            else{
+                /* verify length if size is indicated */
+                if ((CANrxData[0]&0x01) != 0){
+                    uint32_t lenRx;
+                    CO_memcpySwap4(&lenRx, &CANrxData[4]);
+                    SDO->ODF_arg.dataLengthTotal = lenRx;
+
+                    /* verify length except for domain data type */
+                    if ((lenRx != SDO->ODF_arg.dataLength) && (SDO->ODF_arg.ODdataStorage != 0)){
+                        CO_SDO_abort(SDO, CO_SDO_AB_TYPE_MISMATCH);  /* Length of service parameter does not match */
+                        return -1;
+                    }
+                }
+                SDO->bufferOffset = 0U;
+                SDO->sequence = 0U;
+                SDO->state = CO_SDO_ST_DOWNLOAD_SEGMENTED;
+                sendResponse = true;
+            }
+            break;
+        }
+
+        case CO_SDO_ST_DOWNLOAD_SEGMENTED:{
+            /* verify client command specifier */
+            if ((CANrxData[0]&0xE0) != 0x00U){
+                CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */
+                return -1;
+            }
+
+            /* verify toggle bit */
+            i = (CANrxData[0]&0x10U) ? 1U : 0U;
+            if (i != SDO->sequence){
+                CO_SDO_abort(SDO, CO_SDO_AB_TOGGLE_BIT);/* toggle bit not alternated */
+                return -1;
+            }
+
+            /* get size of data in message */
+            len = 7U - ((CANrxData[0] >> 1U) & 0x07U);
+
+            /* verify length. Domain data type enables length larger than SDO buffer size */
+            if ((SDO->bufferOffset + len) > SDO->ODF_arg.dataLength){
+                if (SDO->ODF_arg.ODdataStorage != 0){
+                    CO_SDO_abort(SDO, CO_SDO_AB_DATA_LONG);  /* Length of service parameter too high */
+                    return -1;
+                }
+                else{
+                    /* empty buffer in domain data type */
+                    SDO->ODF_arg.lastSegment = false;
+                    abortCode = CO_SDO_writeOD(SDO, SDO->bufferOffset);
+                    if (abortCode != 0U){
+                        CO_SDO_abort(SDO, abortCode);
+                        return -1;
+                    }
+
+                    SDO->ODF_arg.dataLength = CO_CONFIG_SDO_BUFFER_SIZE;
+                    SDO->bufferOffset = 0U;
+                }
+            }
+
+            /* copy data to buffer */
+            for(i=0U; i<len; i++)
+                SDO->ODF_arg.data[SDO->bufferOffset++] = CANrxData[i+1];
+
+            /* If no more segments to be downloaded, write data to the Object dictionary */
+            if ((CANrxData[0] & 0x01U) != 0U){
+                SDO->ODF_arg.lastSegment = true;
+                abortCode = CO_SDO_writeOD(SDO, SDO->bufferOffset);
+                if (abortCode != 0U){
+                    CO_SDO_abort(SDO, abortCode);
+                    return -1;
+                }
+
+                /* finish the communication and run mainline processing again */
+                SDO->state = CO_SDO_ST_IDLE;
+#if (CO_CONFIG_SDO) & CO_CONFIG_FLAG_TIMERNEXT
+                if (timerNext_us != NULL)
+                    *timerNext_us = 0;
+#endif
+            }
+
+            /* download segment response and alternate toggle bit */
+            SDO->CANtxBuff->data[0] = 0x20 | (SDO->sequence ? 0x10 : 0x00);
+            SDO->sequence = (SDO->sequence) ? 0 : 1;
+            sendResponse = true;
+            break;
+        }
+
+        case CO_SDO_ST_DOWNLOAD_BL_INITIATE:{
+            /* verify client command specifier and subcommand */
+            if ((CANrxData[0]&0xE1U) != 0xC0U){
+                CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */
+                return -1;
+            }
+
+            /* prepare response */
+            SDO->CANtxBuff->data[0] = 0xA4;
+            SDO->CANtxBuff->data[1] = CANrxData[1];
+            SDO->CANtxBuff->data[2] = CANrxData[2];
+            SDO->CANtxBuff->data[3] = CANrxData[3];
+
+            /* blksize */
+            SDO->blksize = (CO_CONFIG_SDO_BUFFER_SIZE > (7*127)) ? 127 : (CO_CONFIG_SDO_BUFFER_SIZE / 7);
+            SDO->CANtxBuff->data[4] = SDO->blksize;
+
+            /* is CRC enabled */
+            SDO->crcEnabled = (CANrxData[0] & 0x04) ? true : false;
+            SDO->crc = 0;
+
+            /* verify length if size is indicated */
+            if ((CANrxData[0]&0x02) != 0U){
+                uint32_t lenRx;
+                CO_memcpySwap4(&lenRx, &CANrxData[4]);
+                SDO->ODF_arg.dataLengthTotal = lenRx;
+
+                /* verify length except for domain data type */
+                if ((lenRx != SDO->ODF_arg.dataLength) && (SDO->ODF_arg.ODdataStorage != 0)){
+                    CO_SDO_abort(SDO, CO_SDO_AB_TYPE_MISMATCH);  /* Length of service parameter does not match */
+                    return -1;
+                }
+            }
+
+            SDO->bufferOffset = 0U;
+            SDO->sequence = 0U;
+            SDO->timeoutSubblockDownolad = false;
+            SDO->state = CO_SDO_ST_DOWNLOAD_BL_SUBBLOCK;
+
+            /* send response */
+            sendResponse = true;
+            break;
+        }
+
+        case CO_SDO_ST_DOWNLOAD_BL_SUBBLOCK:{
+            /* data are copied directly in receive function */
+            break;
+        }
+
+        case CO_SDO_ST_DOWNLOAD_BL_SUB_RESP:
+        case CO_SDO_ST_DOWNLOAD_BL_SUB_RESP_2:{
+            /* check if last segment received */
+            lastSegmentInSubblock = (!SDO->timeoutSubblockDownolad &&
+                        ((CANrxData[0] & 0x80U) == 0x80U)) ? true : false;
+
+            /* prepare response */
+            SDO->CANtxBuff->data[0] = 0xA2;
+            SDO->CANtxBuff->data[1] = SDO->sequence;
+
+            /* reset sequence on reception break */
+            if (state == CO_SDO_ST_DOWNLOAD_BL_SUB_RESP) {
+                SDO->sequence = 0U;
+            }
+
+            /* empty buffer in domain data type if not last segment */
+            if ((SDO->ODF_arg.ODdataStorage == 0) && (SDO->bufferOffset != 0) && !lastSegmentInSubblock){
+                /* calculate CRC on next bytes, if enabled */
+                if (SDO->crcEnabled){
+                    SDO->crc = crc16_ccitt(SDO->ODF_arg.data, SDO->bufferOffset, SDO->crc);
+                }
+
+                /* write data to the Object dictionary */
+                SDO->ODF_arg.lastSegment = false;
+                abortCode = CO_SDO_writeOD(SDO, SDO->bufferOffset);
+                if (abortCode != 0U){
+                    CO_SDO_abort(SDO, abortCode);
+                    return -1;
+                }
+
+                SDO->ODF_arg.dataLength = CO_CONFIG_SDO_BUFFER_SIZE;
+                SDO->bufferOffset = 0U;
+            }
+
+            /* blksize */
+            len = CO_CONFIG_SDO_BUFFER_SIZE - SDO->bufferOffset;
+            SDO->blksize = (len > (7*127)) ? 127 : (len / 7);
+            SDO->CANtxBuff->data[2] = SDO->blksize;
+
+            /* set next state */
+            if (lastSegmentInSubblock) {
+                SDO->state = CO_SDO_ST_DOWNLOAD_BL_END;
+            }
+            else if (SDO->bufferOffset >= CO_CONFIG_SDO_BUFFER_SIZE) {
+                CO_SDO_abort(SDO, CO_SDO_AB_DEVICE_INCOMPAT);
+                return -1;
+            }
+            else {
+                SDO->state = CO_SDO_ST_DOWNLOAD_BL_SUBBLOCK;
+            }
+
+            /* send response */
+            sendResponse = true;
+
+            break;
+        }
+
+        case CO_SDO_ST_DOWNLOAD_BL_END:{
+            /* verify client command specifier and subcommand */
+            if ((CANrxData[0]&0xE1U) != 0xC1U){
+                CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */
+                return -1;
+            }
+
+            /* number of bytes in the last segment of the last block that do not contain data. */
+            len = (CANrxData[0]>>2U) & 0x07U;
+            SDO->bufferOffset -= len;
+
+            /* calculate and verify CRC, if enabled */
+            if (SDO->crcEnabled){
+                uint16_t crc;
+                SDO->crc = crc16_ccitt(SDO->ODF_arg.data, SDO->bufferOffset, SDO->crc);
+
+                CO_memcpySwap2(&crc, &CANrxData[1]);
+
+                if (SDO->crc != crc){
+                    CO_SDO_abort(SDO, CO_SDO_AB_CRC);   /* CRC error (block mode only). */
+                    return -1;
+                }
+            }
+
+            /* write data to the Object dictionary */
+            SDO->ODF_arg.lastSegment = true;
+            abortCode = CO_SDO_writeOD(SDO, SDO->bufferOffset);
+            if (abortCode != 0U){
+                CO_SDO_abort(SDO, abortCode);
+                return -1;
+            }
+
+            /* finish the communication and run mainline processing again */
+            SDO->CANtxBuff->data[0] = 0xA1;
+            SDO->state = CO_SDO_ST_IDLE;
+            sendResponse = true;
+#if (CO_CONFIG_SDO) & CO_CONFIG_FLAG_TIMERNEXT
+            if (timerNext_us != NULL)
+                *timerNext_us = 0;
+#endif
+            break;
+        }
+
+        case CO_SDO_ST_UPLOAD_INITIATE:{
+            /* default response */
+            SDO->CANtxBuff->data[1] = CANrxData[1];
+            SDO->CANtxBuff->data[2] = CANrxData[2];
+            SDO->CANtxBuff->data[3] = CANrxData[3];
+
+            /* Expedited transfer */
+            if (SDO->ODF_arg.dataLength <= 4U){
+                for(i=0U; i<SDO->ODF_arg.dataLength; i++)
+                    SDO->CANtxBuff->data[4U+i] = SDO->ODF_arg.data[i];
+
+                SDO->CANtxBuff->data[0] = 0x43U | ((4U-SDO->ODF_arg.dataLength) << 2U);
+
+                /* finish the communication and run mainline processing again */
+                SDO->state = CO_SDO_ST_IDLE;
+                sendResponse = true;
+#if (CO_CONFIG_SDO) & CO_CONFIG_FLAG_TIMERNEXT
+                if (timerNext_us != NULL)
+                    *timerNext_us = 0;
+#endif
+            }
+
+            /* Segmented transfer */
+            else{
+                SDO->bufferOffset = 0U;
+                SDO->sequence = 0U;
+                SDO->state = CO_SDO_ST_UPLOAD_SEGMENTED;
+
+                /* indicate data size, if known */
+                if (SDO->ODF_arg.dataLengthTotal != 0U){
+                    uint32_t dlentot = SDO->ODF_arg.dataLengthTotal;
+                    CO_memcpySwap4(&SDO->CANtxBuff->data[4], &dlentot);
+                    SDO->CANtxBuff->data[0] = 0x41U;
+                }
+                else{
+                    SDO->CANtxBuff->data[0] = 0x40U;
+                }
+
+                /* send response */
+                sendResponse = true;
+            }
+            break;
+        }
+
+        case CO_SDO_ST_UPLOAD_SEGMENTED:{
+            /* verify client command specifier */
+            if ((CANrxData[0]&0xE0U) != 0x60U){
+                CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */
+                return -1;
+            }
+
+            /* verify toggle bit */
+            i = ((CANrxData[0]&0x10U) != 0) ? 1U : 0U;
+            if (i != SDO->sequence){
+                CO_SDO_abort(SDO, CO_SDO_AB_TOGGLE_BIT);/* toggle bit not alternated */
+                return -1;
+            }
+
+            /* calculate length to be sent */
+            len = SDO->ODF_arg.dataLength - SDO->bufferOffset;
+            if (len > 7U) len = 7U;
+
+            /* If data type is domain, re-fill the data buffer if neccessary and indicated so. */
+            if ((SDO->ODF_arg.ODdataStorage == 0) && (len < 7U) && (!SDO->ODF_arg.lastSegment)){
+                /* copy previous data to the beginning */
+                for(i=0U; i<len; i++){
+                    SDO->ODF_arg.data[i] = SDO->ODF_arg.data[SDO->bufferOffset+i];
+                }
+
+                /* move the beginning of the data buffer */
+                SDO->ODF_arg.data += len;
+                SDO->ODF_arg.dataLength = CO_OD_getLength(SDO, SDO->entryNo, SDO->ODF_arg.subIndex) - len;
+
+                /* read next data from Object dictionary function */
+                abortCode = CO_SDO_readOD(SDO, CO_CONFIG_SDO_BUFFER_SIZE);
+                if (abortCode != 0U){
+                    CO_SDO_abort(SDO, abortCode);
+                    return -1;
+                }
+
+                /* return to the original data buffer */
+                SDO->ODF_arg.data -= len;
+                SDO->ODF_arg.dataLength +=  len;
+                SDO->bufferOffset = 0;
+
+                /* re-calculate the length */
+                len = SDO->ODF_arg.dataLength;
+                if (len > 7U) len = 7U;
+            }
+
+            /* fill response data bytes */
+            for(i=0U; i<len; i++)
+                SDO->CANtxBuff->data[i+1] = SDO->ODF_arg.data[SDO->bufferOffset++];
+
+            /* first response byte */
+            SDO->CANtxBuff->data[0] = 0x00 | (SDO->sequence ? 0x10 : 0x00) | ((7-len)<<1);
+            SDO->sequence = (SDO->sequence) ? 0 : 1;
+
+            /* verify end of transfer */
+            if ((SDO->bufferOffset == SDO->ODF_arg.dataLength) && (SDO->ODF_arg.lastSegment)){
+                SDO->CANtxBuff->data[0] |= 0x01;
+
+                /* finish the communication and run mainline processing again */
+                SDO->state = CO_SDO_ST_IDLE;
+#if (CO_CONFIG_SDO) & CO_CONFIG_FLAG_TIMERNEXT
+                if (timerNext_us != NULL)
+                    *timerNext_us = 0;
+#endif
+            }
+
+            /* send response */
+            sendResponse = true;
+            break;
+        }
+
+        case CO_SDO_ST_UPLOAD_BL_INITIATE:{
+            /* default response */
+            SDO->CANtxBuff->data[1] = CANrxData[1];
+            SDO->CANtxBuff->data[2] = CANrxData[2];
+            SDO->CANtxBuff->data[3] = CANrxData[3];
+
+            /* calculate CRC, if enabled */
+            if ((CANrxData[0] & 0x04U) != 0U){
+                SDO->crcEnabled = true;
+                SDO->crc = crc16_ccitt(SDO->ODF_arg.data, SDO->ODF_arg.dataLength, 0);
+            }
+            else{
+                SDO->crcEnabled = false;
+                SDO->crc = 0;
+            }
+
+            /* Number of segments per block */
+            SDO->blksize = CANrxData[4];
+
+            /* verify client subcommand */
+            if ((CANrxData[0]&0x03U) != 0x00U){
+                CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */
+                return -1;
+            }
+
+            /* verify blksize and if SDO data buffer is large enough */
+            if ((SDO->blksize < 1U) || (SDO->blksize > 127U) ||
+               (((SDO->blksize*7U) > SDO->ODF_arg.dataLength) && (!SDO->ODF_arg.lastSegment))){
+                CO_SDO_abort(SDO, CO_SDO_AB_BLOCK_SIZE); /* Invalid block size (block mode only). */
+                return -1;
+            }
+
+            /* indicate data size, if known */
+            if (SDO->ODF_arg.dataLengthTotal != 0U){
+                uint32_t dlentot = SDO->ODF_arg.dataLengthTotal;
+                CO_memcpySwap4(&SDO->CANtxBuff->data[4], &dlentot);
+                SDO->CANtxBuff->data[0] = 0xC6U;
+            }
+            else{
+                SDO->CANtxBuff->data[0] = 0xC4U;
+            }
+
+            /* send response */
+            SDO->state = CO_SDO_ST_UPLOAD_BL_INITIATE_2;
+            sendResponse = true;
+            break;
+        }
+
+        case CO_SDO_ST_UPLOAD_BL_INITIATE_2:{
+            /* verify client command specifier and subcommand */
+            if ((CANrxData[0]&0xE3U) != 0xA3U){
+                CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */
+                return -1;
+            }
+
+            SDO->bufferOffset = 0U;
+            SDO->sequence = 0U;
+            SDO->endOfTransfer = false;
+            CO_SDO_process_done(SDO, timerNext_us);
+            isNew = false;
+            SDO->state = CO_SDO_ST_UPLOAD_BL_SUBBLOCK;
+            /* continue in next case */
+        }
+        // fallthrough
+
+        case CO_SDO_ST_UPLOAD_BL_SUBBLOCK:{
+            /* is block confirmation received */
+            if (isNew){
+                uint8_t ackseq;
+                uint16_t j;
+
+                /* verify client command specifier and subcommand */
+                if ((CANrxData[0]&0xE3U) != 0xA2U){
+                    CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */
+                    return -1;
+                }
+
+                ackseq = CANrxData[1];   /* sequence number of the last segment, that was received correctly. */
+
+                /* verify if response is too early */
+                if (ackseq > SDO->sequence){
+                    CO_SDO_abort(SDO, CO_SDO_AB_SEQ_NUM); /* Invalid sequence */
+                    return -1;
+                }
+
+                /* end of transfer */
+                if ((SDO->endOfTransfer) && (ackseq == SDO->blksize)){
+                    /* first response byte */
+                    SDO->CANtxBuff->data[0] = 0xC1 | ((7 - SDO->lastLen) << 2);
+
+                    /* CRC */
+                    if (SDO->crcEnabled)
+                        CO_memcpySwap2(&SDO->CANtxBuff->data[1], &SDO->crc);
+
+                    SDO->state = CO_SDO_ST_UPLOAD_BL_END;
+
+                    /* send response */
+                    sendResponse = true;
+                    break;
+                }
+
+                /* move remaining data to the beginning */
+                for(i=ackseq*7, j=0; i<SDO->ODF_arg.dataLength; i++, j++)
+                    SDO->ODF_arg.data[j] = SDO->ODF_arg.data[i];
+
+                /* set remaining data length in buffer */
+                SDO->ODF_arg.dataLength -= ackseq * 7U;
+
+                /* new block size */
+                SDO->blksize = CANrxData[2];
+
+                /* If data type is domain, re-fill the data buffer if necessary and indicated so. */
+                if ((SDO->ODF_arg.ODdataStorage == 0) && (SDO->ODF_arg.dataLength < (SDO->blksize*7U)) && (!SDO->ODF_arg.lastSegment)){
+                    /* move the beginning of the data buffer */
+                    len = SDO->ODF_arg.dataLength; /* length of valid data in buffer */
+                    SDO->ODF_arg.data += len;
+                    SDO->ODF_arg.dataLength = CO_OD_getLength(SDO, SDO->entryNo, SDO->ODF_arg.subIndex) - len;
+
+                    /* read next data from Object dictionary function */
+                    abortCode = CO_SDO_readOD(SDO, CO_CONFIG_SDO_BUFFER_SIZE);
+                    if (abortCode != 0U){
+                        CO_SDO_abort(SDO, abortCode);
+                        return -1;
+                    }
+
+                    /* calculate CRC on next bytes, if enabled */
+                    if (SDO->crcEnabled){
+                        SDO->crc = crc16_ccitt(SDO->ODF_arg.data, SDO->ODF_arg.dataLength, SDO->crc);
+                    }
+
+                  /* return to the original data buffer */
+                    SDO->ODF_arg.data -= len;
+                    SDO->ODF_arg.dataLength +=  len;
+                }
+
+                /* verify if SDO data buffer is large enough */
+                if (((SDO->blksize*7U) > SDO->ODF_arg.dataLength) && (!SDO->ODF_arg.lastSegment)){
+                    CO_SDO_abort(SDO, CO_SDO_AB_BLOCK_SIZE); /* Invalid block size (block mode only). */
+                    return -1;
+                }
+
+                SDO->bufferOffset = 0U;
+                SDO->sequence = 0U;
+                SDO->endOfTransfer = false;
+            }
+
+            /* return, if all segments was already transfered or on end of transfer */
+            if ((SDO->sequence == SDO->blksize) || (SDO->endOfTransfer)){
+                break;
+            }
+
+            /* reset timeout */
+            SDO->timeoutTimer = 0;
+
+            /* calculate length to be sent */
+            len = SDO->ODF_arg.dataLength - SDO->bufferOffset;
+            if (len > 7U){
+                len = 7U;
+            }
+
+            /* fill response data bytes */
+            for(i=0U; i<len; i++){
+                SDO->CANtxBuff->data[i+1] = SDO->ODF_arg.data[SDO->bufferOffset++];
+            }
+
+            /* first response byte */
+            SDO->CANtxBuff->data[0] = ++SDO->sequence;
+
+            /* verify end of transfer */
+            if ((SDO->bufferOffset == SDO->ODF_arg.dataLength) && (SDO->ODF_arg.lastSegment)){
+                SDO->CANtxBuff->data[0] |= 0x80;
+                SDO->lastLen = len;
+                SDO->blksize = SDO->sequence;
+                SDO->endOfTransfer = true;
+            }
+
+            /* send response */
+            sendResponse = true;
+
+#if (CO_CONFIG_SDO) & CO_CONFIG_FLAG_TIMERNEXT
+            /* Inform OS to call this function again without delay. */
+            if (timerNext_us != NULL) {
+                *timerNext_us = 0;
+            }
+#endif
+
+            break;
+        }
+
+        case CO_SDO_ST_UPLOAD_BL_END:{
+            /* verify client command specifier */
+            if ((CANrxData[0]&0xE1U) != 0xA1U){
+                CO_SDO_abort(SDO, CO_SDO_AB_CMD);/* Client command specifier not valid or unknown. */
+                return -1;
+            }
+
+            /* finish the communication and run mainline processing again */
+            SDO->state = CO_SDO_ST_IDLE;
+#if (CO_CONFIG_SDO) & CO_CONFIG_FLAG_TIMERNEXT
+            if (timerNext_us != NULL)
+                *timerNext_us = 0;
+#endif
+            break;
+        }
+
+        case CO_SDO_ST_IDLE:
+        {
+            /* Nothing to do, this never happens. */
+            break;
+        }
+
+        default:{
+            CO_SDO_abort(SDO, CO_SDO_AB_DEVICE_INCOMPAT);/* general internal incompatibility in the device */
+            return -1;
+        }
+    }
+
+    /* free receive buffer if it is not empty */
+    CO_SDO_process_done(SDO, timerNext_us);
+
+    /* send message */
+    if (sendResponse) {
+        CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff);
+    }
+
+    if (SDO->state != CO_SDO_ST_IDLE){
+        return 1;
+    }
+
+    return 0;
+}

controller_yy_app/middleware/CANopenNode/301/CO_SDOserver.h

@@ -0,0 +1,1165 @@
+/**
+ * CANopen Service Data Object - server protocol.
+ *
+ * @file        CO_SDOserver.h
+ * @ingroup     CO_SDOserver
+ * @author      Janez Paternoster
+ * @copyright   2004 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+
+#ifndef CO_SDO_SERVER_H
+#define CO_SDO_SERVER_H
+
+#include <string.h>
+
+#include "301/CO_driver.h"
+
+/* default configuration, see CO_config.h */
+#ifndef CO_CONFIG_SDO_SRV
+#define CO_CONFIG_SDO_SRV (CO_CONFIG_SDO_SRV_SEGMENTED)
+#endif
+#ifndef CO_CONFIG_SDO_SRV_BUFFER_SIZE
+#define CO_CONFIG_SDO_SRV_BUFFER_SIZE 32
+#endif
+
+#define CO_CONFIG_SDO CO_CONFIG_SDO_SRV
+#define CO_CONFIG_SDO_BUFFER_SIZE CO_CONFIG_SDO_SRV_BUFFER_SIZE
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_SDOserver SDO server
+ * @ingroup CO_CANopen_301
+ * @{
+ *
+ * CANopen Service Data Object - server protocol.
+ *
+ * Service data objects (SDOs) allow the access to any entry of the CANopen
+ * Object dictionary. An SDO establishes a peer-to-peer communication channel
+ * between two devices. In addition, the SDO protocol enables to transfer any
+ * amount of data in a segmented way. Therefore the SDO protocol is mainly used
+ * in order to communicate configuration data.
+ *
+ * All CANopen devices must have implemented SDO server and first SDO server
+ * channel. Servers serves data from Object dictionary. Object dictionary
+ * is a collection of variables, arrays or records (structures), which can be
+ * used by the stack or by the application. This file (CO_SDOserver.h)
+ * implements SDO server.
+ *
+ * SDO client can be (optionally) implemented on one (or multiple, if multiple
+ * SDO channels are used) device in CANopen network. Usually this is master
+ * device and provides also some kind of user interface, so configuration of
+ * the network is possible. Code for the SDO client is in file CO_SDOclient.h.
+ *
+ * SDO communication cycle is initiated by the client. Client can upload (read)
+ * data from device or can download (write) data to device. If data size is less
+ * or equal to 4 bytes, communication is finished by one server response
+ * (expedited transfer). If data size is longer, data are split into multiple
+ * segments of request/response pairs (normal or segmented transfer). For longer
+ * data there is also a block transfer protocol, which transfers larger block of
+ * data in secure way with little protocol overhead. If error occurs during SDO
+ * transfer #CO_SDO_abortCode_t is send by client or server and transfer is
+ * terminated. For more details see #CO_SDO_state_t.
+ */
+
+
+/**
+ * Internal state flags indicate type of transfer
+ *
+ * These flags correspond to the upper nibble of the SDO state machine states
+ * and can be used to determine the type of state an SDO object is in.
+ */
+#define CO_SDO_ST_FLAG_DOWNLOAD     0x10U
+#define CO_SDO_ST_FLAG_UPLOAD       0x20U
+#define CO_SDO_ST_FLAG_BLOCK        0x40U
+
+/**
+ * Internal states of the SDO state machine.
+ *
+ * Upper nibble of byte indicates type of state:
+ * 0x10: Download
+ * 0x20: Upload
+ * 0x40: Block Mode
+ *
+ * Note: CANopen has little endian byte order.
+ */
+typedef enum {
+/**
+ * - SDO client may start new download to or upload from specified node,
+ * specified index and specified subindex. It can start normal or block
+ * communication.
+ * - SDO server is waiting for client request. */
+CO_SDO_ST_IDLE = 0x00U,
+/**
+ * - SDO client or server may send SDO abort message in case of error:
+ *  - byte 0: @b 10000000 binary.
+ *  - byte 1..3: Object index and subIndex.
+ *  - byte 4..7: #CO_SDO_abortCode_t. */
+CO_SDO_ST_ABORT = 0x01U,
+
+/**
+ * - SDO client: Node-ID of the SDO server is the same as node-ID of this node,
+ *   SDO client is the same device as SDO server. Transfer data directly without
+ *   communication on CAN.
+ * - SDO server does not use this state. */
+CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER = 0x10U,
+/**
+ * - SDO client initiates SDO download:
+ *  - byte 0: @b 0010nnes binary: (nn: if e=s=1, number of data bytes, that do
+ *    @b not contain data; e=1 for expedited transfer; s=1 if data size is
+ *    indicated.)
+ *  - byte 1..3: Object index and subIndex.
+ *  - byte 4..7: If e=1, expedited data are here. If e=0 s=1, size of data for
+ *    segmented transfer is indicated here.
+ * - SDO server is in #CO_SDO_ST_IDLE state and waits for client request. */
+CO_SDO_ST_DOWNLOAD_INITIATE_REQ = 0x11U,
+/**
+ * - SDO client waits for response.
+ * - SDO server responses:
+ *  - byte 0: @b 01100000 binary.
+ *  - byte 1..3: Object index and subIndex.
+ *  - byte 4..7: Reserved.
+ * - In case of expedited transfer communication ends here. */
+CO_SDO_ST_DOWNLOAD_INITIATE_RSP = 0x12U,
+/**
+ * - SDO client sends SDO segment:
+ *  - byte 0: @b 000tnnnc binary: (t: toggle bit, set to 0 in first segment;
+ *    nnn: number of data bytes, that do @b not contain data; c=1 if this is the
+ *    last segment).
+ *  - byte 1..7: Data segment.
+ * - SDO server waits for segment. */
+CO_SDO_ST_DOWNLOAD_SEGMENT_REQ = 0x13U,
+/**
+ * - SDO client waits for response.
+ * - SDO server responses:
+ *  - byte 0: @b 001t0000 binary: (t: toggle bit, set to 0 in first segment).
+ *  - byte 1..7: Reserved.
+ * - If c was set to 1, then communication ends here. */
+CO_SDO_ST_DOWNLOAD_SEGMENT_RSP = 0x14U,
+
+/**
+ * - SDO client: Node-ID of the SDO server is the same as node-ID of this node,
+ *   SDO client is the same device as SDO server. Transfer data directly without
+ *   communication on CAN.
+ * - SDO server does not use this state. */
+CO_SDO_ST_UPLOAD_LOCAL_TRANSFER = 0x20U,
+/**
+ * - SDO client initiates SDO upload:
+ *  - byte 0: @b 01000000 binary.
+ *  - byte 1..3: Object index and subIndex.
+ *  - byte 4..7: Reserved.
+ * - SDO server is in #CO_SDO_ST_IDLE state and waits for client request. */
+CO_SDO_ST_UPLOAD_INITIATE_REQ = 0x21U,
+/**
+ * - SDO client waits for response.
+ * - SDO server responses:
+ *  - byte 0: @b 0100nnes binary: (nn: if e=s=1, number of data bytes, that do
+ *    @b not contain data; e=1 for expedited transfer; s=1 if data size is
+ *    indicated).
+ *  - byte 1..3: Object index and subIndex.
+ *  - byte 4..7: If e=1, expedited data are here. If e=0 s=1, size of data for
+ *    segmented transfer is indicated here.
+ * - In case of expedited transfer communication ends here. */
+CO_SDO_ST_UPLOAD_INITIATE_RSP = 0x22U,
+/**
+ * - SDO client requests SDO segment:
+ *  - byte 0: @b 011t0000 binary: (t: toggle bit, set to 0 in first segment).
+ *  - byte 1..7: Reserved.
+ * - SDO server waits for segment request. */
+CO_SDO_ST_UPLOAD_SEGMENT_REQ = 0x23U,
+/**
+ * - SDO client waits for response.
+ * - SDO server responses with data:
+ *  - byte 0: @b 000tnnnc binary: (t: toggle bit, set to 0 in first segment;
+ *    nnn: number of data bytes, that do @b not contain data; c=1 if this is the
+ *    last segment).
+ *  - byte 1..7: Data segment.
+ * - If c is set to 1, then communication ends here. */
+CO_SDO_ST_UPLOAD_SEGMENT_RSP = 0x24U,
+
+/**
+ * - SDO client initiates SDO block download:
+ *  - byte 0: @b 11000rs0 binary: (r=1 if client supports generating CRC on
+ *    data; s=1 if data size is indicated.)
+ *  - byte 1..3: Object index and subIndex.
+ *  - byte 4..7: If s=1, then size of data for block download is indicated here.
+ * - SDO server is in #CO_SDO_ST_IDLE state and waits for client request. */
+CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ = 0x51U,
+/**
+ * - SDO client waits for response.
+ * - SDO server responses:
+ *  - byte 0: @b 10100r00 binary: (r=1 if server supports generating CRC on
+ *    data.)
+ *  - byte 1..3: Object index and subIndex.
+ *  - byte 4: blksize: Number of segments per block that shall be used by the
+ *    client for the following block download with 0 < blksize < 128.
+ *  - byte 5..7: Reserved. */
+CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP = 0x52U,
+/**
+ * - SDO client sends 'blksize' segments of data in sequence:
+ *  - byte 0: @b cnnnnnnn binary: (c=1 if no more segments to be downloaded,
+ *    enter SDO block download end phase; nnnnnnn is sequence number of segment,
+ *    1..127.
+ *  - byte 1..7: At most 7 bytes of segment data to be downloaded.
+ * - SDO server reads sequence of 'blksize' blocks. */
+CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ = 0x53U,
+/**
+ * - SDO client waits for response.
+ * - SDO server responses:
+ *  - byte 0: @b 10100010 binary.
+ *  - byte 1: ackseq: sequence number of last segment that was received
+ *    successfully during the last block download. If ackseq is set to 0 the
+ *    server indicates the client that the segment with the sequence number 1
+ *    was not received correctly and all segments shall be retransmitted by the
+ *    client.
+ *  - byte 2: Number of segments per block that shall be used by the client for
+ *    the following block download with 0 < blksize < 128.
+ *  - byte 3..7: Reserved.
+ * - If c was set to 1, then communication enters SDO block download end phase.
+ */
+CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP = 0x54U,
+/**
+ * - SDO client sends SDO block download end:
+ *  - byte 0: @b 110nnn01 binary: (nnn: number of data bytes, that do @b not
+ *    contain data)
+ *  - byte 1..2: 16 bit CRC for the data set, if enabled by client and server.
+ *  - byte 3..7: Reserved.
+ * - SDO server waits for client request. */
+CO_SDO_ST_DOWNLOAD_BLK_END_REQ = 0x55U,
+/**
+ * - SDO client waits for response.
+ * - SDO server responses:
+ *  - byte 0: @b 10100001 binary.
+ *  - byte 1..7: Reserved.
+ * - Block download successfully ends here.
+ */
+CO_SDO_ST_DOWNLOAD_BLK_END_RSP = 0x56U,
+
+/**
+ * - SDO client initiates SDO block upload:
+ *  - byte 0: @b 10100r00 binary: (r=1 if client supports generating CRC on
+ *    data.)
+ *  - byte 1..3: Object index and subIndex.
+ *  - byte 4: blksize: Number of segments per block with 0 < blksize < 128.
+ *  - byte 5: pst - protocol switch threshold. If pst > 0 and size of the data
+ *    in bytes is less or equal pst, then the server may switch to the SDO
+ *    upload protocol #CO_SDO_ST_UPLOAD_INITIATE_RSP.
+ *  - byte 6..7: Reserved.
+ * - SDO server is in #CO_SDO_ST_IDLE state and waits for client request. */
+CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ = 0x61U,
+/**
+ * - SDO client waits for response.
+ * - SDO server responses:
+ *  - byte 0: @b 11000rs0 binary: (r=1 if server supports generating CRC on
+ *    data; s=1 if data size is indicated. )
+ *  - byte 1..3: Object index and subIndex.
+ *  - byte 4..7: If s=1, then size of data for block upload is indicated here.
+ * - If enabled by pst, then server may alternatively response with
+ *   #CO_SDO_ST_UPLOAD_INITIATE_RSP */
+CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP = 0x62U,
+/**
+ * - SDO client sends second initiate for SDO block upload:
+ *  - byte 0: @b 10100011 binary.
+ *  - byte 1..7: Reserved.
+ * - SDO server waits for client request. */
+CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2 = 0x63U,
+/**
+ * - SDO client reads sequence of 'blksize' blocks.
+ * - SDO server sends 'blksize' segments of data in sequence:
+ *  - byte 0: @b cnnnnnnn binary: (c=1 if no more segments to be uploaded,
+ *    enter SDO block upload end phase; nnnnnnn is sequence number of segment,
+ *    1..127.
+ *  - byte 1..7: At most 7 bytes of segment data to be uploaded. */
+CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ = 0x64U,
+/**
+ * - SDO client responses:
+ *  - byte 0: @b 10100010 binary.
+ *  - byte 1: ackseq: sequence number of last segment that was received
+ *    successfully during the last block upload. If ackseq is set to 0 the
+ *    client indicates the server that the segment with the sequence number 1
+ *    was not received correctly and all segments shall be retransmitted by the
+ *    server.
+ *  - byte 2: Number of segments per block that shall be used by the server for
+ *    the following block upload with 0 < blksize < 128.
+ *  - byte 3..7: Reserved.
+ * - SDO server waits for response.
+ * - If c was set to 1, then communication enters SDO block upload end phase. */
+CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP = 0x65U,
+/**
+ * - SDO client waits for server request.
+ * - SDO server sends SDO block upload end:
+ *  - byte 0: @b 110nnn01 binary: (nnn: number of data bytes, that do @b not
+ *    contain data)
+ *  - byte 1..2: 16 bit CRC for the data set, if enabled by client and server.
+ *  - byte 3..7: Reserved. */
+CO_SDO_ST_UPLOAD_BLK_END_SREQ = 0x66U,
+/**
+ * - SDO client responses:
+ *  - byte 0: @b 10100001 binary.
+ *  - byte 1..7: Reserved.
+ * - SDO server waits for response.
+ * - Block download successfully ends here. Note that this communication ends
+ *   with client response. Client may then start next SDO communication
+ *   immediately.
+ */
+CO_SDO_ST_UPLOAD_BLK_END_CRSP = 0x67U,
+
+/* old state names, will be removed */
+CO_SDO_ST_DOWNLOAD_INITIATE = 0xA1U,
+CO_SDO_ST_DOWNLOAD_SEGMENTED = 0xA2U,
+CO_SDO_ST_DOWNLOAD_BL_INITIATE = 0xA4U,
+CO_SDO_ST_DOWNLOAD_BL_SUBBLOCK = 0xA5U,
+CO_SDO_ST_DOWNLOAD_BL_SUB_RESP = 0xA6U,
+CO_SDO_ST_DOWNLOAD_BL_SUB_RESP_2 = 0xA7U,
+CO_SDO_ST_DOWNLOAD_BL_END = 0xA8U,
+CO_SDO_ST_UPLOAD_INITIATE = 0xB1U,
+CO_SDO_ST_UPLOAD_SEGMENTED = 0xB2U,
+CO_SDO_ST_UPLOAD_BL_INITIATE = 0xB4U,
+CO_SDO_ST_UPLOAD_BL_INITIATE_2 = 0xB5U,
+CO_SDO_ST_UPLOAD_BL_SUBBLOCK = 0xB6U,
+CO_SDO_ST_UPLOAD_BL_END = 0xB7U
+} CO_SDO_state_t;
+
+
+/**
+ * SDO abort codes.
+ *
+ * Send with Abort SDO transfer message.
+ *
+ * The abort codes not listed here are reserved.
+ */
+typedef enum{
+    CO_SDO_AB_NONE                  = 0x00000000UL, /**< 0x00000000, No abort */
+    CO_SDO_AB_TOGGLE_BIT            = 0x05030000UL, /**< 0x05030000, Toggle bit not altered */
+    CO_SDO_AB_TIMEOUT               = 0x05040000UL, /**< 0x05040000, SDO protocol timed out */
+    CO_SDO_AB_CMD                   = 0x05040001UL, /**< 0x05040001, Command specifier not valid or unknown */
+    CO_SDO_AB_BLOCK_SIZE            = 0x05040002UL, /**< 0x05040002, Invalid block size in block mode */
+    CO_SDO_AB_SEQ_NUM               = 0x05040003UL, /**< 0x05040003, Invalid sequence number in block mode */
+    CO_SDO_AB_CRC                   = 0x05040004UL, /**< 0x05040004, CRC error (block mode only) */
+    CO_SDO_AB_OUT_OF_MEM            = 0x05040005UL, /**< 0x05040005, Out of memory */
+    CO_SDO_AB_UNSUPPORTED_ACCESS    = 0x06010000UL, /**< 0x06010000, Unsupported access to an object */
+    CO_SDO_AB_WRITEONLY             = 0x06010001UL, /**< 0x06010001, Attempt to read a write only object */
+    CO_SDO_AB_READONLY              = 0x06010002UL, /**< 0x06010002, Attempt to write a read only object */
+    CO_SDO_AB_NOT_EXIST             = 0x06020000UL, /**< 0x06020000, Object does not exist in the object dictionary */
+    CO_SDO_AB_NO_MAP                = 0x06040041UL, /**< 0x06040041, Object cannot be mapped to the PDO */
+    CO_SDO_AB_MAP_LEN               = 0x06040042UL, /**< 0x06040042, Number and length of object to be mapped exceeds PDO length */
+    CO_SDO_AB_PRAM_INCOMPAT         = 0x06040043UL, /**< 0x06040043, General parameter incompatibility reasons */
+    CO_SDO_AB_DEVICE_INCOMPAT       = 0x06040047UL, /**< 0x06040047, General internal incompatibility in device */
+    CO_SDO_AB_HW                    = 0x06060000UL, /**< 0x06060000, Access failed due to hardware error */
+    CO_SDO_AB_TYPE_MISMATCH         = 0x06070010UL, /**< 0x06070010, Data type does not match, length of service parameter does not match */
+    CO_SDO_AB_DATA_LONG             = 0x06070012UL, /**< 0x06070012, Data type does not match, length of service parameter too high */
+    CO_SDO_AB_DATA_SHORT            = 0x06070013UL, /**< 0x06070013, Data type does not match, length of service parameter too short */
+    CO_SDO_AB_SUB_UNKNOWN           = 0x06090011UL, /**< 0x06090011, Sub index does not exist */
+    CO_SDO_AB_INVALID_VALUE         = 0x06090030UL, /**< 0x06090030, Invalid value for parameter (download only). */
+    CO_SDO_AB_VALUE_HIGH            = 0x06090031UL, /**< 0x06090031, Value range of parameter written too high */
+    CO_SDO_AB_VALUE_LOW             = 0x06090032UL, /**< 0x06090032, Value range of parameter written too low */
+    CO_SDO_AB_MAX_LESS_MIN          = 0x06090036UL, /**< 0x06090036, Maximum value is less than minimum value. */
+    CO_SDO_AB_NO_RESOURCE           = 0x060A0023UL, /**< 0x060A0023, Resource not available: SDO connection */
+    CO_SDO_AB_GENERAL               = 0x08000000UL, /**< 0x08000000, General error */
+    CO_SDO_AB_DATA_TRANSF           = 0x08000020UL, /**< 0x08000020, Data cannot be transferred or stored to application */
+    CO_SDO_AB_DATA_LOC_CTRL         = 0x08000021UL, /**< 0x08000021, Data cannot be transferred or stored to application because of local control */
+    CO_SDO_AB_DATA_DEV_STATE        = 0x08000022UL, /**< 0x08000022, Data cannot be transferred or stored to application because of present device state */
+    CO_SDO_AB_DATA_OD               = 0x08000023UL, /**< 0x08000023, Object dictionary not present or dynamic generation fails */
+    CO_SDO_AB_NO_DATA               = 0x08000024UL  /**< 0x08000024, No data available */
+}CO_SDO_abortCode_t;
+
+
+/**
+ * @defgroup CO_SDO_objectDictionary Object dictionary
+ *
+ * CANopen Object dictionary implementation in CANopenNode.
+ *
+ * CANopen Object dictionary is a collection of different data items, which can
+ * be used by the stack or by the application.
+ *
+ * Each Object dictionary entry is located under 16-bit index, as specified
+ * by the CANopen:
+ *  - 0x0001..0x025F: Data type definitions.
+ *  - 0x1000..0x1FFF: Communication profile area.
+ *  - 0x2000..0x5FFF: Manufacturer-specific profile area.
+ *  - 0x6000..0x9FFF: Standardized device profile area for eight logical devices.
+ *  - 0xA000..0xAFFF: Standardized network variable area.
+ *  - 0xB000..0xBFFF: Standardized system variable area.
+ *  - Other:          Reserved.
+ *
+ * If Object dictionary entry has complex data type (array or structure),
+ * then 8-bit subIndex specifies the sub-member of the entry. In that case
+ * subIndex 0x00 is encoded as uint8_t and specifies the highest available
+ * subIndex with that entry. Subindex 0xFF has special meaning in the standard
+ * and is not supported by CANopenNode.
+ *
+ * ####Object type of one Object dictionary entry
+ *  - NULL:         Not used by CANopenNode.
+ *  - DOMAIN:       Block of data of variable length. Data and length are
+ *                  under control of the application.
+ *  - DEFTYPE:      Definition of CANopen basic data type, for example
+ *                  INTEGER16.
+ *  - DEFSTRUCT:    Definition of complex data type - structure, which is
+ *                  used with RECORD.
+ *  - VAR:          Variable of CANopen basic data type. Subindex is 0.
+ *  - ARRAY:        Array of multiple variables of the same CANopen basic
+ *                  data type. Subindex 1..arrayLength specifies sub-member.
+ *  - RECORD:       Record or structure of multiple variables of different
+ *                  CANopen basic data type. Subindex specifies sub-member.
+ *
+ *
+ * ####Implementation in CANopenNode
+ * Object dictionary in CANopenNode is implemented in CO_OD.h and CO_OD.c files.
+ * These files are application specific and must be generated by Object
+ * dictionary editor (application is included by the stack).
+ *
+ * CO_OD.h and CO_OD.c files include:
+ *  - Structure definitions for records.
+ *  - Global declaration and initialization of all variables, arrays and records
+ *    mapped to Object dictionary. Variables are distributed in multiple objects,
+ *    depending on memory location. This eases storage to different memories in
+ *    microcontroller, like eeprom or flash.
+ *  - Constant array of multiple Object dictionary entries of type
+ *    CO_OD_entry_t. If object type is record, then entry includes additional
+ *    constant array with members of type CO_OD_entryRecord_t. Each OD entry
+ *    includes information: index, maxSubIndex, #CO_SDO_OD_attributes_t, data size and
+ *    pointer to variable.
+ *
+ *
+ * Function CO_SDO_init() initializes object CO_SDO_t, which includes SDO
+ * server and Object dictionary.
+ *
+ * Application doesn't need to know anything about the Object dictionary. It can
+ * use variables specified in CO_OD.h file directly. If it needs more control
+ * over the CANopen communication with the variables, it can configure additional
+ * functionality with function CO_OD_configure(). Additional functionality
+ * include: @ref CO_SDO_OD_function and #CO_SDO_OD_flags_t.
+ *
+ * Interface to Object dictionary is provided by following functions: CO_OD_find()
+ * finds OD entry by index, CO_OD_getLength() returns length of variable,
+ * CO_OD_getAttribute returns attribute and CO_OD_getDataPointer() returns pointer
+ * to data. These functions are used by SDO server and by PDO configuration. They
+ * can also be used to access the OD by index like this.
+ *
+ * \code{.c}
+ * index = CO_OD_find(CO->SDO[0], OD_H1001_ERR_REG);
+ * if (index == 0xffff) {
+ *     return;
+ * }
+ * length = CO_OD_getLength(CO->SDO[0], index, 1);
+ * if (length != sizeof(new_data)) {
+ *    return;
+ * }
+ *
+ * p = CO_OD_getDataPointer(CO->SDO[0], index, 1);
+ * if (p == NULL) {
+ *     return;
+ * }
+ * CO_LOCK_OD();
+ * *p = new_data;
+ * CO_UNLOCK_OD();
+ * \endcode
+ *
+ * Be aware that accessing the OD directly using CO_OD.h files is more CPU
+ * efficient as CO_OD_find() has to do a search everytime it is called.
+ *
+ */
+
+
+/**
+ * @defgroup CO_SDO_OD_function Object Dictionary function
+ *
+ * Optional application specific function, which may manipulate data downloaded
+ * or uploaded via SDO.
+ *
+ * Object dictionary function is external function defined by application or
+ * by other stack files. It may be registered for specific Object dictionary
+ * entry (with specific index). If it is registered, it is called (through
+ * function pointer) from SDO server. It may verify and manipulate data during
+ * SDO transfer. Object dictionary function can be registered by function
+ * CO_OD_configure().
+ *
+ * ####SDO download (writing to Object dictionary)
+ *     After SDO client transfers data to the server, data are stored in internal
+ *     buffer. If data contains multibyte variable and processor is big endian,
+ *     then data bytes are swapped. Object dictionary function is called if
+ *     registered. Data may be verified and manipulated inside that function. After
+ *     function exits, data are copied to location as specified in CO_OD_entry_t.
+ *
+ * ####SDO upload (reading from Object dictionary)
+ *     Before start of SDO upload, data are read from Object dictionary into
+ *     internal buffer. If necessary, bytes are swapped.
+ *     Object dictionary function is called if registered. Data may be
+ *     manipulated inside that function. After function exits, data are
+ *     transferred via SDO server.
+ *
+ * ####Domain data type
+ *     If data type is domain, then length is not specified by Object dictionary.
+ *     In that case Object dictionary function must be used. In case of
+ *     download it must store the data in own location. In case of upload it must
+ *     write the data (maximum size is specified by length) into data buffer and
+ *     specify actual length. With domain data type it is possible to transfer
+ *     data, which are longer than #CO_CONFIG_SDO_BUFFER_SIZE. In that case
+ *     Object dictionary function is called multiple times between SDO transfer.
+ *
+ * ####Parameter to function:
+ *     ODF_arg     - Pointer to CO_ODF_arg_t object filled before function call.
+ *
+ * ####Return from function:
+ *  - 0: Data transfer is successful
+ *  - Different than 0: Failure. See #CO_SDO_abortCode_t.
+ */
+
+
+/**
+ * Size of fifo queue for SDO received messages.
+ *
+ * If block transfers are used size of fifo queue should be more that 1 message
+ * to avoid possible drops in consecutive SDO block upload transfers.
+ * To increase performance, value can be set to 1 if block transfers are not used
+ *
+ * Min value is 1.
+ */
+    #ifndef CO_SDO_RX_DATA_SIZE
+        #define CO_SDO_RX_DATA_SIZE   2
+    #endif
+
+
+/**
+ * Object Dictionary attributes. Bit masks for attribute in CO_OD_entry_t.
+ */
+typedef enum{
+    CO_ODA_MEM_ROM          = 0x0001U,  /**< Variable is located in ROM memory */
+    CO_ODA_MEM_RAM          = 0x0002U,  /**< Variable is located in RAM memory */
+    CO_ODA_MEM_EEPROM       = 0x0003U,  /**< Variable is located in EEPROM memory */
+    CO_ODA_READABLE         = 0x0004U,  /**< SDO server may read from the variable */
+    CO_ODA_WRITEABLE        = 0x0008U,  /**< SDO server may write to the variable */
+    CO_ODA_RPDO_MAPABLE     = 0x0010U,  /**< Variable is mappable for RPDO */
+    CO_ODA_TPDO_MAPABLE     = 0x0020U,  /**< Variable is mappable for TPDO */
+    CO_ODA_TPDO_DETECT_COS  = 0x0040U,  /**< If variable is mapped to any PDO, then
+                                             PDO is automatically send, if variable
+                                             changes its value */
+    CO_ODA_MB_VALUE         = 0x0080U   /**< True when variable is a multibyte value */
+}CO_SDO_OD_attributes_t;
+
+
+/**
+ * Common DS301 object dictionary entries.
+ */
+typedef enum{
+    OD_H1000_DEV_TYPE             = 0x1000U,/**< Device type */
+    OD_H1001_ERR_REG              = 0x1001U,/**< Error register */
+    OD_H1002_MANUF_STATUS_REG     = 0x1002U,/**< Manufacturer status register */
+    OD_H1003_PREDEF_ERR_FIELD     = 0x1003U,/**< Predefined error field */
+    OD_H1004_RSV                  = 0x1004U,/**< Reserved */
+    OD_H1005_COBID_SYNC           = 0x1005U,/**< Sync message cob-id */
+    OD_H1006_COMM_CYCL_PERIOD     = 0x1006U,/**< Communication cycle period */
+    OD_H1007_SYNC_WINDOW_LEN      = 0x1007U,/**< Sync windows length */
+    OD_H1008_MANUF_DEV_NAME       = 0x1008U,/**< Manufacturer device name */
+    OD_H1009_MANUF_HW_VERSION     = 0x1009U,/**< Manufacturer hardware version */
+    OD_H100A_MANUF_SW_VERSION     = 0x100AU,/**< Manufacturer software version */
+    OD_H100B_RSV                  = 0x100BU,/**< Reserved */
+    OD_H100C_GUARD_TIME           = 0x100CU,/**< Guard time */
+    OD_H100D_LIFETIME_FACTOR      = 0x100DU,/**< Life time factor */
+    OD_H100E_RSV                  = 0x100EU,/**< Reserved */
+    OD_H100F_RSV                  = 0x100FU,/**< Reserved */
+    OD_H1010_STORE_PARAM_FUNC     = 0x1010U,/**< Store parameter in persistent memory function */
+    OD_H1011_REST_PARAM_FUNC      = 0x1011U,/**< Restore default parameter function */
+    OD_H1012_COBID_TIME           = 0x1012U,/**< Timestamp message cob-id */
+    OD_H1013_HIGH_RES_TIMESTAMP   = 0x1013U,/**< High resolution timestamp */
+    OD_H1014_COBID_EMERGENCY      = 0x1014U,/**< Emergency message cob-id */
+    OD_H1015_INHIBIT_TIME_MSG     = 0x1015U,/**< Inhibit time message */
+    OD_H1016_CONSUMER_HB_TIME     = 0x1016U,/**< Consumer heartbeat time */
+    OD_H1017_PRODUCER_HB_TIME     = 0x1017U,/**< Producer heartbeat time */
+    OD_H1018_IDENTITY_OBJECT      = 0x1018U,/**< Identity object */
+    OD_H1019_SYNC_CNT_OVERFLOW    = 0x1019U,/**< Sync counter overflow value */
+    OD_H1020_VERIFY_CONFIG        = 0x1020U,/**< Verify configuration */
+    OD_H1021_STORE_EDS            = 0x1021U,/**< Store EDS */
+    OD_H1022_STORE_FORMAT         = 0x1022U,/**< Store format */
+    OD_H1023_OS_CMD               = 0x1023U,/**< OS command */
+    OD_H1024_OS_CMD_MODE          = 0x1024U,/**< OS command mode */
+    OD_H1025_OS_DBG_INTERFACE     = 0x1025U,/**< OS debug interface */
+    OD_H1026_OS_PROMPT            = 0x1026U,/**< OS prompt */
+    OD_H1027_MODULE_LIST          = 0x1027U,/**< Module list */
+    OD_H1028_EMCY_CONSUMER        = 0x1028U,/**< Emergency consumer object */
+    OD_H1029_ERR_BEHAVIOR         = 0x1029U,/**< Error behaviour */
+    OD_H1200_SDO_SERVER_PARAM     = 0x1200U,/**< SDO server parameters */
+    OD_H1280_SDO_CLIENT_PARAM     = 0x1280U,/**< SDO client parameters */
+    OD_H1300_GFC_PARAM            = 0x1300U,/**< GFC parameter */
+    OD_H1301_SRDO_1_PARAM         = 0x1301U,/**< SRDO communication parameters */
+    OD_H1381_SRDO_1_MAPPING       = 0x1381U,/**< SRDO mapping parameters */
+    OD_H13FE_SRDO_VALID           = 0x13FEU,/**< SRDO valid flag */
+    OD_H13FF_SRDO_CHECKSUM        = 0x13FFU,/**< SRDO checksum */
+    OD_H1400_RXPDO_1_PARAM        = 0x1400U,/**< RXPDO communication parameter */
+    OD_H1401_RXPDO_2_PARAM        = 0x1401U,/**< RXPDO communication parameter */
+    OD_H1402_RXPDO_3_PARAM        = 0x1402U,/**< RXPDO communication parameter */
+    OD_H1403_RXPDO_4_PARAM        = 0x1403U,/**< RXPDO communication parameter */
+    OD_H1600_RXPDO_1_MAPPING      = 0x1600U,/**< RXPDO mapping parameters */
+    OD_H1601_RXPDO_2_MAPPING      = 0x1601U,/**< RXPDO mapping parameters */
+    OD_H1602_RXPDO_3_MAPPING      = 0x1602U,/**< RXPDO mapping parameters */
+    OD_H1603_RXPDO_4_MAPPING      = 0x1603U,/**< RXPDO mapping parameters */
+    OD_H1800_TXPDO_1_PARAM        = 0x1800U,/**< TXPDO communication parameter */
+    OD_H1801_TXPDO_2_PARAM        = 0x1801U,/**< TXPDO communication parameter */
+    OD_H1802_TXPDO_3_PARAM        = 0x1802U,/**< TXPDO communication parameter */
+    OD_H1803_TXPDO_4_PARAM        = 0x1803U,/**< TXPDO communication parameter */
+    OD_H1A00_TXPDO_1_MAPPING      = 0x1A00U,/**< TXPDO mapping parameters */
+    OD_H1A01_TXPDO_2_MAPPING      = 0x1A01U,/**< TXPDO mapping parameters */
+    OD_H1A02_TXPDO_3_MAPPING      = 0x1A02U,/**< TXPDO mapping parameters */
+    OD_H1A03_TXPDO_4_MAPPING      = 0x1A03U /**< TXPDO mapping parameters */
+}CO_ObjDicId_t;
+
+
+/**
+ * Bit masks for flags associated with variable from @ref CO_SDO_objectDictionary.
+ *
+ * This additional functionality of any variable in @ref CO_SDO_objectDictionary can be
+ * enabled by function CO_OD_configure(). Location of the flag byte can be
+ * get from function CO_OD_getFlagsPointer().
+ */
+typedef enum{
+    /** Variable was written by RPDO. Flag can be cleared by application */
+    CO_ODFL_RPDO_WRITTEN        = 0x01U,
+    /** Variable is mapped to TPDO */
+    CO_ODFL_TPDO_MAPPED         = 0x02U,
+    /** Change of state bit, initially copy of attribute from CO_OD_entry_t.
+    If set and variable is mapped to TPDO, TPDO will be automatically send,
+    if variable changed */
+    CO_ODFL_TPDO_COS_ENABLE     = 0x04U,
+    /** PDO send bit, can be set by application. If variable is mapped into
+    TPDO, TPDO will be send and bit will be cleared. */
+    CO_ODFL_TPDO_SEND           = 0x08U,
+    /** Variable was accessed by SDO download */
+    CO_ODFL_SDO_DOWNLOADED      = 0x10U,
+    /** Variable was accessed by SDO upload */
+    CO_ODFL_SDO_UPLOADED        = 0x20U,
+    /** Reserved */
+    CO_ODFL_BIT_6               = 0x40U,
+    /** Reserved */
+    CO_ODFL_BIT_7               = 0x80U
+}CO_SDO_OD_flags_t;
+
+
+/**
+ * Return values from SDO server or client functions.
+ */
+typedef enum {
+    /** Data buffer is full.
+     * SDO client: data must be read before next upload cycle begins. */
+    CO_SDO_RT_uploadDataBufferFull = 5,
+    /** CAN transmit buffer is full. Waiting. */
+    CO_SDO_RT_transmittBufferFull = 4,
+    /** Block download is in progress. Sending train of messages. */
+    CO_SDO_RT_blockDownldInProgress = 3,
+    /** Block upload is in progress. Receiving train of messages.
+     * SDO client: Data must not be read in this state. */
+    CO_SDO_RT_blockUploadInProgress = 2,
+    /** Waiting server or client response */
+    CO_SDO_RT_waitingResponse = 1,
+    /** Success, end of communication. SDO client: uploaded data must be read.*/
+    CO_SDO_RT_ok_communicationEnd = 0,
+    /** Error in arguments */
+    CO_SDO_RT_wrongArguments = -2,
+    /** Communication ended with client abort */
+    CO_SDO_RT_endedWithClientAbort = -9,
+    /** Communication ended with server abort */
+    CO_SDO_RT_endedWithServerAbort = -10,
+} CO_SDO_return_t;
+
+
+/**
+ * Object for one entry with specific index in @ref CO_SDO_objectDictionary.
+ */
+typedef struct {
+    /** The index of Object from 0x1000 to 0xFFFF */
+    uint16_t            index;
+    /** Number of (sub-objects - 1). If Object Type is variable, then
+    maxSubIndex is 0, otherwise maxSubIndex is equal or greater than 1. */
+    uint8_t             maxSubIndex;
+    /** If Object Type is record, attribute is set to zero. Attribute for
+    each member is then set in special array with members of type
+    CO_OD_entryRecord_t. If Object Type is Array, attribute is common for
+    all array members. See #CO_SDO_OD_attributes_t. */
+    uint16_t            attribute;
+    /** If Object Type is Variable, length is the length of variable in bytes.
+    If Object Type is Array, length is the length of one array member.
+    If Object Type is Record, length is zero. Length for each member is
+    set in special array with members of type CO_OD_entryRecord_t.
+    If Object Type is Domain, length is zero. Length is specified
+    by application in @ref CO_SDO_OD_function. */
+    uint16_t            length;
+    /** If Object Type is Variable, pData is pointer to data.
+    If Object Type is Array, pData is pointer to data. Data doesn't
+    include Sub-Object 0.
+    If object type is Record, pData is pointer to special array
+    with members of type CO_OD_entryRecord_t.
+    If object type is Domain, pData is null. */
+    void               *pData;
+}CO_OD_entry_t;
+
+
+/**
+ * Object for record type entry in @ref CO_SDO_objectDictionary.
+ *
+ * See CO_OD_entry_t.
+ */
+typedef struct{
+    /** Pointer to data. If object type is Domain, pData is null */
+    void               *pData;
+    /** See #CO_SDO_OD_attributes_t */
+    uint16_t            attribute;
+    /** Length of variable in bytes. If object type is Domain, length is zero */
+    uint16_t            length;
+}CO_OD_entryRecord_t;
+
+
+/**
+ * Object contains all information about the object being transferred by SDO server.
+ *
+ * Object is used as an argument to @ref CO_SDO_OD_function. It is also
+ * part of the CO_SDO_t object.
+ */
+typedef struct{
+    /** Informative parameter. It may point to object, which is connected
+    with this OD entry. It can be used inside @ref CO_SDO_OD_function, ONLY
+    if it was registered by CO_OD_configure() function before. */
+    void               *object;
+    /** SDO data buffer contains data, which are exchanged in SDO transfer.
+    @ref CO_SDO_OD_function may verify or manipulate that data before (after)
+    they are written to (read from) Object dictionary. Data have the same
+    endianes as processor. Pointer must NOT be changed. (Data up to length
+    can be changed.) */
+    uint8_t            *data;
+    /** Pointer to location in object dictionary, where data are stored.
+    (informative reference to old data, read only). Data have the same
+    endianes as processor. If data type is Domain, this variable is null. */
+    const void         *ODdataStorage;
+    /** Length of data in the above buffer. Read only, except for domain. If
+    data type is domain see @ref CO_SDO_OD_function for special rules by upload. */
+    uint16_t            dataLength;
+    /** Attribute of object in Object dictionary (informative, must NOT be changed). */
+    uint16_t            attribute;
+    /** Pointer to the #CO_SDO_OD_flags_t byte. */
+    uint8_t            *pFlags;
+    /** Index of object in Object dictionary (informative, must NOT be changed). */
+    uint16_t            index;
+    /** Subindex of object in Object dictionary (informative, must NOT be changed). */
+    uint8_t             subIndex;
+    /** True, if SDO upload is in progress, false if SDO download is in progress. */
+    bool_t              reading;
+    /** Used by domain data type. Indicates the first segment. Variable is informative. */
+    bool_t              firstSegment;
+    /** Used by domain data type. If false by download, then application will
+    receive more segments during SDO communication cycle. If uploading,
+    application may set variable to false, so SDO server will call
+    @ref CO_SDO_OD_function again for filling the next data. */
+    bool_t              lastSegment;
+    /** Used by domain data type. By upload @ref CO_SDO_OD_function may write total
+    data length, so this information will be send in SDO upload initiate phase. It
+    is not necessary to specify this variable. By download this variable contains
+    total data size, if size is indicated in SDO download initiate phase */
+    uint32_t            dataLengthTotal;
+    /** Used by domain data type. In case of multiple segments, this indicates the offset
+    into the buffer this segment starts at. */
+    uint32_t            offset;
+}CO_ODF_arg_t;
+
+
+/**
+ * Object is used as array inside CO_SDO_t, parallel to @ref CO_SDO_objectDictionary.
+ *
+ * Object is generated by function CO_OD_configure(). It is then used as
+ * extension to Object dictionary entry at specific index.
+ */
+typedef struct{
+    /** Pointer to @ref CO_SDO_OD_function */
+    CO_SDO_abortCode_t (*pODFunc)(CO_ODF_arg_t *ODF_arg);
+    /** Pointer to object, which will be passed to @ref CO_SDO_OD_function */
+    void               *object;
+    /** Pointer to #CO_SDO_OD_flags_t. If object type is array or record, this
+    variable points to array with length equal to number of subindexes. */
+    uint8_t            *flags;
+}CO_OD_extension_t;
+
+
+/**
+ * SDO server object.
+ */
+typedef struct{
+    /** FIFO queue of the received message 8 data bytes each */
+    uint8_t             CANrxData[CO_SDO_RX_DATA_SIZE][8];
+    /** SDO data buffer of size #CO_CONFIG_SDO_BUFFER_SIZE. */
+    uint8_t             databuffer[CO_CONFIG_SDO_BUFFER_SIZE];
+    /** Internal flag indicates, that this object has own OD */
+    bool_t              ownOD;
+    /** Pointer to the @ref CO_SDO_objectDictionary (array) */
+    const CO_OD_entry_t *OD;
+    /** Size of the @ref CO_SDO_objectDictionary */
+    uint16_t            ODSize;
+    /** Pointer to array of CO_OD_extension_t objects. Size of the array is
+    equal to ODSize. */
+    CO_OD_extension_t  *ODExtensions;
+    /** Offset in buffer of next data segment being read/written */
+    uint16_t            bufferOffset;
+    /** Sequence number of OD entry as returned from CO_OD_find() */
+    uint16_t            entryNo;
+    /** CO_ODF_arg_t object with additional variables. Reference to this object
+    is passed to @ref CO_SDO_OD_function */
+    CO_ODF_arg_t        ODF_arg;
+    /** From CO_SDO_init() */
+    uint8_t             nodeId;
+    /** Current internal state of the SDO server state machine #CO_SDO_state_t */
+    CO_SDO_state_t      state;
+    /** Toggle bit in segmented transfer or block sequence in block transfer */
+    uint8_t             sequence;
+    /** Maximum timeout time between request and response in microseconds. */
+    uint32_t            SDOtimeoutTime_us;
+    /** Timeout timer for SDO communication */
+    uint32_t            timeoutTimer;
+    /** Number of segments per block with 1 <= blksize <= 127 */
+    uint8_t             blksize;
+    /** True, if CRC calculation by block transfer is enabled */
+    bool_t              crcEnabled;
+    /** Calculated CRC code */
+    uint16_t            crc;
+    /** Length of data in the last segment in block upload */
+    uint8_t             lastLen;
+    /** Indication timeout in sub-block transfer */
+    bool_t              timeoutSubblockDownolad;
+    /** Indication end of block transfer */
+    bool_t              endOfTransfer;
+    /** Variables indicates, if new SDO message received from CAN bus */
+    volatile void      *CANrxNew[CO_SDO_RX_DATA_SIZE];
+    /** Index of CANrxData for new received SDO message */
+    uint8_t             CANrxRcv;
+    /** Index of CANrxData SDO message to processed */
+    uint8_t             CANrxProc;
+    /** Number of new SDO messages in CANrxData to process */
+    uint8_t             CANrxSize;
+#if ((CO_CONFIG_SDO) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+    /** From CO_SDO_initCallbackPre() or NULL */
+    void              (*pFunctSignalPre)(void *object);
+    /** From CO_SDO_initCallbackPre() or NULL */
+    void               *functSignalObjectPre;
+#endif
+    /** From CO_SDO_init() */
+    CO_CANmodule_t     *CANdevTx;
+    /** CAN transmit buffer inside CANdev for CAN tx message */
+    CO_CANtx_t         *CANtxBuff;
+}CO_SDO_t;
+
+
+/**
+ * Copy 2 data bytes from source to destination. Swap bytes if
+ * microcontroller is big-endian.
+ *
+ * @param dest Destination location.
+ * @param src Source location.
+ */
+#ifdef CO_LITTLE_ENDIAN
+#define CO_memcpySwap2(dest, src) memcpy(dest, src, 2)
+#endif
+#if defined CO_BIG_ENDIAN || defined CO_DOXYGEN
+static inline void CO_memcpySwap2(void* dest, const void* src){
+    char *cdest;
+    char *csrc;
+    cdest = (char *) dest;
+    csrc = (char *) src;
+    cdest[0] = csrc[1];
+    cdest[1] = csrc[0];
+}
+#endif
+
+
+/**
+ * Copy 4 data bytes from source to destination. Swap bytes if
+ * microcontroller is big-endian.
+ *
+ * @param dest Destination location.
+ * @param src Source location.
+ */
+#ifdef CO_LITTLE_ENDIAN
+#define CO_memcpySwap4(dest, src) memcpy(dest, src, 4)
+#endif
+#if defined CO_BIG_ENDIAN || defined CO_DOXYGEN
+static inline void CO_memcpySwap4(void* dest, const void* src){
+    char *cdest;
+    char *csrc;
+    cdest = (char *) dest;
+    csrc = (char *) src;
+    cdest[0] = csrc[3];
+    cdest[1] = csrc[2];
+    cdest[2] = csrc[1];
+    cdest[3] = csrc[0];
+}
+#endif
+
+
+/**
+ * Copy 8 data bytes from source to destination. Swap bytes if
+ * microcontroller is big-endian.
+ *
+ * @param dest Destination location.
+ * @param src Source location.
+ */
+#ifdef CO_LITTLE_ENDIAN
+#define CO_memcpySwap8(dest, src) memcpy(dest, src, 8)
+#endif
+#if defined CO_BIG_ENDIAN || defined CO_DOXYGEN
+static inline void CO_memcpySwap8(void* dest, const void* src){
+    char *cdest;
+    char *csrc;
+    cdest = (char *) dest;
+    csrc = (char *) src;
+    cdest[0] = csrc[7];
+    cdest[1] = csrc[6];
+    cdest[2] = csrc[5];
+    cdest[3] = csrc[4];
+    cdest[4] = csrc[3];
+    cdest[5] = csrc[2];
+    cdest[6] = csrc[1];
+    cdest[7] = csrc[0];
+}
+#endif
+
+
+/**
+ * Initialize SDO object.
+ *
+ * Function must be called in the communication reset section.
+ *
+ * @param SDO This object will be initialized.
+ * @param COB_IDClientToServer COB ID for client to server for this SDO object.
+ * @param COB_IDServerToClient COB ID for server to client for this SDO object.
+ * @param ObjDictIndex_SDOServerParameter Index in Object dictionary.
+ * @param parentSDO Pointer to SDO object, which contains object dictionary and
+ * its extension. For first (default) SDO object this argument must be NULL.
+ * If this argument is specified, then OD, ODSize and ODExtensions arguments
+ * are ignored.
+ * @param OD Pointer to @ref CO_SDO_objectDictionary array defined externally.
+ * @param ODSize Size of the above array.
+ * @param ODExtensions Pointer to the externally defined array of the same size
+ * as ODSize.
+ * @param nodeId CANopen Node ID of this device.
+ * @param SDOtimeoutTime_ms Timeout time for SDO communication in milliseconds.
+ * @param CANdevRx CAN device for SDO server reception.
+ * @param CANdevRxIdx Index of receive buffer in the above CAN device.
+ * @param CANdevTx CAN device for SDO server transmission.
+ * @param CANdevTxIdx Index of transmit buffer in the above CAN device.
+ *
+ * @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ */
+CO_ReturnError_t CO_SDO_init(
+        CO_SDO_t               *SDO,
+        uint32_t                COB_IDClientToServer,
+        uint32_t                COB_IDServerToClient,
+        uint16_t                ObjDictIndex_SDOServerParameter,
+        CO_SDO_t               *parentSDO,
+        const CO_OD_entry_t     OD[],
+        uint16_t                ODSize,
+        CO_OD_extension_t       ODExtensions[],
+        uint8_t                 nodeId,
+        uint16_t                SDOtimeoutTime_ms,
+        CO_CANmodule_t         *CANdevRx,
+        uint16_t                CANdevRxIdx,
+        CO_CANmodule_t         *CANdevTx,
+        uint16_t                CANdevTxIdx);
+
+
+#if ((CO_CONFIG_SDO) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+/**
+ * Initialize SDOrx callback function.
+ *
+ * Function initializes optional callback function, which should immediately
+ * start processing of CO_SDO_process() function.
+ * Callback is called after SDOserver message is received from the CAN bus or
+ * when new call without delay is necessary (SDO block transfer is in progress).
+ *
+ * @param SDO This object.
+ * @param object Pointer to object, which will be passed to pFunctSignalPre(). Can be NULL
+ * @param pFunctSignalPre Pointer to the callback function. Not called if NULL.
+ */
+void CO_SDO_initCallbackPre(
+        CO_SDO_t               *SDO,
+        void                   *object,
+        void                  (*pFunctSignalPre)(void *object));
+#endif
+
+
+/**
+ * Process SDO communication.
+ *
+ * Function must be called cyclically.
+ *
+ * @param SDO This object.
+ * @param NMTisPreOrOperational Different than zero, if #CO_NMT_internalState_t is
+ * NMT_PRE_OPERATIONAL or NMT_OPERATIONAL.
+ * @param timeDifference_us Time difference from previous function call in [microseconds].
+ * @param [out] timerNext_us info to OS - see CO_process().
+ *
+ * @return 0: SDO server is idle.
+ * @return 1: SDO server is in transfer state.
+ * @return -1: SDO abort just occurred.
+ */
+int8_t CO_SDO_process(
+        CO_SDO_t               *SDO,
+        bool_t                  NMTisPreOrOperational,
+        uint32_t                timeDifference_us,
+        uint32_t               *timerNext_us);
+
+
+/**
+ * Configure additional functionality to one @ref CO_SDO_objectDictionary entry.
+ *
+ * Additional functionality include: @ref CO_SDO_OD_function and
+ * #CO_SDO_OD_flags_t. It is optional feature and can be used on any object in
+ * Object dictionary. If OD entry does not exist, function returns silently.
+ *
+ * @param SDO This object.
+ * @param index Index of object in the Object dictionary.
+ * @param pODFunc Pointer to @ref CO_SDO_OD_function, specified by application.
+ * If NULL, @ref CO_SDO_OD_function will not be used on this object.
+ * @param object Pointer to object, which will be passed to @ref CO_SDO_OD_function.
+ * @param flags Pointer to array of #CO_SDO_OD_flags_t defined externally. If
+ * zero, #CO_SDO_OD_flags_t will not be used on this OD entry.
+ * @param flagsSize Size of the above array. It must be equal to number
+ * of sub-objects in object dictionary entry. Otherwise #CO_SDO_OD_flags_t will
+ * not be used on this OD entry.
+ */
+void CO_OD_configure(
+        CO_SDO_t               *SDO,
+        uint16_t                index,
+        CO_SDO_abortCode_t    (*pODFunc)(CO_ODF_arg_t *ODF_arg),
+        void                   *object,
+        uint8_t                *flags,
+        uint8_t                 flagsSize);
+
+
+/**
+ * Find object with specific index in Object dictionary.
+ *
+ * @param SDO This object.
+ * @param index Index of the object in Object dictionary.
+ *
+ * @return Sequence number of the @ref CO_SDO_objectDictionary entry, 0xFFFF if not found.
+ */
+uint16_t CO_OD_find(CO_SDO_t *SDO, uint16_t index);
+
+
+/**
+ * Get length of the given object with specific subIndex.
+ *
+ * @param SDO This object.
+ * @param entryNo Sequence number of OD entry as returned from CO_OD_find().
+ * @param subIndex Sub-index of the object in Object dictionary.
+ *
+ * @return Data length of the variable.
+ */
+uint16_t CO_OD_getLength(CO_SDO_t *SDO, uint16_t entryNo, uint8_t subIndex);
+
+
+/**
+ * Get attribute of the given object with specific subIndex. See #CO_SDO_OD_attributes_t.
+ *
+ * If Object Type is array and subIndex is zero, function always returns
+ * 'read-only' attribute. An exception to this rule is ID1003 (Error field).
+ * However, this is supposed to be only written by network.
+ *
+ * @param SDO This object.
+ * @param entryNo Sequence number of OD entry as returned from CO_OD_find().
+ * @param subIndex Sub-index of the object in Object dictionary.
+ *
+ * @return Attribute of the variable.
+ */
+uint16_t CO_OD_getAttribute(CO_SDO_t *SDO, uint16_t entryNo, uint8_t subIndex);
+
+
+/**
+ * Get pointer to data of the given object with specific subIndex.
+ *
+ * If Object Type is array and subIndex is zero, function returns pointer to
+ * object->maxSubIndex variable.
+ *
+ * @param SDO This object.
+ * @param entryNo Sequence number of OD entry as returned from CO_OD_find().
+ * @param subIndex Sub-index of the object in Object dictionary.
+ *
+ * @return Pointer to the variable in @ref CO_SDO_objectDictionary.
+ */
+void* CO_OD_getDataPointer(CO_SDO_t *SDO, uint16_t entryNo, uint8_t subIndex);
+
+
+/**
+ * Get pointer to the #CO_SDO_OD_flags_t byte of the given object with
+ * specific subIndex.
+ *
+ * @param SDO This object.
+ * @param entryNo Sequence number of OD entry as returned from CO_OD_find().
+ * @param subIndex Sub-index of the object in Object dictionary.
+ *
+ * @return Pointer to the #CO_SDO_OD_flags_t of the variable.
+ */
+uint8_t* CO_OD_getFlagsPointer(CO_SDO_t *SDO, uint16_t entryNo, uint8_t subIndex);
+
+
+/**
+ * Initialize SDO transfer.
+ *
+ * Find object in OD, verify, fill ODF_arg s.
+ *
+ * @param SDO This object.
+ * @param index Index of the object in Object dictionary.
+ * @param subIndex subIndex of the object in Object dictionary.
+ *
+ * @return 0 on success, otherwise #CO_SDO_abortCode_t.
+ */
+uint32_t CO_SDO_initTransfer(CO_SDO_t *SDO, uint16_t index, uint8_t subIndex);
+
+
+/**
+ * Read data from @ref CO_SDO_objectDictionary to internal buffer.
+ *
+ * ODF_arg s must be initialized before with CO_SDO_initTransfer().
+ * @ref CO_SDO_OD_function is called if configured.
+ *
+ * @param SDO This object.
+ * @param SDOBufferSize Total size of the SDO buffer.
+ *
+ * @return 0 on success, otherwise #CO_SDO_abortCode_t.
+ */
+uint32_t CO_SDO_readOD(CO_SDO_t *SDO, uint16_t SDOBufferSize);
+
+
+/**
+ * Write data from internal buffer to @ref CO_SDO_objectDictionary.
+ *
+ * ODF_arg s must be initialized before with CO_SDO_initTransfer().
+ * @ref CO_SDO_OD_function is called if configured.
+ *
+ * @param SDO This object.
+ * @param length Length of data (received from network) to write.
+ *
+ * @return 0 on success, otherwise #CO_SDO_abortCode_t.
+ */
+uint32_t CO_SDO_writeOD(CO_SDO_t *SDO, uint16_t length);
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+/** @} */
+#endif

controller_yy_app/middleware/CANopenNode/301/CO_SYNC.c

@@ -0,0 +1,446 @@
+/*
+ * CANopen SYNC object.
+ *
+ * @file        CO_SYNC.c
+ * @ingroup     CO_SYNC
+ * @author      Janez Paternoster
+ * @copyright   2004 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+
+#include "301/CO_SDOserver.h"
+#include "301/CO_Emergency.h"
+#include "301/CO_NMT_Heartbeat.h"
+#include "301/CO_SYNC.h"
+
+#if (CO_CONFIG_SYNC) & CO_CONFIG_SYNC_ENABLE
+
+/*
+ * Read received message from CAN module.
+ *
+ * Function will be called (by CAN receive interrupt) every time, when CAN
+ * message with correct identifier will be received. For more information and
+ * description of parameters see file CO_driver.h.
+ */
+static void CO_SYNC_receive(void *object, void *msg) {
+    CO_SYNC_t *SYNC;
+    CO_NMT_internalState_t operState;
+
+    SYNC = (CO_SYNC_t*)object;   /* this is the correct pointer type of the first argument */
+    operState = *SYNC->operatingState;
+
+    if ((operState == CO_NMT_OPERATIONAL) || (operState == CO_NMT_PRE_OPERATIONAL)){
+        uint8_t DLC = CO_CANrxMsg_readDLC(msg);
+
+        if (SYNC->counterOverflowValue == 0){
+            if (DLC == 0U){
+                CO_FLAG_SET(SYNC->CANrxNew);
+            }
+            else{
+                SYNC->receiveError = (uint16_t)DLC | 0x0100U;
+            }
+        }
+        else{
+            if (DLC == 1U){
+                uint8_t *data = CO_CANrxMsg_readData(msg);
+                SYNC->counter = data[0];
+                CO_FLAG_SET(SYNC->CANrxNew);
+            }
+            else{
+                SYNC->receiveError = (uint16_t)DLC | 0x0200U;
+            }
+        }
+        if (CO_FLAG_READ(SYNC->CANrxNew)) {
+            SYNC->CANrxToggle = SYNC->CANrxToggle ? false : true;
+
+#if (CO_CONFIG_SYNC) & CO_CONFIG_FLAG_CALLBACK_PRE
+            /* Optional signal to RTOS, which can resume task, which handles SYNC. */
+            if (SYNC->pFunctSignalPre != NULL) {
+                SYNC->pFunctSignalPre(SYNC->functSignalObjectPre);
+            }
+#endif
+        }
+    }
+}
+
+
+/*
+ * Function for accessing _COB ID SYNC Message_ (index 0x1005) from SDO server.
+ *
+ * For more information see file CO_SDOserver.h.
+ */
+static CO_SDO_abortCode_t CO_ODF_1005(CO_ODF_arg_t *ODF_arg){
+    CO_SYNC_t *SYNC;
+    uint32_t value;
+    CO_SDO_abortCode_t ret = CO_SDO_AB_NONE;
+
+    SYNC = (CO_SYNC_t*) ODF_arg->object;
+    value = CO_getUint32(ODF_arg->data);
+
+    if (!ODF_arg->reading){
+        uint8_t configureSyncProducer = 0;
+
+        /* only 11-bit CAN identifier is supported */
+        if (value & 0x20000000UL){
+            ret = CO_SDO_AB_INVALID_VALUE;
+        }
+        else{
+            /* is 'generate Sync messge' bit set? */
+            if (value & 0x40000000UL){
+                /* if bit was set before, value can not be changed */
+                if (SYNC->isProducer){
+                    ret = CO_SDO_AB_DATA_DEV_STATE;
+                }
+                else{
+                    configureSyncProducer = 1;
+                }
+            }
+        }
+
+        /* configure sync producer */
+        if (ret == CO_SDO_AB_NONE){
+            SYNC->COB_ID = (uint16_t)(value & 0x7FFU);
+
+            if (configureSyncProducer){
+                uint8_t len = 0U;
+                if (SYNC->counterOverflowValue != 0U){
+                    len = 1U;
+                    SYNC->counter = 0U;
+                    SYNC->timer = 0U;
+                }
+                SYNC->CANtxBuff = CO_CANtxBufferInit(
+                        SYNC->CANdevTx,         /* CAN device */
+                        SYNC->CANdevTxIdx,      /* index of specific buffer inside CAN module */
+                        SYNC->COB_ID,           /* CAN identifier */
+                        0,                      /* rtr */
+                        len,                    /* number of data bytes */
+                        0);                     /* synchronous message flag bit */
+
+                if (SYNC->CANtxBuff == NULL) {
+                    ret = CO_SDO_AB_DATA_DEV_STATE;
+                    SYNC->isProducer = false;
+                } else {
+                    SYNC->isProducer = true;
+                }
+            }
+            else{
+                SYNC->isProducer = false;
+            }
+        }
+
+        /* configure sync consumer */
+        if (ret == CO_SDO_AB_NONE) {
+            CO_ReturnError_t CANret = CO_CANrxBufferInit(
+                SYNC->CANdevRx,         /* CAN device */
+                SYNC->CANdevRxIdx,      /* rx buffer index */
+                SYNC->COB_ID,           /* CAN identifier */
+                0x7FF,                  /* mask */
+                0,                      /* rtr */
+                (void*)SYNC,            /* object passed to receive function */
+                CO_SYNC_receive);       /* this function will process received message */
+
+            if (CANret != CO_ERROR_NO) {
+                ret = CO_SDO_AB_DATA_DEV_STATE;
+            }
+        }
+    }
+
+    return ret;
+}
+
+
+/*
+ * Function for accessing _Communication cycle period_ (index 0x1006) from SDO server.
+ *
+ * For more information see file CO_SDOserver.h.
+ */
+static CO_SDO_abortCode_t CO_ODF_1006(CO_ODF_arg_t *ODF_arg){
+    CO_SYNC_t *SYNC;
+    uint32_t value;
+    CO_SDO_abortCode_t ret = CO_SDO_AB_NONE;
+
+    SYNC = (CO_SYNC_t*) ODF_arg->object;
+    value = CO_getUint32(ODF_arg->data);
+
+    if (!ODF_arg->reading){
+        /* period transition from 0 to something */
+        if ((SYNC->periodTime == 0) && (value != 0)){
+            SYNC->counter = 0;
+        }
+
+        SYNC->periodTime = value;
+        SYNC->periodTimeoutTime = (value / 2U) * 3U;
+        /* overflow? */
+        if (SYNC->periodTimeoutTime < value){
+            SYNC->periodTimeoutTime = 0xFFFFFFFFUL;
+        }
+
+        SYNC->timer = 0;
+    }
+
+    return ret;
+}
+
+
+/**
+ * Function for accessing _Synchronous counter overflow value_ (index 0x1019) from SDO server.
+ *
+ * For more information see file CO_SDOserver.h.
+ */
+static CO_SDO_abortCode_t CO_ODF_1019(CO_ODF_arg_t *ODF_arg){
+    CO_SYNC_t *SYNC;
+    uint8_t value;
+    CO_SDO_abortCode_t ret = CO_SDO_AB_NONE;
+
+    SYNC = (CO_SYNC_t*) ODF_arg->object;
+    value = ODF_arg->data[0];
+
+    if (!ODF_arg->reading){
+        uint8_t len = 0U;
+
+        if (SYNC->periodTime){
+            ret = CO_SDO_AB_DATA_DEV_STATE;
+        }
+        else if ((value == 1) || (value > 240)){
+            ret = CO_SDO_AB_INVALID_VALUE;
+        }
+        else{
+            SYNC->counterOverflowValue = value;
+            if (value != 0){
+                len = 1U;
+            }
+
+            SYNC->CANtxBuff = CO_CANtxBufferInit(
+                    SYNC->CANdevTx,         /* CAN device */
+                    SYNC->CANdevTxIdx,      /* index of specific buffer inside CAN module */
+                    SYNC->COB_ID,           /* CAN identifier */
+                    0,                      /* rtr */
+                    len,                    /* number of data bytes */
+                    0);                     /* synchronous message flag bit */
+
+            if (SYNC->CANtxBuff == NULL) {
+                ret = CO_SDO_AB_DATA_DEV_STATE;
+            }
+        }
+    }
+
+    return ret;
+}
+
+
+/******************************************************************************/
+CO_ReturnError_t CO_SYNC_init(
+        CO_SYNC_t              *SYNC,
+        CO_EM_t                *em,
+        CO_SDO_t               *SDO,
+        CO_NMT_internalState_t *operatingState,
+        uint32_t                COB_ID_SYNCMessage,
+        uint32_t                communicationCyclePeriod,
+        uint8_t                 synchronousCounterOverflowValue,
+        CO_CANmodule_t         *CANdevRx,
+        uint16_t                CANdevRxIdx,
+        CO_CANmodule_t         *CANdevTx,
+        uint16_t                CANdevTxIdx)
+{
+    uint8_t len = 0;
+    CO_ReturnError_t ret = CO_ERROR_NO;
+
+    /* verify arguments */
+    if (SYNC==NULL || em==NULL || SDO==NULL || operatingState==NULL ||
+        CANdevRx==NULL || CANdevTx==NULL){
+        return CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    /* Configure object variables */
+    SYNC->isProducer = (COB_ID_SYNCMessage&0x40000000L) ? true : false;
+    SYNC->COB_ID = COB_ID_SYNCMessage&0x7FF;
+
+    SYNC->periodTime = communicationCyclePeriod;
+    SYNC->periodTimeoutTime = communicationCyclePeriod / 2 * 3;
+    /* overflow? */
+    if (SYNC->periodTimeoutTime < communicationCyclePeriod) SYNC->periodTimeoutTime = 0xFFFFFFFFL;
+
+    SYNC->counterOverflowValue = synchronousCounterOverflowValue;
+    if (synchronousCounterOverflowValue) len = 1;
+
+    SYNC->curentSyncTimeIsInsideWindow = true;
+
+    CO_FLAG_CLEAR(SYNC->CANrxNew);
+    SYNC->CANrxToggle = false;
+    SYNC->timer = 0;
+    SYNC->counter = 0;
+    SYNC->receiveError = 0U;
+
+    SYNC->em = em;
+    SYNC->operatingState = operatingState;
+
+    SYNC->CANdevRx = CANdevRx;
+    SYNC->CANdevRxIdx = CANdevRxIdx;
+
+#if (CO_CONFIG_SYNC) & CO_CONFIG_FLAG_CALLBACK_PRE
+    SYNC->pFunctSignalPre = NULL;
+    SYNC->functSignalObjectPre = NULL;
+#endif
+
+    /* Configure Object dictionary entry at index 0x1005, 0x1006 and 0x1019 */
+    CO_OD_configure(SDO, OD_H1005_COBID_SYNC,        CO_ODF_1005, (void*)SYNC, 0, 0);
+    CO_OD_configure(SDO, OD_H1006_COMM_CYCL_PERIOD,  CO_ODF_1006, (void*)SYNC, 0, 0);
+    CO_OD_configure(SDO, OD_H1019_SYNC_CNT_OVERFLOW, CO_ODF_1019, (void*)SYNC, 0, 0);
+
+    /* configure SYNC CAN reception */
+    ret = CO_CANrxBufferInit(
+            CANdevRx,               /* CAN device */
+            CANdevRxIdx,            /* rx buffer index */
+            SYNC->COB_ID,           /* CAN identifier */
+            0x7FF,                  /* mask */
+            0,                      /* rtr */
+            (void*)SYNC,            /* object passed to receive function */
+            CO_SYNC_receive);       /* this function will process received message */
+
+    /* configure SYNC CAN transmission */
+    SYNC->CANdevTx = CANdevTx;
+    SYNC->CANdevTxIdx = CANdevTxIdx;
+    SYNC->CANtxBuff = CO_CANtxBufferInit(
+            CANdevTx,               /* CAN device */
+            CANdevTxIdx,            /* index of specific buffer inside CAN module */
+            SYNC->COB_ID,           /* CAN identifier */
+            0,                      /* rtr */
+            len,                    /* number of data bytes */
+            0);                     /* synchronous message flag bit */
+
+    if (SYNC->CANtxBuff == NULL) {
+        ret = CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    return ret;
+}
+
+
+#if (CO_CONFIG_SYNC) & CO_CONFIG_FLAG_CALLBACK_PRE
+/******************************************************************************/
+void CO_SYNC_initCallbackPre(
+        CO_SYNC_t              *SYNC,
+        void                   *object,
+        void                  (*pFunctSignalPre)(void *object))
+{
+    if (SYNC != NULL){
+        SYNC->functSignalObjectPre = object;
+        SYNC->pFunctSignalPre = pFunctSignalPre;
+    }
+}
+#endif
+
+/******************************************************************************/
+CO_ReturnError_t CO_SYNCsend(CO_SYNC_t *SYNC){
+    if (++SYNC->counter > SYNC->counterOverflowValue) SYNC->counter = 1;
+    SYNC->timer = 0;
+    SYNC->CANrxToggle = SYNC->CANrxToggle ? false : true;
+    SYNC->CANtxBuff->data[0] = SYNC->counter;
+    return CO_CANsend(SYNC->CANdevTx, SYNC->CANtxBuff);
+}
+
+/******************************************************************************/
+CO_SYNC_status_t CO_SYNC_process(
+        CO_SYNC_t              *SYNC,
+        uint32_t                timeDifference_us,
+        uint32_t                ObjDict_synchronousWindowLength,
+        uint32_t               *timerNext_us)
+{
+    (void)timerNext_us; /* may be unused */
+
+    CO_SYNC_status_t ret = CO_SYNC_NONE;
+    uint32_t timerNew;
+
+    if (*SYNC->operatingState == CO_NMT_OPERATIONAL || *SYNC->operatingState == CO_NMT_PRE_OPERATIONAL){
+        /* update sync timer, no overflow */
+        timerNew = SYNC->timer + timeDifference_us;
+        if (timerNew > SYNC->timer) SYNC->timer = timerNew;
+
+        /* was SYNC just received */
+        if (CO_FLAG_READ(SYNC->CANrxNew)){
+            SYNC->timer = 0;
+            ret = CO_SYNC_RECEIVED;
+            CO_FLAG_CLEAR(SYNC->CANrxNew);
+        }
+
+        /* SYNC producer */
+        if (SYNC->isProducer && SYNC->periodTime){
+            if (SYNC->timer >= SYNC->periodTime){
+                ret = CO_SYNC_RECEIVED;
+                CO_SYNCsend(SYNC);
+            }
+#if (CO_CONFIG_SYNC) & CO_CONFIG_FLAG_TIMERNEXT
+            /* Calculate when next SYNC needs to be sent */
+            if (timerNext_us != NULL){
+                uint32_t diff = SYNC->periodTime - SYNC->timer;
+                if (*timerNext_us > diff){
+                    *timerNext_us = diff;
+                }
+            }
+#endif
+        }
+
+        /* Synchronous PDOs are allowed only inside time window */
+        if (ObjDict_synchronousWindowLength){
+            if (SYNC->timer > ObjDict_synchronousWindowLength){
+                if (SYNC->curentSyncTimeIsInsideWindow){
+                    ret = CO_SYNC_OUTSIDE_WINDOW;
+                }
+                SYNC->curentSyncTimeIsInsideWindow = false;
+            }
+            else{
+                SYNC->curentSyncTimeIsInsideWindow = true;
+            }
+        }
+        else{
+            SYNC->curentSyncTimeIsInsideWindow = true;
+        }
+
+        /* Verify timeout of SYNC */
+        if (SYNC->periodTime && (*SYNC->operatingState == CO_NMT_OPERATIONAL || *SYNC->operatingState == CO_NMT_PRE_OPERATIONAL)){
+            if (SYNC->timer > SYNC->periodTimeoutTime) {
+                CO_errorReport(SYNC->em, CO_EM_SYNC_TIME_OUT, CO_EMC_COMMUNICATION, SYNC->timer);
+            }
+            else {
+                CO_errorReset(SYNC->em, CO_EM_SYNC_TIME_OUT, CO_EMC_COMMUNICATION);
+#if (CO_CONFIG_SYNC) & CO_CONFIG_FLAG_TIMERNEXT
+                if (timerNext_us != NULL) {
+                    uint32_t diff = SYNC->periodTimeoutTime - SYNC->timer;
+                    if (*timerNext_us > diff){
+                        *timerNext_us = diff;
+                    }
+                }
+#endif
+            }
+        }
+    }
+    else {
+        CO_FLAG_CLEAR(SYNC->CANrxNew);
+    }
+
+    /* verify error from receive function */
+    if (SYNC->receiveError != 0U){
+        CO_errorReport(SYNC->em, CO_EM_SYNC_LENGTH, CO_EMC_SYNC_DATA_LENGTH, (uint32_t)SYNC->receiveError);
+        SYNC->receiveError = 0U;
+    }
+
+    return ret;
+}
+
+#endif /* (CO_CONFIG_SYNC) & CO_CONFIG_SYNC_ENABLE */

controller_yy_app/middleware/CANopenNode/301/CO_SYNC.h

@@ -0,0 +1,223 @@
+/**
+ * CANopen Synchronisation protocol.
+ *
+ * @file        CO_SYNC.h
+ * @ingroup     CO_SYNC
+ * @author      Janez Paternoster
+ * @copyright   2004 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CO_SYNC_H
+#define CO_SYNC_H
+
+#include "301/CO_driver.h"
+
+/* default configuration, see CO_config.h */
+#ifndef CO_CONFIG_SYNC
+#define CO_CONFIG_SYNC (CO_CONFIG_SYNC_ENABLE | CO_CONFIG_SYNC_PRODUCER)
+#endif
+
+#if ((CO_CONFIG_SYNC) & CO_CONFIG_SYNC_ENABLE) || defined CO_DOXYGEN
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_SYNC SYNC
+ * @ingroup CO_CANopen_301
+ * @{
+ *
+ * CANopen Synchronisation protocol.
+ *
+ * For CAN identifier see #CO_Default_CAN_ID_t
+ *
+ * SYNC message is used for synchronization of the nodes on network. One node
+ * can be SYNC producer, others can be SYNC consumers. Synchronous TPDOs are
+ * transmitted after the CANopen SYNC message. Synchronous received PDOs are
+ * accepted(copied to OD) immediatelly after the reception of the next SYNC
+ * message.
+ *
+ * ####Contents of SYNC message
+ * By default SYNC message has no data. If _Synchronous counter overflow value_
+ * from Object dictionary (index 0x1019) is different than 0, SYNC message has
+ * one data byte: _counter_ incremented by 1 with every SYNC transmission.
+ *
+ * ####SYNC in CANopenNode
+ * According to CANopen, synchronous RPDOs must be processed after reception of
+ * the next sync messsage. For that reason, there is a double receive buffer
+ * for each synchronous RPDO. At the moment, when SYNC is received or
+ * transmitted, internal variable CANrxToggle toggles. That variable is then
+ * used by synchronous RPDO to determine, which of the two buffers is used for
+ * RPDO reception and which for RPDO processing.
+ */
+
+
+/**
+ * SYNC producer and consumer object.
+ */
+typedef struct{
+    CO_EM_t            *em;             /**< From CO_SYNC_init() */
+    CO_NMT_internalState_t *operatingState; /**< From CO_SYNC_init() */
+    /** True, if device is SYNC producer. Calculated from _COB ID SYNC Message_
+    variable from Object dictionary (index 0x1005). */
+    bool_t              isProducer;
+    /** COB_ID of SYNC message. Calculated from _COB ID SYNC Message_
+    variable from Object dictionary (index 0x1005). */
+    uint16_t            COB_ID;
+    /** Sync period time in [microseconds]. Calculated from _Communication cycle period_
+    variable from Object dictionary (index 0x1006). */
+    uint32_t            periodTime;
+    /** Sync period timeout time in [microseconds].
+    (periodTimeoutTime = periodTime * 1,5) */
+    uint32_t            periodTimeoutTime;
+    /** Value from _Synchronous counter overflow value_ variable from Object
+    dictionary (index 0x1019) */
+    uint8_t             counterOverflowValue;
+    /** True, if current time is inside synchronous window.
+    In this case synchronous PDO may be sent. */
+    bool_t              curentSyncTimeIsInsideWindow;
+    /** Indicates, if new SYNC message received from CAN bus */
+    volatile void      *CANrxNew;
+    /** Variable toggles, if new SYNC message received from CAN bus */
+    bool_t              CANrxToggle;
+    /** Counter of the SYNC message if counterOverflowValue is different than zero */
+    uint8_t             counter;
+    /** Timer for the SYNC message in [microseconds].
+    Set to zero after received or transmitted SYNC message */
+    uint32_t            timer;
+    /** Set to nonzero value, if SYNC with wrong data length is received from CAN */
+    uint16_t            receiveError;
+#if ((CO_CONFIG_SYNC) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+    /** From CO_SYNC_initCallbackPre() or NULL */
+    void              (*pFunctSignalPre)(void *object);
+    /** From CO_SYNC_initCallbackPre() or NULL */
+    void               *functSignalObjectPre;
+#endif
+    CO_CANmodule_t     *CANdevRx;       /**< From CO_SYNC_init() */
+    uint16_t            CANdevRxIdx;    /**< From CO_SYNC_init() */
+    CO_CANmodule_t     *CANdevTx;       /**< From CO_SYNC_init() */
+    CO_CANtx_t         *CANtxBuff;      /**< CAN transmit buffer inside CANdevTx */
+    uint16_t            CANdevTxIdx;    /**< From CO_SYNC_init() */
+}CO_SYNC_t;
+
+
+/** Return value for #CO_SYNC_process */
+typedef enum {
+    CO_SYNC_NONE            = 0, /**< SYNC not received */
+    CO_SYNC_RECEIVED        = 1, /**< SYNC received */
+    CO_SYNC_OUTSIDE_WINDOW  = 2  /**< SYNC received outside SYNC window */
+} CO_SYNC_status_t;
+
+
+/**
+ * Initialize SYNC object.
+ *
+ * Function must be called in the communication reset section.
+ *
+ * @param SYNC This object will be initialized.
+ * @param em Emergency object.
+ * @param SDO SDO server object.
+ * @param operatingState Pointer to variable indicating CANopen device NMT internal state.
+ * @param COB_ID_SYNCMessage From Object dictionary (index 0x1005).
+ * @param communicationCyclePeriod From Object dictionary (index 0x1006).
+ * @param synchronousCounterOverflowValue From Object dictionary (index 0x1019).
+ * @param CANdevRx CAN device for SYNC reception.
+ * @param CANdevRxIdx Index of receive buffer in the above CAN device.
+ * @param CANdevTx CAN device for SYNC transmission.
+ * @param CANdevTxIdx Index of transmit buffer in the above CAN device.
+ *
+ * @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ */
+CO_ReturnError_t CO_SYNC_init(
+        CO_SYNC_t              *SYNC,
+        CO_EM_t                *em,
+        CO_SDO_t               *SDO,
+        CO_NMT_internalState_t *operatingState,
+        uint32_t                COB_ID_SYNCMessage,
+        uint32_t                communicationCyclePeriod,
+        uint8_t                 synchronousCounterOverflowValue,
+        CO_CANmodule_t         *CANdevRx,
+        uint16_t                CANdevRxIdx,
+        CO_CANmodule_t         *CANdevTx,
+        uint16_t                CANdevTxIdx);
+
+
+#if ((CO_CONFIG_SYNC) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+/**
+ * Initialize SYNC callback function.
+ *
+ * Function initializes optional callback function, which should immediately
+ * start processing of CO_SYNC_process() function.
+ * Callback is called after SYNC message is received from the CAN bus.
+ *
+ * @param SYNC This object.
+ * @param object Pointer to object, which will be passed to pFunctSignalPre(). Can be NULL
+ * @param pFunctSignalPre Pointer to the callback function. Not called if NULL.
+ */
+void CO_SYNC_initCallbackPre(
+        CO_SYNC_t              *SYNC,
+        void                   *object,
+        void                  (*pFunctSignalPre)(void *object));
+#endif
+
+
+/**
+ * Send SYNC message.
+ *
+ * This function prepares and sends a SYNC object. The application should only
+ * call this if direct control of SYNC transmission is needed, otherwise use
+ * CO_SYNC_process().
+ *
+ *
+ * @param SYNC SYNC object.
+ *
+ * @return Same as CO_CANsend().
+ */
+CO_ReturnError_t CO_SYNCsend(CO_SYNC_t *SYNC);
+
+
+/**
+ * Process SYNC communication.
+ *
+ * Function must be called cyclically.
+ *
+ * @param SYNC This object.
+ * @param timeDifference_us Time difference from previous function call in [microseconds].
+ * @param ObjDict_synchronousWindowLength _Synchronous window length_ variable from
+ * Object dictionary (index 0x1007).
+ * @param [out] timerNext_us info to OS - see CO_process_SYNC_PDO().
+ *
+ * @return #CO_SYNC_status_t: CO_SYNC_NONE, CO_SYNC_RECEIVED or CO_SYNC_OUTSIDE_WINDOW.
+ */
+CO_SYNC_status_t CO_SYNC_process(
+        CO_SYNC_t              *SYNC,
+        uint32_t                timeDifference_us,
+        uint32_t                ObjDict_synchronousWindowLength,
+        uint32_t               *timerNext_us);
+
+/** @} */ /* CO_SYNC */
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+#endif /* (CO_CONFIG_SYNC) & CO_CONFIG_SYNC_ENABLE */
+
+#endif /* CO_SYNC_H */

controller_yy_app/middleware/CANopenNode/301/CO_TIME.c

@@ -0,0 +1,212 @@
+/*
+ * CANopen TIME object.
+ *
+ * @file        CO_TIME.c
+ * @ingroup     CO_TIME
+ * @author      Julien PEYREGNE
+ * @copyright   2019 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <string.h>
+
+#include "301/CO_SDOserver.h"
+#include "301/CO_Emergency.h"
+#include "301/CO_NMT_Heartbeat.h"
+#include "301/CO_TIME.h"
+
+#if (CO_CONFIG_TIME) & CO_CONFIG_TIME_ENABLE
+
+#define DIV_ROUND_UP(_n, _d) (((_n) + (_d) - 1) / (_d))
+
+/*
+ * Read received message from CAN module.
+ *
+ * Function will be called (by CAN receive interrupt) every time, when CAN
+ * message with correct identifier will be received.
+ */
+static void CO_TIME_receive(void *object, void *msg){
+    CO_TIME_t *TIME;
+    CO_NMT_internalState_t operState;
+    uint8_t DLC = CO_CANrxMsg_readDLC(msg);
+    uint8_t *data = CO_CANrxMsg_readData(msg);
+
+    TIME = (CO_TIME_t*)object;   /* this is the correct pointer type of the first argument */
+    operState = *TIME->operatingState;
+
+    if ((operState == CO_NMT_OPERATIONAL) || (operState == CO_NMT_PRE_OPERATIONAL)){
+        // Process Time from msg buffer
+        memcpy(&TIME->Time.ullValue, data, DLC);
+        CO_FLAG_SET(TIME->CANrxNew);
+
+#if (CO_CONFIG_TIME) & CO_CONFIG_FLAG_CALLBACK_PRE
+        /* Optional signal to RTOS, which can resume task, which handles TIME. */
+        if (TIME->pFunctSignalPre != NULL) {
+            TIME->pFunctSignalPre(TIME->functSignalObjectPre);
+        }
+#endif
+    }
+    else{
+        TIME->receiveError = (uint16_t)DLC;
+    }
+}
+
+/******************************************************************************/
+CO_ReturnError_t CO_TIME_init(
+        CO_TIME_t              *TIME,
+        CO_EM_t                *em,
+        CO_SDO_t               *SDO,
+        CO_NMT_internalState_t *operatingState,
+        uint32_t                COB_ID_TIMEMessage,
+        uint32_t                TIMECyclePeriod,
+        CO_CANmodule_t         *CANdevRx,
+        uint16_t                CANdevRxIdx,
+        CO_CANmodule_t         *CANdevTx,
+        uint16_t                CANdevTxIdx)
+{
+    CO_ReturnError_t ret = CO_ERROR_NO;
+
+    /* verify arguments */
+    if (TIME==NULL || em==NULL || SDO==NULL || operatingState==NULL ||
+    CANdevRx==NULL || CANdevTx==NULL){
+        return CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    /* Configure object variables */
+    TIME->isConsumer = (COB_ID_TIMEMessage&0x80000000L) ? true : false;
+    TIME->isProducer = (COB_ID_TIMEMessage&0x40000000L) ? true : false;
+    TIME->COB_ID = COB_ID_TIMEMessage&0x7FF; // 11 bit ID
+
+    TIME->periodTime = TIMECyclePeriod;
+    TIME->periodTimeoutTime = TIMECyclePeriod / 2 * 3;
+    /* overflow? */
+    if (TIME->periodTimeoutTime < TIMECyclePeriod)
+        TIME->periodTimeoutTime = 0xFFFFFFFFL;
+
+    CO_FLAG_CLEAR(TIME->CANrxNew);
+    TIME->timer = 0;
+    TIME->receiveError = 0U;
+
+    TIME->em = em;
+    TIME->operatingState = operatingState;
+
+#if (CO_CONFIG_TIME) & CO_CONFIG_FLAG_CALLBACK_PRE
+    TIME->pFunctSignalPre = NULL;
+    TIME->functSignalObjectPre = NULL;
+#endif
+
+
+    /* configure TIME consumer message reception */
+    TIME->CANdevRx = CANdevRx;
+    TIME->CANdevRxIdx = CANdevRxIdx;
+	if (TIME->isConsumer) {
+        ret = CO_CANrxBufferInit(
+                CANdevRx,               /* CAN device */
+                CANdevRxIdx,            /* rx buffer index */
+                TIME->COB_ID,           /* CAN identifier */
+                0x7FF,                  /* mask */
+                0,                      /* rtr */
+                (void*)TIME,            /* object passed to receive function */
+                CO_TIME_receive);       /* this function will process received message */
+    }
+
+    /* configure TIME producer message transmission */
+    TIME->CANdevTx = CANdevTx;
+    TIME->CANdevTxIdx = CANdevTxIdx;
+    if (TIME->isProducer) {
+        TIME->TXbuff = CO_CANtxBufferInit(
+            CANdevTx,               /* CAN device */
+            CANdevTxIdx,            /* index of specific buffer inside CAN module */
+            TIME->COB_ID,           /* CAN identifier */
+            0,                      /* rtr */
+            TIME_MSG_LENGTH,        /* number of data bytes */
+            0);                     /* synchronous message flag bit */
+
+        if (TIME->TXbuff == NULL) {
+            ret = CO_ERROR_ILLEGAL_ARGUMENT;
+        }
+    }
+
+    return ret;
+}
+
+#if (CO_CONFIG_TIME) & CO_CONFIG_FLAG_CALLBACK_PRE
+/******************************************************************************/
+void CO_TIME_initCallbackPre(
+        CO_TIME_t              *TIME,
+        void                   *object,
+        void                  (*pFunctSignalPre)(void *object))
+{
+    if (TIME != NULL){
+        TIME->functSignalObjectPre = object;
+        TIME->pFunctSignalPre = pFunctSignalPre;
+    }
+}
+#endif
+
+/******************************************************************************/
+uint8_t CO_TIME_process(
+        CO_TIME_t              *TIME,
+        uint32_t                timeDifference_us)
+{
+    uint8_t ret = 0;
+    uint32_t timerNew;
+
+    if (*TIME->operatingState == CO_NMT_OPERATIONAL || *TIME->operatingState == CO_NMT_PRE_OPERATIONAL){
+        /* update TIME timer, no overflow */
+        uint32_t timeDifference_ms = DIV_ROUND_UP(timeDifference_us, 1000);
+        timerNew = TIME->timer + timeDifference_ms;
+        if (timerNew > TIME->timer)
+            TIME->timer = timerNew;
+
+        /* was TIME just received */
+        if (CO_FLAG_READ(TIME->CANrxNew)){
+            TIME->timer = 0;
+            ret = 1;
+            CO_FLAG_CLEAR(TIME->CANrxNew);
+        }
+
+        /* TIME producer */
+        if (TIME->isProducer && TIME->periodTime){
+            if (TIME->timer >= TIME->periodTime){
+                TIME->timer = 0;
+                ret = 1;
+                memcpy(TIME->TXbuff->data, &TIME->Time.ullValue, TIME_MSG_LENGTH);
+                CO_CANsend(TIME->CANdevTx, TIME->TXbuff);
+            }
+        }
+
+        /* Verify TIME timeout if node is consumer */
+        if (TIME->isConsumer && TIME->periodTime && TIME->timer > TIME->periodTimeoutTime
+        && *TIME->operatingState == CO_NMT_OPERATIONAL)
+            CO_errorReport(TIME->em, CO_EM_TIME_TIMEOUT, CO_EMC_COMMUNICATION, TIME->timer);
+    }
+    else {
+        CO_FLAG_CLEAR(TIME->CANrxNew);
+    }
+
+    /* verify error from receive function */
+    if (TIME->receiveError != 0U){
+        CO_errorReport(TIME->em, CO_EM_TIME_LENGTH, CO_EMC_TIME_DATA_LENGTH, (uint32_t)TIME->receiveError);
+        TIME->receiveError = 0U;
+    }
+
+    return ret;
+}
+
+#endif /* (CO_CONFIG_TIME) & CO_CONFIG_TIME_ENABLE */

controller_yy_app/middleware/CANopenNode/301/CO_TIME.h

@@ -0,0 +1,207 @@
+/**
+ * CANopen Time-stamp protocol.
+ *
+ * @file        CO_TIME.h
+ * @ingroup     CO_TIME
+ * @author      Julien PEYREGNE
+ * @copyright   2019 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CO_TIME_H
+#define CO_TIME_H
+
+#include "301/CO_driver.h"
+
+/* default configuration, see CO_config.h */
+#ifndef CO_CONFIG_TIME
+#define CO_CONFIG_TIME (0)
+#endif
+
+#if ((CO_CONFIG_TIME) & CO_CONFIG_TIME_ENABLE) || defined CO_DOXYGEN
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_TIME TIME
+ * @ingroup CO_CANopen_301
+ * @{
+ *
+ * CANopen Time-stamp protocol.
+ *
+ * For CAN identifier see #CO_Default_CAN_ID_t
+ *
+ * TIME message is used for time synchronization of the nodes on network. One node
+ * should be TIME producer, others can be TIME consumers. This is configured with
+ * COB_ID_TIME object 0x1012 :
+ *
+ * - bit 31 should be set for a consumer
+ * - bit 30 should be set for a producer
+ *
+ *
+ * ###TIME CONSUMER
+ *
+ * CO_TIME_init() configuration :
+ * - COB_ID_TIME : 0x80000100L -> TIME consumer with TIME_COB_ID = 0x100
+ * - TIMECyclePeriod :
+ *      - 0 -> no EMCY will be transmitted in case of TIME timeout
+ *      - X -> an EMCY will be transmitted in case of TIME timeout (X * 1.5) ms
+ *
+ * Latest time value is stored in \p CO->TIME->Time variable.
+ *
+ *
+ * ###TIME PRODUCER
+ *
+ * CO_TIME_init() configuration :
+ * - COB_ID_TIME : 0x40000100L -> TIME producer with TIME_COB_ID = 0x100
+ * - TIMECyclePeriod : Time transmit period in ms
+ *
+ * Write time value in \p CO->TIME->Time variable, this will be sent at TIMECyclePeriod.
+ */
+
+#define TIME_MSG_LENGTH 6U
+
+#ifndef timeOfDay_t
+typedef union {
+    unsigned long long ullValue;
+    struct {
+        unsigned long ms:28;
+        unsigned reserved:4;
+        unsigned days:16;
+        unsigned reserved2:16;
+    };
+} timeOfDay_t;
+#endif
+
+typedef timeOfDay_t TIME_OF_DAY;
+typedef timeOfDay_t TIME_DIFFERENCE;
+
+/**
+ * TIME producer and consumer object.
+ */
+typedef struct{
+    CO_EM_t            *em;             /**< From CO_TIME_init() */
+    CO_NMT_internalState_t *operatingState; /**< From CO_TIME_init() */
+	/** True, if device is TIME consumer. Calculated from _COB ID TIME Message_
+    variable from Object dictionary (index 0x1012). */
+    bool_t              isConsumer;
+	/** True, if device is TIME producer. Calculated from _COB ID TIME Message_
+    variable from Object dictionary (index 0x1012). */
+    bool_t              isProducer;
+    uint16_t            COB_ID;         /**< From CO_TIME_init() */
+    /** TIME period time in [milliseconds]. Set to TIME period to enable
+    timeout detection */
+    uint32_t            periodTime;
+    /** TIME period timeout time in [milliseconds].
+    (periodTimeoutTime = periodTime * 1,5) */
+    uint32_t            periodTimeoutTime;
+    /** Variable indicates, if new TIME message received from CAN bus */
+    volatile void      *CANrxNew;
+    /** Timer for the TIME message in [microseconds].
+    Set to zero after received or transmitted TIME message */
+    uint32_t            timer;
+    /** Set to nonzero value, if TIME with wrong data length is received from CAN */
+    uint16_t            receiveError;
+#if ((CO_CONFIG_TIME) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+    /** From CO_TIME_initCallbackPre() or NULL */
+    void              (*pFunctSignalPre)(void *object);
+    /** From CO_TIME_initCallbackPre() or NULL */
+    void               *functSignalObjectPre;
+#endif
+    CO_CANmodule_t     *CANdevRx;       /**< From CO_TIME_init() */
+    uint16_t            CANdevRxIdx;    /**< From CO_TIME_init() */
+	CO_CANmodule_t     *CANdevTx;       /**< From CO_TIME_init() */
+    uint16_t            CANdevTxIdx;    /**< From CO_TIME_init() */
+    CO_CANtx_t         *TXbuff;         /**< CAN transmit buffer */
+    TIME_OF_DAY         Time;
+}CO_TIME_t;
+
+/**
+ * Initialize TIME object.
+ *
+ * Function must be called in the communication reset section.
+ *
+ * @param TIME This object will be initialized.
+ * @param em Emergency object.
+ * @param SDO SDO server object.
+ * @param operatingState Pointer to variable indicating CANopen device NMT internal state.
+ * @param COB_ID_TIMEMessage Should be intialized with CO_CAN_ID_TIME_STAMP
+ * @param TIMECyclePeriod TIME period in ms (may also be used in consumer mode for timeout detection (1.5x period)).
+ * @param CANdevRx CAN device for TIME reception.
+ * @param CANdevRxIdx Index of receive buffer in the above CAN device.
+ * @param CANdevTx CAN device for TIME transmission.
+ * @param CANdevTxIdx Index of transmit buffer in the above CAN device.
+ *
+ * @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ */
+CO_ReturnError_t CO_TIME_init(
+        CO_TIME_t              *TIME,
+        CO_EM_t                *em,
+        CO_SDO_t               *SDO,
+        CO_NMT_internalState_t *operatingState,
+        uint32_t                COB_ID_TIMEMessage,
+        uint32_t                TIMECyclePeriod,
+        CO_CANmodule_t         *CANdevRx,
+        uint16_t                CANdevRxIdx,
+        CO_CANmodule_t         *CANdevTx,
+        uint16_t                CANdevTxIdx);
+
+#if ((CO_CONFIG_TIME) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+/**
+ * Initialize TIME callback function.
+ *
+ * Function initializes optional callback function, which should immediately
+ * start processing of CO_TIME_process() function.
+ * Callback is called after TIME message is received from the CAN bus.
+ *
+ * @param TIME This object.
+ * @param object Pointer to object, which will be passed to pFunctSignalPre(). Can be NULL
+ * @param pFunctSignalPre Pointer to the callback function. Not called if NULL.
+ */
+void CO_TIME_initCallbackPre(
+        CO_TIME_t              *TIME,
+        void                   *object,
+        void                  (*pFunctSignalPre)(void *object));
+#endif
+
+/**
+ * Process TIME communication.
+ *
+ * Function must be called cyclically.
+ *
+ * @param TIME This object.
+ * @param timeDifference_us Time difference from previous function call in [microseconds].
+ *
+ * @return 0: No special meaning.
+ * @return 1: New TIME message recently received (consumer) / transmited (producer).
+ */
+uint8_t CO_TIME_process(
+        CO_TIME_t              *TIME,
+        uint32_t                timeDifference_us);
+
+/** @} */ /* CO_TIME */
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+#endif /* (CO_CONFIG_TIME) & CO_CONFIG_TIME_ENABLE */
+
+#endif /* CO_TIME_H */

controller_yy_app/middleware/CANopenNode/301/CO_config.h

@@ -0,0 +1,745 @@
+/**
+ * Configuration macros for CANopenNode.
+ *
+ * @file        CO_config.h
+ * @ingroup     CO_driver
+ * @author      Janez Paternoster
+ * @copyright   2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+
+#ifndef CO_CONFIG_FLAGS_H
+#define CO_CONFIG_FLAGS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_STACK_CONFIG Stack configuration
+ * @ingroup CO_driver
+ *
+ * Stack configuration macros specify, which parts of the stack will be enabled.
+ *
+ * Default values for stack configuration macros are set in corresponding
+ * header files. The same default values are also provided in this file, but
+ * only for documentation generator. Default values can be overridden by
+ * CO_driver_target.h file. If specified so, they can further be overridden by
+ * CO_driver_custom.h file.
+ *
+ * Stack configuration macro is specified as bits, where each bit
+ * enables/disables some part of the configurable CANopenNode object. Flags are
+ * used for enabling or checking specific bit. Multiple flags can be ORed
+ * together.
+ *
+ * Some functionalities of CANopenNode objects, enabled by configuration macros,
+ * requires some objects from Object Dictionary to exist. Object Dictionary
+ * configuration must match @ref CO_STACK_CONFIG.
+ * @{
+ */
+
+
+/**
+ * @defgroup CO_STACK_CONFIG_COMMON Common definitions
+ * @{
+ */
+/**
+ * Enable custom callback after CAN receive
+ *
+ * Flag enables optional callback functions, which are part of some CANopenNode
+ * objects. Callbacks can optionally be registered by application, which
+ * configures threads in operating system. Callbacks are called after something
+ * has been preprocessed by higher priority thread and must be further
+ * processed by lower priority thread. For example when CAN message is received
+ * and preprocessed, callback should wake up mainline processing function.
+ * See also @ref CO_process() function.
+ *
+ * If callback functions are used, they must be initialized separately, after
+ * the object initialization.
+ *
+ * This flag is common to multiple configuration macros.
+ */
+#define CO_CONFIG_FLAG_CALLBACK_PRE 0x1000
+
+/**
+ * Enable calculation of timerNext_us variable.
+ *
+ * Calculation of the timerNext_us variable is useful for smooth operation on
+ * operating system. See also @ref CO_process() function.
+ *
+ * This flag is common to multiple configuration macros.
+ */
+#define CO_CONFIG_FLAG_TIMERNEXT 0x2000
+
+/**
+ * Enable dynamic behaviour of Object Dictionary variables
+ *
+ * Some CANopen objects uses Object Dictionary variables as arguments to
+ * initialization functions, which are processed in communication reset section.
+ * If this flag is set, then writing to OD variable will reconfigure
+ * corresponding CANopen object also during CANopen normal operation.
+ *
+ * This flag is common to multiple configuration macros.
+ */
+#define CO_CONFIG_FLAG_OD_DYNAMIC 0x4000
+/** @} */ /* CO_STACK_CONFIG_COMMON */
+
+
+/**
+ * @defgroup CO_STACK_CONFIG_NMT_HB NMT master/slave and HB producer/consumer
+ * @{
+ */
+/**
+ * Configuration of @ref CO_NMT_Heartbeat.
+ *
+ * Possible flags, can be ORed:
+ * - CO_CONFIG_NMT_CALLBACK_CHANGE - Enable custom callback after NMT
+ *   state changes. Callback is configured by
+ *   CO_NMT_initCallbackChanged().
+ * - CO_CONFIG_NMT_MASTER - Enable simple NMT master
+ * - #CO_CONFIG_FLAG_CALLBACK_PRE - Enable custom callback after preprocessing
+ *   received NMT CAN message.
+ *   Callback is configured by CO_NMT_initCallbackPre().
+ * - #CO_CONFIG_FLAG_TIMERNEXT - Enable calculation of timerNext_us variable
+ *   inside CO_NMT_process().
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_NMT (0)
+#endif
+#define CO_CONFIG_NMT_CALLBACK_CHANGE 0x01
+#define CO_CONFIG_NMT_MASTER 0x02
+
+/**
+ * Configuration of @ref CO_HBconsumer
+ *
+ * Possible flags, can be ORed:
+ * - #CO_CONFIG_FLAG_CALLBACK_PRE - Enable custom callback after preprocessing
+ *   received heartbeat CAN message.
+ *   Callback is configured by CO_HBconsumer_initCallbackPre().
+ * - #CO_CONFIG_FLAG_TIMERNEXT - Enable calculation of timerNext_us variable
+ *   inside CO_HBconsumer_process().
+ * - CO_CONFIG_HB_CONS_ENABLE - Enable heartbeat consumer.
+ * - CO_CONFIG_HB_CONS_CALLBACK_CHANGE - Enable custom common callback after NMT
+ *   state of the monitored node changes. Callback is configured by
+ *   CO_HBconsumer_initCallbackNmtChanged().
+ * - CO_CONFIG_HB_CONS_CALLBACK_MULTI - Enable multiple custom callbacks, which
+ *   can be configured individually for each monitored node. Callbacks are
+ *   configured by CO_HBconsumer_initCallbackNmtChanged(),
+ *   CO_HBconsumer_initCallbackHeartbeatStarted(),
+ *   CO_HBconsumer_initCallbackTimeout() and
+ *   CO_HBconsumer_initCallbackRemoteReset() functions.
+ * - CO_CONFIG_HB_CONS_QUERY_FUNCT - Enable functions for query HB state or
+ *   NMT state of the specific monitored node.
+ * Note that CO_CONFIG_HB_CONS_CALLBACK_CHANGE and
+ * CO_CONFIG_HB_CONS_CALLBACK_MULTI cannot be set simultaneously.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_HB_CONS (CO_CONFIG_HB_CONS_ENABLE)
+#endif
+#define CO_CONFIG_HB_CONS_ENABLE 0x01
+#define CO_CONFIG_HB_CONS_CALLBACK_CHANGE 0x02
+#define CO_CONFIG_HB_CONS_CALLBACK_MULTI 0x04
+#define CO_CONFIG_HB_CONS_QUERY_FUNCT 0x08
+
+/**
+ * Number of heartbeat consumer objects, where each object corresponds to one
+ * sub-index in OD object 0x1016, "Consumer heartbeat time".
+ *
+ * If heartbeat consumer is enabled, then valid values are 1 to 127.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_HB_CONS_SIZE 8
+#endif
+/** @} */ /* CO_STACK_CONFIG_NMT_HB */
+
+
+/**
+ * @defgroup CO_STACK_CONFIG_EMERGENCY Emergency producer/consumer
+ * @{
+ */
+/**
+ * Configuration of @ref CO_Emergency
+ *
+ * Possible flags, can be ORed:
+ * - CO_CONFIG_EM_PRODUCER - Enable emergency producer.
+ * - CO_CONFIG_EM_PROD_CONFIGURABLE - Emergency producer COB-ID is configurable,
+ *   OD object 0x1014. If not configurable, then 0x1014 is read-only, COB_ID
+ *   is set to CO_CAN_ID_EMERGENCY + nodeId and write is not verified.
+ * - CO_CONFIG_EM_PROD_INHIBIT - Enable inhibit timer on emergency producer,
+ *   OD object 0x1015.
+ * - CO_CONFIG_EM_HISTORY - Enable error history, OD object 0x1003,
+ *   "Pre-defined error field"
+ * - CO_CONFIG_EM_CONSUMER - Enable simple emergency consumer with callback.
+ * - CO_CONFIG_EM_STATUS_BITS - Access @ref CO_EM_errorStatusBits_t from OD.
+ * - #CO_CONFIG_FLAG_CALLBACK_PRE - Enable custom callback after preprocessing
+ *   emergency condition by CO_errorReport() or CO_errorReset() call.
+ *   Callback is configured by CO_EM_initCallbackPre().
+ * - #CO_CONFIG_FLAG_TIMERNEXT - Enable calculation of timerNext_us variable
+ *   inside CO_EM_process().
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_EM (CO_CONFIG_EM_PRODUCER | CO_CONFIG_EM_HISTORY)
+#endif
+#define CO_CONFIG_EM_PRODUCER 0x01
+#define CO_CONFIG_EM_PROD_CONFIGURABLE 0x02
+#define CO_CONFIG_EM_PROD_INHIBIT 0x04
+#define CO_CONFIG_EM_HISTORY 0x08
+#define CO_CONFIG_EM_STATUS_BITS 0x10
+#define CO_CONFIG_EM_CONSUMER 0x20
+
+/**
+ * Maximum number of @ref CO_EM_errorStatusBits_t
+ *
+ * Stack uses 6*8 = 48 @ref CO_EM_errorStatusBits_t, others are free to use by
+ * manufacturer. Allowable value range is from 48 to 256 bits in steps of 8.
+ * Default is 80.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_EM_ERR_STATUS_BITS_COUNT (10*8)
+#endif
+
+/**
+ * Size of the internal buffer, where emergencies are stored after error
+ * indication with @ref CO_error() function. Each emergency has to be post-
+ * processed by the @ref CO_EM_process() function. In case of overflow, error is
+ * indicated but emergency message is not sent.
+ *
+ * The same buffer is also used for OD object 0x1003, "Pre-defined error field".
+ *
+ * Each buffer element consumes 8 bytes. Valid values are 1 to 254.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_EM_BUFFER_SIZE 16
+#endif
+
+/**
+ * Condition for calculating CANopen Error register, "generic" error bit.
+ *
+ * Condition must observe suitable @ref CO_EM_errorStatusBits_t and use
+ * corresponding member of errorStatusBits array from CO_EM_t to calculate the
+ * condition. See also @ref CO_errorRegister_t.
+ *
+ * @warning Size of @ref CO_CONFIG_EM_ERR_STATUS_BITS_COUNT must be large
+ * enough. (CO_CONFIG_EM_ERR_STATUS_BITS_COUNT/8) must be larger than index of
+ * array member in em->errorStatusBits[index].
+ *
+ * em->errorStatusBits[5] should be included in the condition, because they are
+ * used by the stack.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_ERR_CONDITION_GENERIC (em->errorStatusBits[5] != 0)
+#endif
+
+/**
+ * Condition for calculating CANopen Error register, "current" error bit.
+ * See @ref CO_CONFIG_ERR_CONDITION_GENERIC for description.
+ * Macro is not defined by default, so no error is verified.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_ERR_CONDITION_CURRENT
+#endif
+
+/**
+ * Condition for calculating CANopen Error register, "voltage" error bit.
+ * See @ref CO_CONFIG_ERR_CONDITION_GENERIC for description.
+ * Macro is not defined by default, so no error is verified.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_ERR_CONDITION_VOLTAGE
+#endif
+
+/**
+ * Condition for calculating CANopen Error register, "temperature" error bit.
+ * See @ref CO_CONFIG_ERR_CONDITION_GENERIC for description.
+ * Macro is not defined by default, so no error is verified.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_ERR_CONDITION_TEMPERATURE
+#endif
+
+/**
+ * Condition for calculating CANopen Error register, "communication" error bit.
+ * See @ref CO_CONFIG_ERR_CONDITION_GENERIC for description.
+ *
+ * em->errorStatusBits[2] and em->errorStatusBits[3] must be included in the
+ * condition, because they are used by the stack.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_ERR_CONDITION_COMMUNICATION (em->errorStatusBits[2] || em->errorStatusBits[3])
+#endif
+
+/**
+ * Condition for calculating CANopen Error register, "device profile" error bit.
+ * See @ref CO_CONFIG_ERR_CONDITION_GENERIC for description.
+ * Macro is not defined by default, so no error is verified.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_ERR_CONDITION_DEV_PROFILE
+#endif
+
+/**
+ * Condition for calculating CANopen Error register, "manufacturer" error bit.
+ * See @ref CO_CONFIG_ERR_CONDITION_GENERIC for description.
+ *
+ * em->errorStatusBits[8] and em->errorStatusBits[8] are pre-defined, but can
+ * be changed.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_ERR_CONDITION_MANUFACTURER (em->errorStatusBits[8] || em->errorStatusBits[9])
+#endif
+/** @} */ /* CO_STACK_CONFIG_EMERGENCY */
+
+
+/**
+ * @defgroup CO_STACK_CONFIG_SDO SDO server/client
+ * @{
+ */
+/**
+ * Configuration of @ref CO_SDOserver
+ *
+ * Possible flags, can be ORed:
+ * - CO_CONFIG_SDO_SRV_SEGMENTED - Enable SDO server segmented transfer.
+ * - CO_CONFIG_SDO_SRV_BLOCK - Enable SDO server block transfer. If set, then
+ *   CO_CONFIG_SDO_SRV_SEGMENTED must also be set.
+ * - #CO_CONFIG_FLAG_CALLBACK_PRE - Enable custom callback after preprocessing
+ *   received SDO CAN message.
+ *   Callback is configured by CO_SDOserver_initCallbackPre().
+ * - #CO_CONFIG_FLAG_TIMERNEXT - Enable calculation of timerNext_us variable
+ *   inside CO_SDOserver_process().
+ * - #CO_CONFIG_FLAG_OD_DYNAMIC - Enable dynamic configuration of additional SDO
+ *   servers (Writing to object 0x1201+ re-configures the additional server).
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_SDO_SRV (CO_CONFIG_SDO_SRV_SEGMENTED)
+#endif
+#define CO_CONFIG_SDO_SRV_SEGMENTED 0x02
+#define CO_CONFIG_SDO_SRV_BLOCK 0x04
+
+/**
+ * Size of the internal data buffer for the SDO server.
+ *
+ * If size is less than size of some variables in Object Dictionary, then data
+ * will be transferred to internal buffer in several segments. Minimum size is
+ * 8 or 899 (127*7) for block transfer.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_SDO_SRV_BUFFER_SIZE 32
+#endif
+
+/**
+ * Configuration of @ref CO_SDOclient
+ *
+ * Possible flags, can be ORed:
+ * - CO_CONFIG_SDO_CLI_ENABLE - Enable SDO client.
+ * - CO_CONFIG_SDO_CLI_SEGMENTED - Enable SDO client segmented transfer.
+ * - CO_CONFIG_SDO_CLI_BLOCK - Enable SDO client block transfer. If set, then
+ *   CO_CONFIG_SDO_CLI_SEGMENTED, CO_CONFIG_FIFO_ALT_READ and
+ *   CO_CONFIG_FIFO_CRC16_CCITT must also be set.
+ * - CO_CONFIG_SDO_CLI_LOCAL - Enable local transfer, if Node-ID of the SDO
+ *   server is the same as node-ID of the SDO client. (SDO client is the same
+ *   device as SDO server.) Transfer data directly without communication on CAN.
+ * - #CO_CONFIG_FLAG_CALLBACK_PRE - Enable custom callback after preprocessing
+ *   received SDO CAN message.
+ *   Callback is configured by CO_SDOclient_initCallbackPre().
+ * - #CO_CONFIG_FLAG_TIMERNEXT - Enable calculation of timerNext_us variable
+ *   inside CO_SDOclientDownloadInitiate(), CO_SDOclientDownload(),
+ *   CO_SDOclientUploadInitiate(), CO_SDOclientUpload().
+ * - #CO_CONFIG_FLAG_OD_DYNAMIC - Enable dynamic configuration of SDO clients
+ *   (Writing to object 0x1280+ re-configures the client).
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_SDO_CLI (0)
+#endif
+#define CO_CONFIG_SDO_CLI_ENABLE 0x01
+#define CO_CONFIG_SDO_CLI_SEGMENTED 0x02
+#define CO_CONFIG_SDO_CLI_BLOCK 0x04
+#define CO_CONFIG_SDO_CLI_LOCAL 0x08
+
+/**
+ * Size of the internal data buffer for the SDO client.
+ *
+ * Circular buffer is used for SDO communication. It can be read or written
+ * between successive SDO calls. So size of the buffer can be lower than size of
+ * the actual size of data transferred. If only segmented transfer is used, then
+ * buffer size can be as low as 7 bytes, if data are read/written each cycle. If
+ * block transfer is used, buffer size should be set to at least 1000 bytes, so
+ * maximum blksize can be used (blksize is calculated from free buffer space).
+ * Default value for block transfer is 1000, otherwise 32.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_SDO_CLI_BUFFER_SIZE 32
+#endif
+/** @} */ /* CO_STACK_CONFIG_SDO */
+
+
+/**
+ * @defgroup CO_STACK_CONFIG_TIME Time producer/consumer
+ * @{
+ */
+/**
+ * Configuration of @ref CO_TIME
+ *
+ * Possible flags, can be ORed:
+ * - CO_CONFIG_TIME_ENABLE - Enable TIME object and TIME consumer.
+ * - CO_CONFIG_TIME_PRODUCER - Enable TIME producer.
+ * - #CO_CONFIG_FLAG_CALLBACK_PRE - Enable custom callback after preprocessing
+ *   received TIME CAN message.
+ *   Callback is configured by CO_TIME_initCallbackPre().
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_TIME (0)
+#endif
+#define CO_CONFIG_TIME_ENABLE 0x01
+#define CO_CONFIG_TIME_PRODUCER 0x02
+/** @} */ /* CO_STACK_CONFIG_TIME */
+
+
+/**
+ * @defgroup CO_STACK_CONFIG_SYNC_PDO SYNC and PDO producer/consumer
+ * @{
+ */
+/**
+ * Configuration of @ref CO_SYNC
+ *
+ * Possible flags, can be ORed:
+ * - CO_CONFIG_SYNC_ENABLE - Enable SYNC object and SYNC consumer.
+ * - CO_CONFIG_SYNC_PRODUCER - Enable SYNC producer.
+ * - #CO_CONFIG_FLAG_CALLBACK_PRE - Enable custom callback after preprocessing
+ *   received SYNC CAN message.
+ *   Callback is configured by CO_SYNC_initCallbackPre().
+ * - #CO_CONFIG_FLAG_TIMERNEXT - Enable calculation of timerNext_us variable
+ *   inside CO_SYNC_process().
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_SYNC (CO_CONFIG_SYNC_ENABLE | CO_CONFIG_SYNC_PRODUCER)
+#endif
+#define CO_CONFIG_SYNC_ENABLE 0x01
+#define CO_CONFIG_SYNC_PRODUCER 0x02
+
+/**
+ * Configuration of @ref CO_PDO
+ *
+ * Possible flags, can be ORed:
+ * - CO_CONFIG_RPDO_ENABLE - Enable receive PDO objects.
+ * - CO_CONFIG_TPDO_ENABLE - Enable transmit PDO objects.
+ * - CO_CONFIG_PDO_SYNC_ENABLE - Enable SYNC in PDO objects.
+ * - CO_CONFIG_RPDO_CALLS_EXTENSION - Enable calling configured extension
+ *   callbacks when received RPDO CAN message modifies OD entries.
+ * - CO_CONFIG_TPDO_CALLS_EXTENSION - Enable calling configured extension
+ *   callbacks before TPDO CAN message is sent.
+ * - #CO_CONFIG_FLAG_CALLBACK_PRE - Enable custom callback after preprocessing
+ *   received RPDO CAN message.
+ *   Callback is configured by CO_RPDO_initCallbackPre().
+ * - #CO_CONFIG_FLAG_TIMERNEXT - Enable calculation of timerNext_us variable
+ *   inside CO_TPDO_process().
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_PDO (CO_CONFIG_RPDO_ENABLE | CO_CONFIG_TPDO_ENABLE | CO_CONFIG_PDO_SYNC_ENABLE)
+#endif
+#define CO_CONFIG_RPDO_ENABLE 0x01
+#define CO_CONFIG_TPDO_ENABLE 0x02
+#define CO_CONFIG_PDO_SYNC_ENABLE 0x04
+#define CO_CONFIG_RPDO_CALLS_EXTENSION 0x08
+#define CO_CONFIG_TPDO_CALLS_EXTENSION 0x10
+/** @} */ /* CO_STACK_CONFIG_SYNC_PDO */
+
+
+/**
+ * @defgroup CO_STACK_CONFIG_LEDS CANopen LED diodes
+ * Specified in standard CiA 303-3
+ * @{
+ */
+/**
+ * Configuration of @ref CO_LEDs
+ *
+ * Possible flags, can be ORed:
+ * - CO_CONFIG_LEDS_ENABLE - Enable calculation of the CANopen LED indicators.
+ * - #CO_CONFIG_FLAG_TIMERNEXT - Enable calculation of timerNext_us variable
+ *   inside CO_NMT_process().
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_LEDS (CO_CONFIG_LEDS_ENABLE)
+#endif
+#define CO_CONFIG_LEDS_ENABLE 0x01
+/** @} */ /* CO_STACK_CONFIG_LEDS */
+
+
+/**
+ * @defgroup CO_STACK_CONFIG_SRDO Safety Related Data Objects (SRDO)
+ * Specified in standard EN 50325-5 (CiA 304)
+ * @{
+ */
+/**
+ * Configuration of @ref CO_GFC
+ *
+ * Possible flags, can be ORed:
+ * - CO_CONFIG_GFC_ENABLE - Enable the GFC object
+ * - CO_CONFIG_GFC_CONSUMER - Enable the GFC consumer
+ * - CO_CONFIG_GFC_PRODUCER - Enable the GFC producer
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_GFC (0)
+#endif
+#define CO_CONFIG_GFC_ENABLE 0x01
+#define CO_CONFIG_GFC_CONSUMER 0x02
+#define CO_CONFIG_GFC_PRODUCER 0x04
+
+/**
+ * Configuration of @ref CO_SRDO
+ *
+ * Possible flags, can be ORed:
+ * - CO_CONFIG_SRDO_ENABLE - Enable the SRDO object.
+ * - CO_CONFIG_SRDO_CHECK_TX - Enable checking data before sending.
+ * - CO_CONFIG_RSRDO_CALLS_EXTENSION - Enable calling configured extension
+ *   callbacks when received RSRDO CAN message modifies OD entries.
+ * - CO_CONFIG_TRSRDO_CALLS_EXTENSION - Enable calling configured extension
+ *   callbacks before TSRDO CAN message is sent.
+ * - #CO_CONFIG_FLAG_CALLBACK_PRE - Enable custom callback after preprocessing
+ *   received RSRDO CAN message.
+ *   Callback is configured by CO_SRDO_initCallbackPre().
+ * - #CO_CONFIG_FLAG_TIMERNEXT - Enable calculation of timerNext_us variable
+ *   inside CO_SRDO_process() (Tx SRDO only).
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_SRDO (0)
+#endif
+#define CO_CONFIG_SRDO_ENABLE 0x01
+#define CO_CONFIG_SRDO_CHECK_TX 0x02
+#define CO_CONFIG_RSRDO_CALLS_EXTENSION 0x04
+#define CO_CONFIG_TSRDO_CALLS_EXTENSION 0x08
+
+/**
+ * SRDO Tx time delay
+ *
+ * minimum time between the first and second SRDO (Tx) message
+ * in us
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_SRDO_MINIMUM_DELAY 0
+#endif
+/** @} */ /* CO_STACK_CONFIG_SRDO */
+
+
+/**
+ * @defgroup CO_STACK_CONFIG_LSS LSS master/slave
+ * Specified in standard CiA 305
+ * @{
+ */
+/**
+ * Configuration of @ref CO_LSS
+ *
+ * Possible flags, can be ORed:
+ * - CO_CONFIG_LSS_SLAVE - Enable LSS slave
+ * - CO_CONFIG_LSS_SLAVE_FASTSCAN_DIRECT_RESPOND - Send LSS fastscan respond
+ *   directly from CO_LSSslave_receive() function.
+ * - CO_CONFIG_LSS_MASTER - Enable LSS master
+ * - #CO_CONFIG_FLAG_CALLBACK_PRE - Enable custom callback after preprocessing
+ *   received CAN message.
+ *   Callback is configured by CO_LSSmaster_initCallbackPre().
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_LSS (CO_CONFIG_LSS_SLAVE)
+#endif
+#define CO_CONFIG_LSS_SLAVE 0x01
+#define CO_CONFIG_LSS_SLAVE_FASTSCAN_DIRECT_RESPOND 0x02
+#define CO_CONFIG_LSS_MASTER 0x10
+/** @} */ /* CO_STACK_CONFIG_LSS */
+
+
+/**
+ * @defgroup CO_STACK_CONFIG_GATEWAY CANopen gateway
+ * Specified in standard CiA 309
+ * @{
+ */
+/**
+ * Configuration of @ref CO_CANopen_309_3
+ *
+ * Gateway object is covered by standard CiA 309 - CANopen access from other
+ * networks. It enables usage of the NMT master, SDO client and LSS master as a
+ * gateway device.
+ *
+ * Possible flags, can be ORed:
+ * - CO_CONFIG_GTW_MULTI_NET - Enable multiple network interfaces in gateway
+ *   device. This functionality is currently not implemented.
+ * - CO_CONFIG_GTW_ASCII - Enable gateway device with ASCII mapping (CiA 309-3)
+ *   If set, then CO_CONFIG_FIFO_ASCII_COMMANDS must also be set.
+ * - CO_CONFIG_GTW_ASCII_SDO - Enable SDO client. If set, then
+ *   CO_CONFIG_FIFO_ASCII_DATATYPES must also be set.
+ * - CO_CONFIG_GTW_ASCII_NMT - Enable NMT master
+ * - CO_CONFIG_GTW_ASCII_LSS - Enable LSS master
+ * - CO_CONFIG_GTW_ASCII_LOG - Enable non-standard message log read
+ * - CO_CONFIG_GTW_ASCII_ERROR_DESC - Print error description as additional
+ *   comments in gateway-ascii device for SDO and gateway errors.
+ * - CO_CONFIG_GTW_ASCII_PRINT_HELP - use non-standard command "help" to print
+ *   help usage.
+ * - CO_CONFIG_GTW_ASCII_PRINT_LEDS - Display "red" and "green" CANopen status
+ *   LED diodes on terminal.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_GTW (0)
+#endif
+#define CO_CONFIG_GTW_MULTI_NET 0x01
+#define CO_CONFIG_GTW_ASCII 0x02
+#define CO_CONFIG_GTW_ASCII_SDO 0x04
+#define CO_CONFIG_GTW_ASCII_NMT 0x08
+#define CO_CONFIG_GTW_ASCII_LSS 0x10
+#define CO_CONFIG_GTW_ASCII_LOG 0x20
+#define CO_CONFIG_GTW_ASCII_ERROR_DESC 0x40
+#define CO_CONFIG_GTW_ASCII_PRINT_HELP 0x80
+#define CO_CONFIG_GTW_ASCII_PRINT_LEDS 0x100
+
+/**
+ * Number of loops of #CO_SDOclientDownload() in case of block download
+ *
+ * If SDO clint has block download in progress and OS has buffer for CAN tx
+ * messages, then #CO_SDOclientDownload() functionion can be called multiple
+ * times within own loop (up to 127). This can speed-up SDO block transfer.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_GTW_BLOCK_DL_LOOP 1
+#endif
+
+/**
+ * Size of command buffer in ASCII gateway object.
+ *
+ * If large amount of data is transferred (block transfer), then this should be
+ * increased to 1000 or more. Buffer may be refilled between the block transfer.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_GTWA_COMM_BUF_SIZE 200
+#endif
+
+/**
+ * Size of message log buffer in ASCII gateway object.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_GTWA_LOG_BUF_SIZE 2000
+#endif
+/** @} */ /* CO_STACK_CONFIG_GATEWAY */
+
+
+/**
+ * @defgroup CO_STACK_CONFIG_CRC16 CRC 16 calculation
+ * Helper object
+ * @{
+ */
+/**
+ * Configuration of @ref CO_crc16_ccitt calculation
+ *
+ * Possible flags, can be ORed:
+ * - CO_CONFIG_CRC16_ENABLE - Enable CRC16 calculation
+ * - CO_CONFIG_CRC16_EXTERNAL - CRC functions are defined externally
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_CRC16 (0)
+#endif
+#define CO_CONFIG_CRC16_ENABLE 0x01
+#define CO_CONFIG_CRC16_EXTERNAL 0x02
+/** @} */ /* CO_STACK_CONFIG_CRC16 */
+
+
+/**
+ * @defgroup CO_STACK_CONFIG_FIFO FIFO buffer
+ * Helper object
+ * @{
+ */
+/**
+ * Configuration of @ref CO_CANopen_301_fifo
+ *
+ * FIFO buffer is basically a simple first-in first-out circular data buffer. It
+ * is used by the SDO client and by the CANopen gateway. It has additional
+ * advanced functions for data passed to FIFO.
+ *
+ *
+ * Possible flags, can be ORed:
+ * - CO_CONFIG_FIFO_ENABLE - Enable FIFO buffer
+ * - CO_CONFIG_FIFO_ALT_READ - This must be enabled, when SDO client has
+ *   CO_CONFIG_SDO_CLI_BLOCK enabled. See @ref CO_fifo_altRead().
+ * - CO_CONFIG_FIFO_CRC16_CCITT - This must be enabled, when SDO client has
+ *   CO_CONFIG_SDO_CLI_BLOCK enabled. It enables CRC calculation on data.
+ * - CO_CONFIG_FIFO_ASCII_COMMANDS - This must be enabled, when CANopen gateway
+ *   has CO_CONFIG_GTW_ASCII enabled. It adds command handling functions.
+ * - CO_CONFIG_FIFO_ASCII_DATATYPES - This must be enabled, when CANopen gateway
+ *   has CO_CONFIG_GTW_ASCII and CO_CONFIG_GTW_ASCII_SDO enabled. It adds
+ *   datatype transform functions between binary and ascii, which are necessary
+ *   for SDO client.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_FIFO (0)
+#endif
+#define CO_CONFIG_FIFO_ENABLE 0x01
+#define CO_CONFIG_FIFO_ALT_READ 0x02
+#define CO_CONFIG_FIFO_CRC16_CCITT 0x04
+#define CO_CONFIG_FIFO_ASCII_COMMANDS 0x08
+#define CO_CONFIG_FIFO_ASCII_DATATYPES 0x10
+/** @} */ /* CO_STACK_CONFIG_FIFO */
+
+
+/**
+ * @defgroup CO_STACK_CONFIG_TRACE Trace recorder
+ * Non standard object
+ * @{
+ */
+/**
+ * Configuration of @ref CO_trace for recording variables over time.
+ *
+ * Possible flags, can be ORed:
+ * - CO_CONFIG_TRACE_ENABLE - Enable Trace recorder
+ * - CO_CONFIG_TRACE_OWN_INTTYPES - If set, then macros PRIu32("u" or "lu")
+ *   and PRId32("d" or "ld") must be set. (File inttypes.h can not be included).
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_TRACE (0)
+#endif
+#define CO_CONFIG_TRACE_ENABLE 0x01
+#define CO_CONFIG_TRACE_OWN_INTTYPES 0x02
+/** @} */ /* CO_STACK_CONFIG_TRACE */
+
+
+/**
+ * @defgroup CO_STACK_CONFIG_DEBUG Debug messages
+ * Messages from different parts of the stack.
+ * @{
+ */
+/**
+ * Configuration of debug messages from different parts of the stack, which can
+ * be logged according to target specific function.
+ *
+ * Possible flags, can be ORed:
+ * - CO_CONFIG_DEBUG_COMMON - Define default CO_DEBUG_COMMON(msg) macro. This
+ *   macro is target specific. This macro is then used as default macro in all
+ *   other defined CO_DEBUG_XXX(msg) macros.
+ * - CO_CONFIG_DEBUG_SDO_CLIENT - Define default CO_DEBUG_SDO_CLIENT(msg) macro.
+ * - CO_CONFIG_DEBUG_SDO_SERVER - Define default CO_DEBUG_SDO_SERVER(msg) macro.
+ */
+#ifdef CO_DOXYGEN
+#define CO_CONFIG_DEBUG (0)
+#endif
+#define CO_CONFIG_DEBUG_COMMON 0x01
+#define CO_CONFIG_DEBUG_SDO_CLIENT 0x02
+#define CO_CONFIG_DEBUG_SDO_SERVER 0x04
+/** @} */ /* CO_STACK_CONFIG_DEBUG */
+
+/** @} */ /* CO_STACK_CONFIG */
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* CO_CONFIG_FLAGS_H */

controller_yy_app/middleware/CANopenNode/301/CO_driver.h

@@ -0,0 +1,725 @@
+/**
+ * Interface between CAN hardware and CANopenNode.
+ *
+ * @file        CO_driver.h
+ * @ingroup     CO_driver
+ * @author      Janez Paternoster
+ * @copyright   2004 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CO_DRIVER_H
+#define CO_DRIVER_H
+
+#include <string.h>
+
+#include "CO_config.h"
+#include "CO_driver_target.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifdef CO_DEBUG_COMMON
+#if (CO_CONFIG_DEBUG) & CO_CONFIG_DEBUG_SDO_CLIENT
+ #define CO_DEBUG_SDO_CLIENT(msg) CO_DEBUG_COMMON(msg)
+#endif
+#if (CO_CONFIG_DEBUG) & CO_CONFIG_DEBUG_SDO_SERVER
+ #define CO_DEBUG_SDO_SERVER(msg) CO_DEBUG_COMMON(msg)
+#endif
+#endif
+
+/**
+ * @defgroup CO_driver Driver
+ * @ingroup CO_CANopen_301
+ * @{
+ *
+ * Interface between CAN hardware and CANopenNode.
+ *
+ * CANopenNode is designed for speed and portability. It runs efficiently on
+ * devices from simple 16-bit microcontrollers to PC computers. It can run in
+ * multiple threads. Reception of CAN messages is pre-processed with very fast
+ * functions. Time critical objects, such as PDO or SYNC are processed in
+ * real-time thread and other objects are processed in normal thread. See
+ * Flowchart in [README.md](index.html) for more information.
+ *
+ * @anchor CO_obj
+ * #### CANopenNode Object
+ * CANopenNode is implemented as a collection of different objects, for example
+ * SDO, SYNC, Emergency, PDO, NMT, Heartbeat, etc. Code is written in C language
+ * and tries to be object oriented. So each CANopenNode Object is implemented in
+ * a pair of .h/.c files. It basically contains a structure with all necessary
+ * variables and some functions which operates on it. CANopenNode Object is
+ * usually connected with one or more CAN receive or transmit Message Objects.
+ * (CAN message Object is a CAN message with specific 11-bit CAN identifier
+ * (usually one fixed or a range).)
+ *
+ * #### Hardware interface of CANopenNode
+ * It consists of minimum three files:
+ * - **CO_driver.h** file declares common functions. This file is part of the
+ * CANopenNode. It is included from each .c file from CANopenNode.
+ * - **CO_driver_target.h** file declares microcontroller specific type
+ * declarations and defines some macros, which are necessary for CANopenNode.
+ * This file is included from CO_driver.h.
+ * - **CO_driver.c** file defines functions declared in CO_driver.h.
+ *
+ * **CO_driver_target.h** and **CO_driver.c** files are specific for each
+ * different microcontroller and are not part of CANopenNode. There are separate
+ * projects for different microcontrollers, which usually include CANopenNode as
+ * a git submodule. CANopenNode only includes those two files in the `example`
+ * directory and they are basically empty. It should be possible to compile the
+ * `CANopenNode/example` on any system, however compiled program is not usable.
+ * CO_driver.h contains documentation for all necessary macros, types and
+ * functions.
+ *
+ * See [CANopenNode/Wiki](https://github.com/CANopenNode/CANopenNode/wiki) for a
+ * known list of available implementations of CANopenNode on different systems
+ * and microcontrollers. Everybody is welcome to extend the list with a link to
+ * his own implementation.
+ *
+ * Implementation of the hardware interface for specific microcontroller is not
+ * always an easy task. For reliable and efficient operation it is necessary to
+ * know some parts of the target microcontroller in detail (for example threads
+ * (or interrupts), CAN module, etc.).
+ */
+
+/** Major version number of CANopenNode */
+#define CO_VERSION_MAJOR 2
+/** Minor version number of CANopenNode */
+#define CO_VERSION_MINOR 0
+
+
+/* Macros and declarations in following part are only used for documentation. */
+#ifdef CO_DOXYGEN
+/**
+ * @defgroup CO_dataTypes Basic definitions
+ * @{
+ *
+ * Target specific basic definitions and data types according to Misra C
+ * specification.
+ *
+ * Must be defined in the **CO_driver_target.h** file.
+ *
+ * Depending on processor or compiler architecture, one of the two macros must
+ * be defined: CO_LITTLE_ENDIAN or CO_BIG_ENDIAN. CANopen itself is little
+ * endian.
+ *
+ * Basic data types may be specified differently on different architectures.
+ * Usually `true` and `false` are defined in `<stdbool.h>`, `NULL` is defined in
+ * `<stddef.h>`, `int8_t` to `uint64_t` are defined in `<stdint.h>`.
+ */
+/** CO_LITTLE_ENDIAN or CO_BIG_ENDIAN must be defined */
+#define CO_LITTLE_ENDIAN
+/** Macro must swap bytes, if CO_BIG_ENDIAN is defined */
+#define CO_SWAP_16(x) x
+/** Macro must swap bytes, if CO_BIG_ENDIAN is defined */
+#define CO_SWAP_32(x) x
+/** Macro must swap bytes, if CO_BIG_ENDIAN is defined */
+#define CO_SWAP_64(x) x
+/** NULL, for general usage */
+#define NULL (0)
+/** Logical true, for general use */
+#define true 1
+/** Logical false, for general use */
+#define false 0
+/** Boolean data type for general use */
+typedef unsigned char bool_t;
+/** INTEGER8 in CANopen (0002h), 8-bit signed integer */
+typedef signed char int8_t;
+/** INTEGER16 in CANopen (0003h), 16-bit signed integer */
+typedef signed int int16_t;
+/** INTEGER32 in CANopen (0004h), 32-bit signed integer */
+typedef signed long int int32_t;
+/** INTEGER64 in CANopen (0015h), 64-bit signed integer */
+typedef signed long long int int64_t;
+/** UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer */
+typedef unsigned char uint8_t;
+/** UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer */
+typedef unsigned int uint16_t;
+/** UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer */
+typedef unsigned long int uint32_t;
+/** UNSIGNED64 in CANopen (001Bh), 64-bit unsigned integer */
+typedef unsigned long long int uint64_t;
+/** REAL32 in CANopen (0008h), single precision floating point value, 32-bit */
+typedef float float32_t;
+/** REAL64 in CANopen (0011h), double precision floating point value, 64-bit */
+typedef double float64_t;
+/** VISIBLE_STRING in CANopen (0009h), string of signed 8-bit values */
+typedef char char_t;
+/** OCTET_STRING in CANopen (000Ah), string of unsigned 8-bit values */
+typedef unsigned char oChar_t;
+/** DOMAIN in CANopen (000Fh), used to transfer a large block of data */
+typedef unsigned char domain_t;
+/** @} */
+
+
+/**
+ * @defgroup CO_CAN_Message_reception Reception of CAN messages
+ * @{
+ *
+ * Target specific definitions and description of CAN message reception
+ *
+ * CAN messages in CANopenNode are usually received by its own thread or higher
+ * priority interrupt. Received CAN messages are first filtered by hardware or
+ * by software. Thread then examines its 11-bit CAN-id and mask and determines,
+ * to which \ref CO_obj "CANopenNode Object" it belongs to. After that it calls
+ * predefined CANrx_callback() function, which quickly pre-processes the message
+ * and fetches the relevant data. CANrx_callback() function is defined by each
+ * \ref CO_obj "CANopenNode Object" separately. Pre-processed fetched data are
+ * later processed in another thread.
+ *
+ * If \ref CO_obj "CANopenNode Object" reception of specific CAN message, it
+ * must first configure its own CO_CANrx_t object with the CO_CANrxBufferInit()
+ * function.
+ */
+
+/**
+ * CAN receive callback function which pre-processes received CAN message
+ *
+ * It is called by fast CAN receive thread. Each \ref CO_obj "CANopenNode
+ * Object" defines its own and registers it with CO_CANrxBufferInit(), by
+ * passing function pointer.
+ *
+ * @param object pointer to specific \ref CO_obj "CANopenNode Object",
+ * registered with CO_CANrxBufferInit()
+ * @param rxMsg pointer to received CAN message
+ */
+void CANrx_callback(void *object, void *rxMsg);
+
+/**
+ * CANrx_callback() can read CAN identifier from received CAN message
+ *
+ * Must be defined in the **CO_driver_target.h** file.
+ *
+ * This is target specific function and is specific for specific
+ * microcontroller. It is best to implement it by using inline function or
+ * macro. `rxMsg` parameter should cast to a pointer to structure. For best
+ * efficiency structure may have the same alignment as CAN registers inside CAN
+ * module.
+ *
+ * @param rxMsg Pointer to received message
+ * @return 11-bit CAN standard identifier.
+ */
+static inline uint16_t CO_CANrxMsg_readIdent(void *rxMsg) {
+    return 0;
+}
+
+/**
+ * CANrx_callback() can read Data Length Code from received CAN message
+ *
+ * See also CO_CANrxMsg_readIdent():
+ *
+ * @param rxMsg Pointer to received message
+ * @return data length in bytes (0 to 8)
+ */
+static inline uint8_t CO_CANrxMsg_readDLC(void *rxMsg) {
+    return 0;
+}
+
+/**
+ * CANrx_callback() can read pointer to data from received CAN message
+ *
+ * See also CO_CANrxMsg_readIdent():
+ *
+ * @param rxMsg Pointer to received message
+ * @return pointer to data buffer
+ */
+static inline uint8_t *CO_CANrxMsg_readData(void *rxMsg) {
+    return NULL;
+}
+
+/**
+ * Configuration object for CAN received message for specific \ref CO_obj
+ * "CANopenNode Object".
+ *
+ * Must be defined in the **CO_driver_target.h** file.
+ *
+ * Data fields of this structure are used exclusively by the driver. Usually it
+ * has the following data fields, but they may differ for different
+ * microcontrollers. Array of multiple CO_CANrx_t objects is included inside
+ * CO_CANmodule_t.
+ */
+typedef struct {
+    uint16_t ident; /**< Standard CAN Identifier (bits 0..10) + RTR (bit 11) */
+    uint16_t mask;  /**< Standard CAN Identifier mask with the same alignment as
+                       ident */
+    void *object;   /**< \ref CO_obj "CANopenNode Object" initialized in from
+                       CO_CANrxBufferInit() */
+    void (*pCANrx_callback)(
+        void *object, void *message); /**< Pointer to CANrx_callback()
+                                         initialized in CO_CANrxBufferInit() */
+} CO_CANrx_t;
+/** @} */
+
+
+/**
+ * @defgroup CO_CAN_Message_transmission Transmission of CAN messages
+ * @{
+ *
+ * Target specific definitions and description of CAN message transmission
+ *
+ * If \ref CO_obj "CANopenNode Object" needs transmitting CAN message, it must
+ * first configure its own CO_CANtx_t object with the CO_CANtxBufferInit()
+ * function. CAN message can then be sent with CO_CANsend() function. If at that
+ * moment CAN transmit buffer inside microcontroller's CAN module is free,
+ * message is copied directly to the CAN module. Otherwise CO_CANsend() function
+ * sets _bufferFull_ flag to true. Message will be then sent by CAN TX interrupt
+ * as soon as CAN module is freed. Until message is not copied to CAN module,
+ * its contents must not change. If there are multiple CO_CANtx_t objects with
+ * _bufferFull_ flag set to true, then CO_CANtx_t with lower index will be sent
+ * first.
+ */
+
+/**
+ * Configuration object for CAN transmit message for specific \ref CO_obj
+ * "CANopenNode Object".
+ *
+ * Must be defined in the **CO_driver_target.h** file.
+ *
+ * Data fields of this structure are used exclusively by the driver. Usually it
+ * has the following data fields, but they may differ for different
+ * microcontrollers. Array of multiple CO_CANtx_t objects is included inside
+ * CO_CANmodule_t.
+ */
+typedef struct {
+    uint32_t ident;             /**< CAN identifier as aligned in CAN module */
+    uint8_t DLC;                /**< Length of CAN message */
+    uint8_t data[8];            /**< 8 data bytes */
+    volatile bool_t bufferFull; /**< True if previous message is still in the
+                                     buffer */
+    volatile bool_t syncFlag;   /**< Synchronous PDO messages has this flag set.
+                  It prevents them to be sent outside the synchronous window */
+} CO_CANtx_t;
+/** @} */
+
+
+/**
+ * Complete CAN module object.
+ *
+ * Must be defined in the **CO_driver_target.h** file.
+ *
+ * Usually it has the following data fields, but they may differ for different
+ * microcontrollers.
+ */
+typedef struct {
+    void *CANptr;                      /**< From CO_CANmodule_init() */
+    CO_CANrx_t *rxArray;               /**< From CO_CANmodule_init() */
+    uint16_t rxSize;                   /**< From CO_CANmodule_init() */
+    CO_CANtx_t *txArray;               /**< From CO_CANmodule_init() */
+    uint16_t txSize;                   /**< From CO_CANmodule_init() */
+    uint16_t CANerrorStatus;           /**< CAN error status bitfield,
+                                            see @ref CO_CAN_ERR_status_t */
+    volatile bool_t CANnormal;         /**< CAN module is in normal mode */
+    volatile bool_t useCANrxFilters;   /**< Value different than zero indicates,
+            that CAN module hardware filters are used for CAN reception. If
+            there is not enough hardware filters, they won't be used. In this
+            case will be *all* received CAN messages processed by software. */
+    volatile bool_t bufferInhibitFlag; /**< If flag is true, then message in
+            transmit buffer is synchronous PDO message, which will be aborted,
+            if CO_clearPendingSyncPDOs() function will be called by application.
+            This may be necessary if Synchronous window time was expired. */
+    volatile bool_t firstCANtxMessage; /**< Equal to 1, when the first
+            transmitted message (bootup message) is in CAN TX buffers */
+    volatile uint16_t CANtxCount;      /**< Number of messages in transmit
+            buffer, which are waiting to be copied to the CAN module */
+    uint32_t errOld;                   /**< Previous state of CAN errors */
+    int32_t errinfo;                   /**< For use with @ref CO_errinfo() */
+} CO_CANmodule_t;
+
+
+/**
+ * @defgroup CO_critical_sections Critical sections
+ * @{
+ * CANopenNode is designed to run in different threads, as described in
+ * [README.md](index.html). Threads are implemented differently in different
+ * systems. In microcontrollers threads are interrupts with different
+ * priorities, for example. It is necessary to protect sections, where different
+ * threads access to the same resource. In simple systems interrupts or
+ * scheduler may be temporary disabled between access to the shared resource.
+ * Otherwise mutexes or semaphores can be used.
+ *
+ * #### Reentrant functions
+ * Functions CO_CANsend() from C_driver.h, CO_errorReport() from CO_Emergency.h
+ * and CO_errorReset() from CO_Emergency.h may be called from different threads.
+ * Critical sections must be protected. Either by disabling scheduler or
+ * interrupts or by mutexes or semaphores.
+ *
+ * #### Object Dictionary variables
+ * In general, there are two threads, which accesses OD variables: mainline and
+ * timer. CANopenNode initialization and SDO server runs in mainline. PDOs runs
+ * in faster timer thread. Processing of PDOs must not be interrupted by
+ * mainline. Mainline thread must protect sections, which accesses the same OD
+ * variables as timer thread. This care must also take the application. Note
+ * that not all variables are allowed to be mapped to PDOs, so they may not need
+ * to be protected. SDO server protects sections with access to OD variables.
+ *
+ * #### Synchronization functions for CAN receive
+ * After CAN message is received, it is pre-processed in CANrx_callback(), which
+ * copies some data into appropriate object and at the end sets **new_message**
+ * flag. This flag is then pooled in another thread, which further processes the
+ * message. The problem is, that compiler optimization may shuffle memory
+ * operations, so it is necessary to ensure, that **new_message** flag is surely
+ * set at the end. It is necessary to use [Memory
+ * barrier](https://en.wikipedia.org/wiki/Memory_barrier).
+ *
+ * If receive function runs inside IRQ, no further synchronization is needed.
+ * Otherwise, some kind of synchronization has to be included. The following
+ * example uses GCC builtin memory barrier `__sync_synchronize()`. More
+ * information can be found
+ * [here](https://stackoverflow.com/questions/982129/what-does-sync-synchronize-do#982179).
+ */
+
+/** Lock critical section in CO_CANsend() */
+#define CO_LOCK_CAN_SEND()
+/** Unlock critical section in CO_CANsend() */
+#define CO_UNLOCK_CAN_SEND()
+/** Lock critical section in CO_errorReport() or CO_errorReset() */
+#define CO_LOCK_EMCY()
+/** Unlock critical section in CO_errorReport() or CO_errorReset() */
+#define CO_UNLOCK_EMCY()
+/** Lock critical section when accessing Object Dictionary */
+#define CO_LOCK_OD()
+/** Unock critical section when accessing Object Dictionary */
+#define CO_UNLOCK_OD()
+
+/** Check if new message has arrived */
+#define CO_FLAG_READ(rxNew) ((rxNew) != NULL)
+/** Set new message flag */
+#define CO_FLAG_SET(rxNew) { __sync_synchronize(); rxNew = (void *)1L; }
+/** Clear new message flag */
+#define CO_FLAG_CLEAR(rxNew) { __sync_synchronize(); rxNew = NULL; }
+
+/** @} */
+#endif /* CO_DOXYGEN */
+
+
+/** Macro for passing additional information about error.
+ *
+ * This macro is called from several CANopen init functions, which returns
+ * @ref CO_ReturnError_t.
+ *
+ * CO_driver_target.h may implement this macro. Usually macro only sets
+ * CANmodule->errinfo to err. Application may then use CANmodule->errinfo to
+ * determine the reason of failure. errinfo must be type of int32_t. By default
+ * macro does not record anything.
+ *
+ * CO_errinfo is called in following @ref CO_ReturnError_t reasons:
+ * - 'CO_ERROR_OD_PARAMETERS' - Index of erroneous OD parameter.
+ */
+#ifndef CO_errinfo
+#define CO_errinfo(CANmodule, err) CANmodule->errinfo = err
+#endif
+
+
+/**
+ * Default CANopen identifiers.
+ *
+ * Default CANopen identifiers for CANopen communication objects. Same as
+ * 11-bit addresses of CAN messages. These are default identifiers and
+ * can be changed in CANopen. Especially PDO identifiers are configured
+ * in PDO linking phase of the CANopen network configuration.
+ */
+typedef enum {
+    CO_CAN_ID_NMT_SERVICE = 0x000, /**< 0x000, Network management */
+    CO_CAN_ID_GFC = 0x001,         /**< 0x001, Global fail-safe command */
+    CO_CAN_ID_SYNC = 0x080,        /**< 0x080, Synchronous message */
+    CO_CAN_ID_EMERGENCY = 0x080,   /**< 0x080, Emergency messages (+nodeID) */
+    CO_CAN_ID_TIME = 0x100,        /**< 0x100, Time message */
+    CO_CAN_ID_SRDO_1 = 0x0FF,      /**< 0x0FF, Default SRDO1 (+2*nodeID) */
+    CO_CAN_ID_TPDO_1 = 0x180,      /**< 0x180, Default TPDO1 (+nodeID) */
+    CO_CAN_ID_RPDO_1 = 0x200,      /**< 0x200, Default RPDO1 (+nodeID) */
+    CO_CAN_ID_TPDO_2 = 0x280,      /**< 0x280, Default TPDO2 (+nodeID) */
+    CO_CAN_ID_RPDO_2 = 0x300,      /**< 0x300, Default RPDO2 (+nodeID) */
+    CO_CAN_ID_TPDO_3 = 0x380,      /**< 0x380, Default TPDO3 (+nodeID) */
+    CO_CAN_ID_RPDO_3 = 0x400,      /**< 0x400, Default RPDO3 (+nodeID) */
+    CO_CAN_ID_TPDO_4 = 0x480,      /**< 0x480, Default TPDO4 (+nodeID) */
+    CO_CAN_ID_RPDO_4 = 0x500,      /**< 0x500, Default RPDO5 (+nodeID) */
+    CO_CAN_ID_SDO_SRV = 0x580, /**< 0x580, SDO response from server (+nodeID) */
+    CO_CAN_ID_SDO_CLI = 0x600, /**< 0x600, SDO request from client (+nodeID) */
+    CO_CAN_ID_HEARTBEAT = 0x700,   /**< 0x700, Heartbeat message */
+    CO_CAN_ID_LSS_SLV = 0x7E4,     /**< 0x7E4, LSS response from slave */
+    CO_CAN_ID_LSS_MST = 0x7E5      /**< 0x7E5, LSS request from master */
+} CO_Default_CAN_ID_t;
+
+
+/**
+ * CAN error status bitmasks.
+ *
+ * CAN warning level is reached, if CAN transmit or receive error counter is
+ * more or equal to 96. CAN passive level is reached, if counters are more or
+ * equal to 128. Transmitter goes in error state 'bus off' if transmit error
+ * counter is more or equal to 256.
+ */
+typedef enum {
+    CO_CAN_ERRTX_WARNING = 0x0001,  /**< 0x0001, CAN transmitter warning */
+    CO_CAN_ERRTX_PASSIVE = 0x0002,  /**< 0x0002, CAN transmitter passive */
+    CO_CAN_ERRTX_BUS_OFF = 0x0004,  /**< 0x0004, CAN transmitter bus off */
+    CO_CAN_ERRTX_OVERFLOW = 0x0008, /**< 0x0008, CAN transmitter overflow */
+
+    CO_CAN_ERRTX_PDO_LATE = 0x0080, /**< 0x0080, TPDO is outside sync window */
+
+    CO_CAN_ERRRX_WARNING = 0x0100,  /**< 0x0100, CAN receiver warning */
+    CO_CAN_ERRRX_PASSIVE = 0x0200,  /**< 0x0200, CAN receiver passive */
+    CO_CAN_ERRRX_OVERFLOW = 0x0800, /**< 0x0800, CAN receiver overflow */
+
+    CO_CAN_ERR_WARN_PASSIVE = 0x0303/**< 0x0303, combination */
+} CO_CAN_ERR_status_t;
+
+
+/**
+ * Return values of some CANopen functions. If function was executed
+ * successfully it returns 0 otherwise it returns <0.
+ */
+typedef enum {
+    CO_ERROR_NO = 0,                /**< Operation completed successfully */
+    CO_ERROR_ILLEGAL_ARGUMENT = -1, /**< Error in function arguments */
+    CO_ERROR_OUT_OF_MEMORY = -2,    /**< Memory allocation failed */
+    CO_ERROR_TIMEOUT = -3,          /**< Function timeout */
+    CO_ERROR_ILLEGAL_BAUDRATE = -4, /**< Illegal baudrate passed to function
+                                         CO_CANmodule_init() */
+    CO_ERROR_RX_OVERFLOW = -5,      /**< Previous message was not processed
+                                         yet */
+    CO_ERROR_RX_PDO_OVERFLOW = -6,  /**< previous PDO was not processed yet */
+    CO_ERROR_RX_MSG_LENGTH = -7,    /**< Wrong receive message length */
+    CO_ERROR_RX_PDO_LENGTH = -8,    /**< Wrong receive PDO length */
+    CO_ERROR_TX_OVERFLOW = -9,      /**< Previous message is still waiting,
+                                         buffer full */
+    CO_ERROR_TX_PDO_WINDOW = -10,   /**< Synchronous TPDO is outside window */
+    CO_ERROR_TX_UNCONFIGURED = -11, /**< Transmit buffer was not configured
+                                         properly */
+    CO_ERROR_OD_PARAMETERS = -12,   /**< Error in Object Dictionary parameters*/
+    CO_ERROR_DATA_CORRUPT = -13,    /**< Stored data are corrupt */
+    CO_ERROR_CRC = -14,             /**< CRC does not match */
+    CO_ERROR_TX_BUSY = -15,         /**< Sending rejected because driver is
+                                         busy. Try again */
+    CO_ERROR_WRONG_NMT_STATE = -16, /**< Command can't be processed in current
+                                         state */
+    CO_ERROR_SYSCALL = -17,         /**< Syscall failed */
+    CO_ERROR_INVALID_STATE = -18,   /**< Driver not ready */
+    CO_ERROR_NODE_ID_UNCONFIGURED_LSS = -19 /**< Node-id is in LSS unconfigured
+                                         state. If objects are handled properly,
+                                         this may not be an error. */
+} CO_ReturnError_t;
+
+#define CLAMP(x, min, max)          (((x) < (min)) ? (min) : ((x) > (max)) ? (max) : (x))
+#define IN_RANGE(val, min, max) ((val) >= (min) && (val) <= (max))
+
+uint16_t CO_CANrxMsg_readIdent(CO_CANrxMsg_t *rxMsg);
+uint8_t CO_CANrxMsg_readDLC(CO_CANrxMsg_t *rxMsg);
+uint8_t *CO_CANrxMsg_readData(CO_CANrxMsg_t *rxMsg);
+
+/**
+ * Request CAN configuration (stopped) mode and *wait* until it is set.
+ *
+ * @param CANptr Pointer to CAN device
+ */
+void CO_CANsetConfigurationMode(void *CANptr);
+
+
+/**
+ * Request CAN normal (operational) mode and *wait* until it is set.
+ *
+ * @param CANmodule CO_CANmodule_t object.
+ */
+void CO_CANsetNormalMode(CO_CANmodule_t *CANmodule);
+
+
+/**
+ * Initialize CAN module object.
+ *
+ * Function must be called in the communication reset section. CAN module must
+ * be in Configuration Mode before.
+ *
+ * @param CANmodule This object will be initialized.
+ * @param CANptr Pointer to CAN device.
+ * @param rxArray Array for handling received CAN messages
+ * @param rxSize Size of the above array. Must be equal to number of receiving
+ * CAN objects.
+ * @param txArray Array for handling transmitting CAN messages
+ * @param txSize Size of the above array. Must be equal to number of
+ * transmitting CAN objects.
+ * @param CANbitRate Valid values are (in kbps): 10, 20, 50, 125, 250, 500, 800,
+ * 1000. If value is illegal, bitrate defaults to 125.
+ *
+ * Return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ */
+CO_ReturnError_t CO_CANmodule_init(CO_CANmodule_t *CANmodule,
+                                   void *CANptr,
+                                   CO_CANrx_t rxArray[],
+                                   uint16_t rxSize,
+                                   CO_CANtx_t txArray[],
+                                   uint16_t txSize,
+                                   uint16_t CANbitRate);
+
+
+/**
+ * Switch off CANmodule. Call at program exit.
+ *
+ * @param CANmodule CAN module object.
+ */
+void CO_CANmodule_disable(CO_CANmodule_t *CANmodule);
+
+
+/**
+ * Configure CAN message receive buffer.
+ *
+ * Function configures specific CAN receive buffer. It sets CAN identifier
+ * and connects buffer with specific object. Function must be called for each
+ * member in _rxArray_ from CO_CANmodule_t.
+ *
+ * @param CANmodule This object.
+ * @param index Index of the specific buffer in _rxArray_.
+ * @param ident 11-bit standard CAN Identifier. If two or more CANrx buffers
+ * have the same _ident_, then buffer with lowest _index_ has precedence and
+ * other CANrx buffers will be ignored.
+ * @param mask 11-bit mask for identifier. Most usually set to 0x7FF.
+ * Received message (rcvMsg) will be accepted if the following
+ * condition is true: (((rcvMsgId ^ ident) & mask) == 0).
+ * @param rtr If true, 'Remote Transmit Request' messages will be accepted.
+ * @param object CANopen object, to which buffer is connected. It will be used
+ * as an argument to CANrx_callback. Its type is (void), CANrx_callback will
+ * change its type back to the correct object type.
+ * @param CANrx_callback Pointer to function, which will be called, if received
+ * CAN message matches the identifier. It must be fast function.
+ *
+ * Return #CO_ReturnError_t: CO_ERROR_NO CO_ERROR_ILLEGAL_ARGUMENT or
+ * CO_ERROR_OUT_OF_MEMORY (not enough masks for configuration).
+ */
+CO_ReturnError_t CO_CANrxBufferInit(CO_CANmodule_t *CANmodule,
+                                    uint16_t index,
+                                    uint16_t ident,
+                                    uint16_t mask,
+                                    bool_t rtr,
+                                    void *object,
+                                    void (*CANrx_callback)(void *object,
+                                                           void *message));
+
+
+/**
+ * Configure CAN message transmit buffer.
+ *
+ * Function configures specific CAN transmit buffer. Function must be called for
+ * each member in _txArray_ from CO_CANmodule_t.
+ *
+ * @param CANmodule This object.
+ * @param index Index of the specific buffer in _txArray_.
+ * @param ident 11-bit standard CAN Identifier.
+ * @param rtr If true, 'Remote Transmit Request' messages will be transmitted.
+ * @param noOfBytes Length of CAN message in bytes (0 to 8 bytes).
+ * @param syncFlag This flag bit is used for synchronous TPDO messages. If it is
+ * set, message will not be sent, if current time is outside synchronous window.
+ *
+ * @return Pointer to CAN transmit message buffer. 8 bytes data array inside
+ * buffer should be written, before CO_CANsend() function is called.
+ * Zero is returned in case of wrong arguments.
+ */
+CO_CANtx_t *CO_CANtxBufferInit(CO_CANmodule_t *CANmodule,
+                               uint16_t index,
+                               uint16_t ident,
+                               bool_t rtr,
+                               uint8_t noOfBytes,
+                               bool_t syncFlag);
+
+
+/**
+ * Send CAN message.
+ *
+ * @param CANmodule This object.
+ * @param buffer Pointer to transmit buffer, returned by CO_CANtxBufferInit().
+ * Data bytes must be written in buffer before function call.
+ *
+ * @return #CO_ReturnError_t: CO_ERROR_NO, CO_ERROR_TX_OVERFLOW or
+ * CO_ERROR_TX_PDO_WINDOW (Synchronous TPDO is outside window).
+ */
+CO_ReturnError_t CO_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer);
+
+
+/**
+ * Clear all synchronous TPDOs from CAN module transmit buffers.
+ *
+ * CANopen allows synchronous PDO communication only inside time between SYNC
+ * message and SYNC Window. If time is outside this window, new synchronous PDOs
+ * must not be sent and all pending sync TPDOs, which may be on CAN TX buffers,
+ * may optionally be cleared.
+ *
+ * This function checks (and aborts transmission if necessary) CAN TX buffers
+ * when it is called. Function should be called by the stack in the moment,
+ * when SYNC time was just passed out of synchronous window.
+ *
+ * @param CANmodule This object.
+ */
+void CO_CANclearPendingSyncPDOs(CO_CANmodule_t *CANmodule);
+
+
+/**
+ * Process can module - verify CAN errors
+ *
+ * Function must be called cyclically. It should calculate CANerrorStatus
+ * bitfield for CAN errors defined in @ref CO_CAN_ERR_status_t.
+ *
+ * @param CANmodule This object.
+ */
+void CO_CANmodule_process(CO_CANmodule_t *CANmodule);
+
+
+/**
+ * Get uint8_t value from memory buffer
+ *
+ * @param buf Memory buffer to get value from.
+ *
+ * @return Value
+ */
+static inline uint8_t CO_getUint8(const void *buf) {
+    uint8_t value; memmove(&value, buf, sizeof(value)); return value;
+}
+/** Get uint16_t value from memory buffer, see @ref CO_getUint8 */
+static inline uint16_t CO_getUint16(const void *buf) {
+    uint16_t value; memmove(&value, buf, sizeof(value)); return value;
+}
+/** Get uint32_t value from memory buffer, see @ref CO_getUint8 */
+static inline uint32_t CO_getUint32(const void *buf) {
+    uint32_t value; memmove(&value, buf, sizeof(value)); return value;
+}
+
+/**
+ * Write uint8_t value into memory buffer
+ *
+ * @param buf Memory buffer.
+ * @param value Value to be written into buf.
+ *
+ * @return number of bytes written.
+ */
+static inline uint8_t CO_setUint8(void *buf, uint8_t value) {
+    memmove(buf, &value, sizeof(value)); return sizeof(value);
+}
+/** Write uint16_t value into memory buffer, see @ref CO_setUint8 */
+static inline uint8_t CO_setUint16(void *buf, uint16_t value) {
+    memmove(buf, &value, sizeof(value)); return sizeof(value);
+}
+/** Write uint32_t value into memory buffer, see @ref CO_setUint8 */
+static inline uint8_t CO_setUint32(void *buf, uint32_t value) {
+    memmove(buf, &value, sizeof(value)); return sizeof(value);
+}
+
+/** @} */ /* CO_driver */
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* CO_DRIVER_H */

controller_yy_app/middleware/CANopenNode/301/CO_fifo.c

@@ -0,0 +1,1381 @@
+/*
+ * FIFO circular buffer
+ *
+ * @file        CO_fifo.c
+ * @ingroup     CO_CANopen_309_fifo
+ * @author      Janez Paternoster
+ * @copyright   2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "301/CO_fifo.h"
+
+#if (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ENABLE
+
+#include <string.h>
+#include <ctype.h>
+#include <stdlib.h>
+#include "crc16-ccitt.h"
+
+#if (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_COMMANDS
+#include <stdio.h>
+#include <inttypes.h>
+
+/* Non-graphical character for command delimiter */
+#define DELIM_COMMAND '\n'
+/* Graphical character for comment delimiter */
+#define DELIM_COMMENT '#'
+#endif /* (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_COMMANDS */
+
+/* verify configuration */
+#if (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_CRC16_CCITT
+ #if !((CO_CONFIG_CRC16) & CO_CONFIG_CRC16_ENABLE)
+  #error CO_CONFIG_CRC16_ENABLE must be enabled.
+ #endif
+#endif
+
+/******************************************************************************/
+void CO_fifo_init(CO_fifo_t *fifo, char *buf, size_t bufSize) {
+
+    if (fifo == NULL || buf == NULL || bufSize < 2) {
+        return;
+    }
+
+    fifo->readPtr = 0;
+    fifo->writePtr = 0;
+    fifo->buf = buf;
+    fifo->bufSize = bufSize;
+
+    return;
+}
+
+
+/* Circular FIFO buffer example for fifo->bufSize = 7 (usable size = 6): ******
+ *                                                                            *
+ *   0      *            *             *            *                         *
+ *   1    rp==wp      readPtr      writePtr         *                         *
+ *   2      *            *             *            *                         *
+ *   3      *            *             *        writePtr                      *
+ *   4      *        writePtr       readPtr      readPtr                      *
+ *   5      *            *             *            *                         *
+ *   6      *            *             *            *                         *
+ *                                                                            *
+ *        empty       3 bytes       4 bytes       buffer                      *
+ *        buffer      in buff       in buff       full                        *
+ ******************************************************************************/
+size_t CO_fifo_write(CO_fifo_t *fifo,
+                     const char *buf,
+                     size_t count,
+                     uint16_t *crc)
+{
+    size_t i;
+    char *bufDest;
+
+    if (fifo == NULL || fifo->buf == NULL || buf == NULL) {
+        return 0;
+    }
+
+    bufDest = &fifo->buf[fifo->writePtr];
+    for (i = count; i > 0; i--) {
+        size_t writePtrNext = fifo->writePtr + 1;
+
+        /* is circular buffer full */
+        if (writePtrNext == fifo->readPtr ||
+            (writePtrNext == fifo->bufSize && fifo->readPtr == 0)) {
+            break;
+        }
+
+        *bufDest = *buf;
+
+#if (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_CRC16_CCITT
+        if (crc != NULL) {
+            crc16_ccitt_single(crc, (unsigned char)*buf);
+        }
+#endif
+
+        /* increment variables */
+        if (writePtrNext == fifo->bufSize) {
+            fifo->writePtr = 0;
+            bufDest = &fifo->buf[0];
+        }
+        else {
+            fifo->writePtr++;
+            bufDest++;
+        }
+        buf++;
+    }
+
+    return count - i;
+}
+
+
+/******************************************************************************/
+size_t CO_fifo_read(CO_fifo_t *fifo, char *buf, size_t count, bool_t *eof) {
+    size_t i;
+    const char *bufSrc;
+
+    if (eof != NULL) {
+        *eof = false;
+    }
+    if (fifo == NULL || buf == NULL || fifo->readPtr == fifo->writePtr) {
+        return 0;
+    }
+
+    bufSrc = &fifo->buf[fifo->readPtr];
+    for (i = count; i > 0; ) {
+        const char c = *bufSrc;
+
+        /* is circular buffer empty */
+        if (fifo->readPtr == fifo->writePtr) {
+            break;
+        }
+
+        *(buf++) = c;
+
+        /* increment variables */
+        if (++fifo->readPtr == fifo->bufSize) {
+            fifo->readPtr = 0;
+            bufSrc = &fifo->buf[0];
+        }
+        else {
+            bufSrc++;
+        }
+        i--;
+
+#if (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_COMMANDS
+        /* is delimiter? */
+        if (eof != NULL && c == DELIM_COMMAND) {
+            *eof = true;
+            break;
+        }
+#endif
+    }
+
+    return count - i;
+}
+
+
+#if (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ALT_READ
+/******************************************************************************/
+size_t CO_fifo_altBegin(CO_fifo_t *fifo, size_t offset) {
+    size_t i;
+
+    if (fifo == NULL) {
+        return 0;
+    }
+
+    fifo->altReadPtr = fifo->readPtr;
+    for (i = offset; i > 0; i--) {
+        /* is circular buffer empty */
+        if (fifo->altReadPtr == fifo->writePtr) {
+            break;
+        }
+
+        /* increment variable */
+        if (++fifo->altReadPtr == fifo->bufSize) {
+            fifo->altReadPtr = 0;
+        }
+    }
+
+    return offset - i;
+}
+
+void CO_fifo_altFinish(CO_fifo_t *fifo, uint16_t *crc) {
+
+    if (fifo == NULL) {
+        return;
+    }
+
+    if (crc == NULL) {
+        fifo->readPtr = fifo->altReadPtr;
+    }
+    else {
+        const char *bufSrc = &fifo->buf[fifo->readPtr];
+        while (fifo->readPtr != fifo->altReadPtr) {
+#if (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_CRC16_CCITT
+            crc16_ccitt_single(crc, (unsigned char)*bufSrc);
+#endif
+            /* increment variable */
+            if (++fifo->readPtr == fifo->bufSize) {
+                fifo->readPtr = 0;
+                bufSrc = &fifo->buf[0];
+            }
+            else {
+                bufSrc++;
+            }
+        }
+    }
+}
+
+size_t CO_fifo_altRead(CO_fifo_t *fifo, char *buf, size_t count) {
+    size_t i;
+    const char *bufSrc;
+
+    bufSrc = &fifo->buf[fifo->altReadPtr];
+    for (i = count; i > 0; i--) {
+        const char c = *bufSrc;
+
+        /* is there no more data */
+        if (fifo->altReadPtr == fifo->writePtr) {
+            break;
+        }
+
+        *(buf++) = c;
+
+        /* increment variables */
+        if (++fifo->altReadPtr == fifo->bufSize) {
+            fifo->altReadPtr = 0;
+            bufSrc = &fifo->buf[0];
+        }
+        else {
+            bufSrc++;
+        }
+    }
+
+    return count - i;
+}
+#endif /* (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ALT_READ */
+
+
+#if (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_COMMANDS
+/******************************************************************************/
+bool_t CO_fifo_CommSearch(CO_fifo_t *fifo, bool_t clear) {
+    bool_t newCommand = false;
+    size_t count;
+    char *commandEnd;
+
+    if (fifo == NULL || fifo->readPtr == fifo->writePtr) {
+        return 0;
+    }
+
+    /* search delimiter until writePtr or until end of buffer */
+    if (fifo->readPtr < fifo->writePtr) {
+        count = fifo->writePtr - fifo->readPtr;
+    } else {
+        count = fifo->bufSize - fifo->readPtr;
+    }
+    commandEnd = (char *)memchr((const void *)&fifo->buf[fifo->readPtr],
+                                DELIM_COMMAND,
+                                count);
+    if (commandEnd != NULL) {
+        newCommand = true;
+    }
+    else if (fifo->readPtr > fifo->writePtr) {
+        /* not found, search in the beginning of the circular buffer */
+        commandEnd = (char *)memchr((const void *)&fifo->buf[0],
+                                    DELIM_COMMAND,
+                                    fifo->writePtr);
+        if (commandEnd != NULL || fifo->readPtr == (fifo->writePtr + 1)) {
+            /* command delimiter found or buffer full */
+            newCommand = true;
+        }
+    }
+    else if (fifo->readPtr == 0 && fifo->writePtr == (fifo->bufSize - 1)) {
+        /* buffer full */
+        newCommand = true;
+    }
+
+    /* Clear buffer if set so */
+    if (clear) {
+        if (commandEnd != NULL) {
+            fifo->readPtr = (int)(commandEnd - fifo->buf) + 1;
+            if (fifo->readPtr == fifo->bufSize) {
+                fifo->readPtr = 0;
+            }
+        }
+        else {
+            fifo->readPtr = fifo->writePtr;
+        }
+    }
+
+    return newCommand;
+}
+
+
+/******************************************************************************/
+bool_t CO_fifo_trimSpaces(CO_fifo_t *fifo, bool_t *insideComment) {
+    bool_t delimCommandFound = false;
+
+    if (fifo != NULL && insideComment != NULL) {
+        while (fifo->readPtr != fifo->writePtr) {
+            char c = fifo->buf[fifo->readPtr];
+
+            if (c == DELIM_COMMENT) {
+                *insideComment = true;
+            }
+            else if (isgraph((int)c) != 0 && !(*insideComment)) {
+                break;
+            }
+            if (++fifo->readPtr == fifo->bufSize) {
+                fifo->readPtr = 0;
+            }
+            if (c == DELIM_COMMAND) {
+                delimCommandFound = true;
+                *insideComment = false;
+                break;
+            }
+        }
+    }
+    return delimCommandFound;
+}
+
+
+/******************************************************************************/
+size_t CO_fifo_readToken(CO_fifo_t *fifo,
+                         char *buf,
+                         size_t count,
+                         char *closed,
+                         bool_t *err)
+{
+    bool_t delimCommandFound = false;
+    bool_t delimCommentFound = false;
+    size_t tokenSize = 0;
+
+    if (fifo != NULL && buf != NULL && count > 1 && (err == NULL || *err == 0)
+        && fifo->readPtr != fifo->writePtr
+    ) {
+        bool_t finished = false;
+        char step = 0;
+        size_t ptr = fifo->readPtr; /* current pointer (integer, 0 based) */
+        char *c = &fifo->buf[ptr];  /* current char */
+        do {
+            switch (step) {
+            case 0: /* skip leading empty characters, stop on delimiter */
+                if (isgraph((int)*c) != 0) {
+                    if (*c == DELIM_COMMENT) {
+                        delimCommentFound = true;
+                    } else {
+                        buf[tokenSize++] = *c;
+                        step++;
+                    }
+                }
+                else if (*c == DELIM_COMMAND) {
+                    delimCommandFound = true;
+                }
+                break;
+            case 1: /* search for end of the token */
+                if (isgraph((int)*c) != 0) {
+                    if (*c == DELIM_COMMENT) {
+                        delimCommentFound = true;
+                    } else if (tokenSize < count) {
+                        buf[tokenSize++] = *c;
+                    }
+                }
+                else {
+                    if (*c == DELIM_COMMAND) {
+                        delimCommandFound = true;
+                    }
+                    step++;
+                }
+                break;
+            case 2: /* skip trailing empty characters */
+                if (isgraph((int)*c) != 0) {
+                    if (*c == DELIM_COMMENT) {
+                        delimCommentFound = true;
+                    } else {
+                        fifo->readPtr = ptr;
+                        finished = true;
+                    }
+                }
+                else if (*c == DELIM_COMMAND) {
+                    delimCommandFound = true;
+                }
+                break;
+            }
+            if (delimCommentFound == true) {
+                /* Comment delimiter found, clear all till end of the line. */
+                fifo->readPtr = ptr;
+                delimCommandFound = CO_fifo_CommSearch(fifo, true);
+                finished = true;
+            }
+            else if (delimCommandFound) {
+                /* command delimiter found, set readPtr behind it. */
+                if (++ptr == fifo->bufSize) ptr = 0;
+                fifo->readPtr = ptr;
+                finished = true;
+            }
+            else if (!finished) {
+                /* find next character in the circular buffer */
+                if (++ptr == fifo->bufSize) {
+                    ptr = 0;
+                    c = &fifo->buf[ptr];
+                }
+                else {
+                    c++;
+                }
+                /* end, if buffer is now empty */
+                if (ptr == fifo->writePtr) {
+                    if (step == 2) {
+                        fifo->readPtr = ptr;
+                    }
+                    else {
+                        tokenSize = 0;
+                    }
+                    finished = true;
+                }
+            }
+        } while (!finished);
+    }
+
+    /* set 'err' return value */
+    if (err != NULL && *err == false) {
+        if (tokenSize == count || (closed != NULL &&
+            ((*closed == 1 && (!delimCommandFound || tokenSize == 0)) ||
+             (*closed == 0 && (delimCommandFound || tokenSize == 0)))
+        )) {
+            *err = true;
+        }
+    }
+    /* set 'closed' return value */
+    if (closed != NULL) {
+        *closed = delimCommandFound ? 1 : 0;
+    }
+
+    /* token was larger then size of the buffer, all was cleaned, return '' */
+    if (tokenSize == count) {
+        tokenSize = 0;
+    }
+    /* write string terminator character */
+    if (buf != NULL && count > tokenSize) {
+        buf[tokenSize] = '\0';
+    }
+
+    return tokenSize;
+}
+#endif /* (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_COMMANDS */
+
+
+#if (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_DATATYPES
+/******************************************************************************/
+/* Tables for mime-base64 encoding, as specified in RFC 2045, (without CR-LF,
+ * but one long string). Base64 is used for encoding binary data into easy
+ * transferable printable characters. In general, each three bytes of binary
+ * data are translated into four characters, where characters are selected from
+ * 64 characters long table. See https://en.wikipedia.org/wiki/Base64 */
+static const char base64EncTable[] =
+    "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
+
+static const char base64DecTable[] = {
+    -1, -1, -1, -1, -1, -1, -1, -1, -1,103,101, -1, -1,102, -1, -1,
+    -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
+   103, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 62, -1, -1, -1, 63,
+    52, 53, 54, 55, 56, 57, 58, 59, 60, 61, -1, -1, -1,100, -1, -1,
+    -1,  0,  1,  2,  3,  4,  5,  6,  7,  8,  9, 10, 11, 12, 13, 14,
+    15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, -1, -1, -1, -1, -1,
+    -1, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40,
+    41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, -1, -1, -1, -1, -1};
+
+size_t CO_fifo_readU82a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
+    uint8_t n=0;
+
+    if (fifo != NULL && count >= 6 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
+        CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
+        return sprintf(buf, "%"PRIu8, n);
+    }
+    else {
+        return CO_fifo_readHex2a(fifo, buf, count, end);
+    }
+}
+
+size_t CO_fifo_readU162a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
+    uint16_t n=0;
+
+    if (fifo != NULL && count >= 8 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
+        CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
+        return sprintf(buf, "%"PRIu16, CO_SWAP_16(n));
+    }
+    else {
+        return CO_fifo_readHex2a(fifo, buf, count, end);
+    }
+}
+
+size_t CO_fifo_readU322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
+    uint32_t n=0;
+
+    if (fifo != NULL && count >= 12 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
+        CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
+        return sprintf(buf, "%"PRIu32, CO_SWAP_32(n));
+    }
+    else {
+        return CO_fifo_readHex2a(fifo, buf, count, end);
+    }
+}
+
+size_t CO_fifo_readU642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
+    uint64_t n=0;
+
+    if (fifo != NULL && count >= 20 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
+        CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
+        return sprintf(buf, "%"PRIu64, CO_SWAP_64(n));
+    }
+    else {
+        return CO_fifo_readHex2a(fifo, buf, count, end);
+    }
+}
+
+size_t CO_fifo_readX82a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
+    uint8_t n=0;
+
+    if (fifo != NULL && count >= 6 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
+        CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
+        return sprintf(buf, "0x%02"PRIX8, n);
+    }
+    else {
+        return CO_fifo_readHex2a(fifo, buf, count, end);
+    }
+}
+
+size_t CO_fifo_readX162a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
+    uint16_t n=0;
+
+    if (fifo != NULL && count >= 8 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
+        CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
+        return sprintf(buf, "0x%04"PRIX16, CO_SWAP_16(n));
+    }
+    else {
+        return CO_fifo_readHex2a(fifo, buf, count, end);
+    }
+}
+
+size_t CO_fifo_readX322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
+    uint32_t n=0;
+
+    if (fifo != NULL && count >= 12 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
+        CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
+        return sprintf(buf, "0x%08"PRIX32, CO_SWAP_32(n));
+    }
+    else {
+        return CO_fifo_readHex2a(fifo, buf, count, end);
+    }
+}
+
+size_t CO_fifo_readX642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
+    uint64_t n=0;
+
+    if (fifo != NULL && count >= 20 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
+        CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
+        return sprintf(buf, "0x%016"PRIX64, CO_SWAP_64(n));
+    }
+    else {
+        return CO_fifo_readHex2a(fifo, buf, count, end);
+    }
+}
+
+size_t CO_fifo_readI82a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
+    int8_t n=0;
+
+    if (fifo != NULL && count >= 6 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
+        CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
+        return sprintf(buf, "%"PRId8, n);
+    }
+    else {
+        return CO_fifo_readHex2a(fifo, buf, count, end);
+    }
+}
+
+size_t CO_fifo_readI162a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
+    int16_t n=0;
+
+    if (fifo != NULL && count >= 8 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
+        CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
+        return sprintf(buf, "%"PRId16, CO_SWAP_16(n));
+    }
+    else {
+        return CO_fifo_readHex2a(fifo, buf, count, end);
+    }
+}
+
+size_t CO_fifo_readI322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
+    int32_t n=0;
+
+    if (fifo != NULL && count >= 13 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
+        CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
+        return sprintf(buf, "%"PRId32, CO_SWAP_32(n));
+    }
+    else {
+        return CO_fifo_readHex2a(fifo, buf, count, end);
+    }
+}
+
+size_t CO_fifo_readI642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
+    int64_t n=0;
+
+    if (fifo != NULL && count >= 23 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
+        CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
+        return sprintf(buf, "%"PRId64, CO_SWAP_64(n));
+    }
+    else {
+        return CO_fifo_readHex2a(fifo, buf, count, end);
+    }
+}
+
+size_t CO_fifo_readR322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
+    float32_t n=0;
+
+    if (fifo != NULL && count >= 20 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
+        CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
+        return sprintf(buf, "%g", CO_SWAP_32(n));
+    }
+    else {
+        return CO_fifo_readHex2a(fifo, buf, count, end);
+    }
+}
+
+size_t CO_fifo_readR642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
+    float64_t n=0;
+
+    if (fifo != NULL && count >= 30 && CO_fifo_getOccupied(fifo) == sizeof(n)) {
+        CO_fifo_read(fifo, (char *)&n, sizeof(n), NULL);
+        return sprintf(buf, "%g", CO_SWAP_64(n));
+    }
+    else {
+        return CO_fifo_readHex2a(fifo, buf, count, end);
+    }
+}
+
+size_t CO_fifo_readHex2a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
+    (void)end;    /* unused */
+
+    size_t len = 0;
+
+    if (fifo != NULL && count > 3) {
+        /* Very first write is without leading space */
+        if (!fifo->started) {
+            char c;
+            if (CO_fifo_getc(fifo, &c)) {
+                len = sprintf(&buf[0], "%02"PRIX8, (uint8_t)c);
+                fifo->started = true;
+            }
+        }
+
+        while ((len + 3) < count) {
+            char c;
+            if (!CO_fifo_getc(fifo, &c)) {
+                break;
+            }
+            len += sprintf(&buf[len], " %02"PRIX8, (uint8_t)c);
+        }
+    }
+
+    return len;
+}
+
+size_t CO_fifo_readVs2a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
+    size_t len = 0;
+
+    if (fifo != NULL && count > 3) {
+        /* Start with '"' */
+        if (!fifo->started) {
+            buf[len++] = '"';
+            fifo->started = true;
+        }
+
+        while ((len + 2) < count) {
+            char c;
+            if (!CO_fifo_getc(fifo, &c)) {
+                if (end) {
+                    buf[len++] = '"';
+                }
+                break;
+            }
+            else if (c != 0 && c != '\r') { /* skip null and CR inside string */
+                buf[len++] = c;
+                if (c == '"') {
+                    buf[len++] = c;
+                }
+            }
+        }
+    }
+
+    return len;
+}
+
+size_t CO_fifo_readB642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end) {
+    /* mime-base64 encoding, see description above base64EncTable */
+
+    size_t len = 0;
+
+    if (fifo != NULL && count >= 4) {
+        uint8_t step;
+        uint16_t word;
+
+        if (!fifo->started) {
+            fifo->started = true;
+            step = 0;
+            word = 0;
+        }
+        else {
+            /* get memorized variables from previous function calls */
+            step = (uint8_t)(fifo->aux >> 16);
+            word = (uint16_t)fifo->aux;
+        }
+
+        while ((len + 3) <= count) {
+            char c;
+
+            if (!CO_fifo_getc(fifo, &c)) {
+                /* buffer is empty, is also SDO communication finished? */
+                if (end) {
+                    /* add padding if necessary */
+                    switch (step) {
+                        case 1:
+                            buf[len++] = base64EncTable[(word >> 4) & 0x3F];
+                            buf[len++] = '=';
+                            buf[len++] = '=';
+                            break;
+                        case 2:
+                            buf[len++] = base64EncTable[(word >> 6) & 0x3F];
+                            buf[len++] = '=';
+                            break;
+                    }
+                }
+                break;
+            }
+
+            word |= (uint8_t)c;
+
+            switch (step++) {
+                case 0:
+                    buf[len++] = base64EncTable[(word >> 2) & 0x3F];
+                    break;
+                case 1:
+                    buf[len++] = base64EncTable[(word >> 4) & 0x3F];
+                    break;
+                default:
+                    buf[len++] = base64EncTable[(word >> 6) & 0x3F];
+                    buf[len++] = base64EncTable[word & 0x3F];
+                    step = 0;
+                    break;
+            }
+            word <<= 8;
+        }
+
+        /* memorize variables for next iteration */
+        fifo->aux = (uint32_t)step << 16 | word;
+    }
+
+    return len;
+}
+
+
+/******************************************************************************/
+size_t CO_fifo_cpyTok2U8(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
+    char buf[15];
+    char closed = -1;
+    bool_t err = 0;
+    size_t nWr = 0;
+    size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
+    CO_fifo_st st = (uint8_t)closed;
+    if (nRd == 0 || err) st |= CO_fifo_st_errTok;
+    else {
+        char *sRet;
+        uint32_t u32 = strtoul(buf, &sRet, 0);
+        if (sRet != strchr(buf, '\0') || u32 > UINT8_MAX) st |= CO_fifo_st_errVal;
+        else {
+            uint8_t num = (uint8_t) u32;
+            nWr = CO_fifo_write(dest, (const char *)&num, sizeof(num), NULL);
+            if (nWr != sizeof(num)) st |= CO_fifo_st_errBuf;
+        }
+    }
+    if (status != NULL) *status = st;
+    return nWr;
+}
+
+size_t CO_fifo_cpyTok2U16(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
+    char buf[15];
+    char closed = -1;
+    bool_t err = 0;
+    size_t nWr = 0;
+    size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
+    CO_fifo_st st = (uint8_t)closed;
+    if (nRd == 0 || err) st |= CO_fifo_st_errTok;
+    else {
+        char *sRet;
+        uint32_t u32 = strtoul(buf, &sRet, 0);
+        if (sRet != strchr(buf, '\0') || u32 > UINT16_MAX) st |= CO_fifo_st_errVal;
+        else {
+            uint16_t num = CO_SWAP_16((uint16_t) u32);
+            nWr = CO_fifo_write(dest, (const char *)&num, sizeof(num), NULL);
+            if (nWr != sizeof(num)) st |= CO_fifo_st_errBuf;
+        }
+    }
+    if (status != NULL) *status = st;
+    return nWr;
+}
+
+size_t CO_fifo_cpyTok2U32(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
+    char buf[15];
+    char closed = -1;
+    bool_t err = 0;
+    size_t nWr = 0;
+    size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
+    CO_fifo_st st = (uint8_t)closed;
+    if (nRd == 0 || err) st |= CO_fifo_st_errTok;
+    else {
+        char *sRet;
+        uint32_t u32 = strtoul(buf, &sRet, 0);
+        if (sRet != strchr(buf, '\0')) st |= CO_fifo_st_errVal;
+        else {
+            uint32_t num = CO_SWAP_32(u32);
+            nWr = CO_fifo_write(dest, (const char *)&num, sizeof(num), NULL);
+            if (nWr != sizeof(num)) st |= CO_fifo_st_errBuf;
+        }
+    }
+    if (status != NULL) *status = st;
+    return nWr;
+}
+
+size_t CO_fifo_cpyTok2U64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
+    char buf[25];
+    char closed = -1;
+    bool_t err = 0;
+    size_t nWr = 0;
+    size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
+    CO_fifo_st st = (uint8_t)closed;
+    if (nRd == 0 || err) st |= CO_fifo_st_errTok;
+    else {
+        char *sRet;
+        uint64_t u64 = strtoull(buf, &sRet, 0);
+        if (sRet != strchr(buf, '\0')) st |= CO_fifo_st_errVal;
+        else {
+            uint64_t num = CO_SWAP_64(u64);
+            nWr = CO_fifo_write(dest, (const char *)&num, sizeof(num), NULL);
+            if (nWr != sizeof(num)) st |= CO_fifo_st_errBuf;
+        }
+    }
+    if (status != NULL) *status = (uint8_t) st;
+    return nWr;
+}
+
+size_t CO_fifo_cpyTok2I8(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
+    char buf[15];
+    char closed = -1;
+    bool_t err = 0;
+    size_t nWr = 0;
+    size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
+    CO_fifo_st st = (uint8_t)closed;
+    if (nRd == 0 || err) st |= CO_fifo_st_errTok;
+    else {
+        char *sRet;
+        int32_t i32 = strtol(buf, &sRet, 0);
+        if (sRet != strchr(buf, '\0') || i32 < INT8_MIN || i32 > INT8_MAX) {
+            st |= CO_fifo_st_errVal;
+        } else {
+            int8_t num = (int8_t) i32;
+            nWr = CO_fifo_write(dest, (const char *)&num, sizeof(num), NULL);
+            if (nWr != sizeof(num)) st |= CO_fifo_st_errBuf;
+        }
+    }
+    if (status != NULL) *status = st;
+    return nWr;
+}
+
+size_t CO_fifo_cpyTok2I16(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
+    char buf[15];
+    char closed = -1;
+    bool_t err = 0;
+    size_t nWr = 0;
+    size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
+    CO_fifo_st st = (uint8_t)closed;
+    if (nRd == 0 || err) st |= CO_fifo_st_errTok;
+    else {
+        char *sRet;
+        int32_t i32 = strtol(buf, &sRet, 0);
+        if (sRet != strchr(buf, '\0') || i32 < INT16_MIN || i32 > INT16_MAX) {
+            st |= CO_fifo_st_errVal;
+        } else {
+            int16_t num = CO_SWAP_16((int16_t) i32);
+            nWr = CO_fifo_write(dest, (const char *)&num, sizeof(num), NULL);
+            if (nWr != sizeof(num)) st |= CO_fifo_st_errBuf;
+        }
+    }
+    if (status != NULL) *status = st;
+    return nWr;
+}
+
+size_t CO_fifo_cpyTok2I32(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
+    char buf[15];
+    char closed = -1;
+    bool_t err = 0;
+    size_t nWr = 0;
+    size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
+    CO_fifo_st st = (uint8_t)closed;
+    if (nRd == 0 || err) st |= CO_fifo_st_errTok;
+    else {
+        char *sRet;
+        int32_t i32 = strtol(buf, &sRet, 0);
+        if (sRet != strchr(buf, '\0')) st |= CO_fifo_st_errVal;
+        else {
+            int32_t num = CO_SWAP_32(i32);
+            nWr = CO_fifo_write(dest, (const char *)&num, sizeof(num), NULL);
+            if (nWr != sizeof(num)) st |= CO_fifo_st_errBuf;
+        }
+    }
+    if (status != NULL) *status = st;
+    return nWr;
+}
+
+size_t CO_fifo_cpyTok2I64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
+    char buf[25];
+    char closed = -1;
+    bool_t err = 0;
+    size_t nWr = 0;
+    size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
+    CO_fifo_st st = (uint8_t)closed;
+    if (nRd == 0 || err) st |= CO_fifo_st_errTok;
+    else {
+        char *sRet;
+        int64_t i64 = strtoll(buf, &sRet, 0);
+        if (sRet != strchr(buf, '\0')) st |= CO_fifo_st_errVal;
+        else {
+            int64_t num = CO_SWAP_64(i64);
+            nWr = CO_fifo_write(dest, (const char *)&num, sizeof(num), NULL);
+            if (nWr != sizeof(num)) st |= CO_fifo_st_errBuf;
+        }
+    }
+    if (status != NULL) *status = (uint8_t) st;
+    return nWr;
+}
+
+size_t CO_fifo_cpyTok2R32(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
+    char buf[30];
+    char closed = -1;
+    bool_t err = 0;
+    size_t nWr = 0;
+    size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
+    CO_fifo_st st = (uint8_t)closed;
+    if (nRd == 0 || err) st |= CO_fifo_st_errTok;
+    else {
+        char *sRet;
+        float32_t f32 = strtof(buf, &sRet);
+        if (sRet != strchr(buf, '\0')) st |= CO_fifo_st_errVal;
+        else {
+            float32_t num = CO_SWAP_32(f32);
+            nWr = CO_fifo_write(dest, (const char *)&num, sizeof(num), NULL);
+            if (nWr != sizeof(num)) st |= CO_fifo_st_errBuf;
+        }
+    }
+    if (status != NULL) *status = st;
+    return nWr;
+}
+
+size_t CO_fifo_cpyTok2R64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
+    char buf[40];
+    char closed = -1;
+    bool_t err = 0;
+    size_t nWr = 0;
+    size_t nRd = CO_fifo_readToken(src, buf, sizeof(buf), &closed, &err);
+    CO_fifo_st st = (uint8_t)closed;
+    if (nRd == 0 || err) st |= CO_fifo_st_errTok;
+    else {
+        char *sRet;
+        float64_t f64 = strtof(buf, &sRet);
+        if (sRet != strchr(buf, '\0')) st |= CO_fifo_st_errVal;
+        else {
+            float64_t num = CO_SWAP_64(f64);
+            nWr = CO_fifo_write(dest, (const char *)&num, sizeof(num), NULL);
+            if (nWr != sizeof(num)) st |= CO_fifo_st_errBuf;
+        }
+    }
+    if (status != NULL) *status = st;
+    return nWr;
+}
+
+size_t CO_fifo_cpyTok2Hex(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
+    size_t destSpace, destSpaceStart;
+    bool_t finished = false;
+    uint8_t step;
+    char firstChar;
+    CO_fifo_st st = 0;
+
+    if (dest == NULL || src == NULL) {
+        return 0;
+    }
+
+    /* get free space of the dest fifo */
+    destSpaceStart = destSpace = CO_fifo_getSpace(dest);
+
+    /* is this the first write into dest? */
+    if (!dest->started) {
+        bool_t insideComment = false;
+        if (CO_fifo_trimSpaces(src, &insideComment) || insideComment) {
+            /* command delimiter found without string, this is an error */
+            st |= CO_fifo_st_errTok;
+        }
+        dest->started = true;
+        step = 0;
+        firstChar = 0;
+    }
+    else {
+        /* get memorized variables from previous function calls */
+        step = (uint8_t)(dest->aux >> 8);
+        firstChar = (char)(dest->aux & 0xFF);
+    }
+
+    /* repeat until destination space available and no error and not finished
+     * and source characters available */
+    while (destSpace > 0 && (st & CO_fifo_st_errMask) == 0 && !finished) {
+        char c;
+        if (!CO_fifo_getc(src, &c)) {
+            break;
+        }
+
+        if (step == 6) {
+            /* command is inside comment, waiting for command delimiter */
+            bool_t insideComment = true;
+            if (c == DELIM_COMMAND || CO_fifo_trimSpaces(src, &insideComment)) {
+                st |= CO_fifo_st_closed;
+                finished = true;
+            }
+            continue;
+        }
+
+        if (isxdigit((int)c) != 0) {
+            /* first or second hex digit */
+            if (step == 0) {
+                firstChar = c;
+                step = 1;
+            }
+            else {
+                /* write the byte */
+                char s[3];
+                int32_t num;
+                s[0] = firstChar; s[1] = c; s[2] = 0;
+                num = strtol(s, NULL, 16);
+                CO_fifo_putc(dest, (char) num);
+                destSpace--;
+                step = 0;
+            }
+        }
+        else if (isgraph((int)c) != 0) {
+            /* printable character, not hex digit */
+            if (c == '#') /* comment start */
+                step = 6;
+            else /* syntax error */
+                st |= CO_fifo_st_errTok;
+        }
+        else {
+            /* this is space or delimiter */
+            if (step == 1) {
+                /* write the byte */
+                char s[2];
+                int32_t num;
+                s[0] = firstChar; s[1] = 0;
+                num = strtol(s, NULL, 16);
+                CO_fifo_putc(dest, (char) num);
+                destSpace--;
+                step = 0;
+            }
+            bool_t insideComment = false;
+            if (c == DELIM_COMMAND || CO_fifo_trimSpaces(src, &insideComment)) {
+                /* newline found, finish */
+                st |= CO_fifo_st_closed;
+                finished = true;
+            }
+            else if (insideComment) {
+                step = 6;
+            }
+        }
+    } /* while ... */
+
+    if (!finished) {
+        st |= CO_fifo_st_partial;
+        /* memorize variables for next iteration */
+        dest->aux = (uint32_t)step << 8 | (uint8_t)firstChar;
+    }
+
+    if (status != NULL) *status = st;
+
+    return destSpaceStart - destSpace;
+}
+
+size_t CO_fifo_cpyTok2Vs(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
+    size_t destSpace, destSpaceStart;
+    bool_t finished = false;
+    uint8_t step;
+    CO_fifo_st st = 0;
+
+    if (dest == NULL || src == NULL) {
+        return 0;
+    }
+
+    /* get free space of the dest fifo */
+    destSpaceStart = destSpace = CO_fifo_getSpace(dest);
+
+    /* is this the first write into dest? */
+    if (!dest->started) {
+        bool_t insideComment = false;
+        if (CO_fifo_trimSpaces(src, &insideComment) || insideComment) {
+            /* command delimiter found without string, this is an error */
+            st |= CO_fifo_st_errTok;
+        }
+        dest->started = true;
+        step = 0;
+    }
+    else {
+        /* get memorized variables from previous function calls */
+        step = (uint8_t)dest->aux;
+    }
+
+    /* repeat until destination space available and no error and not finished
+     * and source characters available */
+    while (destSpace > 0 && (st & CO_fifo_st_errMask) == 0 && !finished) {
+        char c;
+        if (!CO_fifo_getc(src, &c)) {
+            break;
+        }
+
+        switch (step) {
+        case 0: /* beginning of the string, first write into dest */
+            if (c == '"') {
+                /* Indicated beginning of the string, skip this character. */
+                step = 1;
+            }
+            else {
+                /* this must be a single word string without '"' */
+                /* copy the character */
+                CO_fifo_putc(dest, c);
+                destSpace--;
+                step = 2;
+            }
+            break;
+
+        case 1: /* inside string, quoted string */
+        case 2: /* inside string, single word, no quotes */
+            if (c == '"') {
+                /* double quote found, this may be end of the string or escaped
+                 * double quote (with two double quotes) */
+                step += 2;
+            }
+            else if (isgraph((int)c) == 0 && step == 2) {
+                /* end of single word string */
+                bool_t insideComment = false;
+                if (c == DELIM_COMMAND
+                    || CO_fifo_trimSpaces(src, &insideComment)
+                ) {
+                    st |= CO_fifo_st_closed;
+                    finished = true;
+                }
+                else {
+                    step = insideComment ? 6 : 5;
+                }
+            }
+            else if (c == DELIM_COMMAND) {
+                /* no closing quote, error */
+                st |= CO_fifo_st_errTok;
+            }
+            else {
+                /* copy the character */
+                CO_fifo_putc(dest, c);
+                destSpace--;
+            }
+            break;
+
+        case 3: /* previous was double quote, parsing quoted string */
+        case 4: /* previous was double quote, parsing no quoted word */
+            if (c == '"') {
+                /* escaped double quote, copy the character and continue */
+                CO_fifo_putc(dest, c);
+                destSpace--;
+                step -= 2;
+            }
+            else {
+                /* previous character was closing double quote */
+                if (step == 4) {
+                    /* no opening double quote, syntax error */
+                    st |= CO_fifo_st_errTok;
+                }
+                else {
+                    if (isgraph((int)c) == 0) {
+                        /* end of quoted string */
+                        bool_t insideComment = false;
+                        if (c == DELIM_COMMAND
+                            || CO_fifo_trimSpaces(src, &insideComment)
+                        ) {
+                            st |= CO_fifo_st_closed;
+                            finished = true;
+                        }
+                        else {
+                            step = insideComment ? 6 : 5;
+                        }
+                    }
+                    else {
+                        /* space must follow closing double quote, error */
+                        st |= CO_fifo_st_errTok;
+                    }
+                }
+            }
+            break;
+
+        case 5: { /* String token is finished, waiting for command delimiter */
+            bool_t insideComment = false;
+            if (c == DELIM_COMMAND || CO_fifo_trimSpaces(src, &insideComment)) {
+                st |= CO_fifo_st_closed;
+                finished = true;
+            }
+            else if (insideComment) {
+                step = 6;
+            }
+            else if (isgraph((int)c) != 0) {
+                if (c == '#') /* comment start */
+                    step = 6;
+                else /* syntax error */
+                    st |= CO_fifo_st_errTok;
+            }
+            break;
+        }
+        case 6: { /* String token is finished, waiting for command delimiter */
+            bool_t insideComment = true;
+            if (c == DELIM_COMMAND || CO_fifo_trimSpaces(src, &insideComment)) {
+                st |= CO_fifo_st_closed;
+                finished = true;
+            }
+            break;
+        }
+        default: /* internal error */
+            st |= CO_fifo_st_errInt;
+            break;
+        }
+    }
+
+    if (!finished) {
+        st |= CO_fifo_st_partial;
+        /* memorize variables for next iteration */
+        dest->aux = step;
+    }
+
+    if (status != NULL) *status = st;
+
+    return destSpaceStart - destSpace;
+}
+
+size_t CO_fifo_cpyTok2B64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status) {
+    /* mime-base64 decoding, see description above base64EncTable */
+
+    size_t destSpace, destSpaceStart;
+    bool_t finished = false;
+    uint8_t step;
+    uint32_t dword;
+    CO_fifo_st st = 0;
+
+    if (dest == NULL || src == NULL) {
+        return 0;
+    }
+
+    /* get free space of the dest fifo */
+    destSpaceStart = destSpace = CO_fifo_getSpace(dest);
+
+    /* is this the first write into dest? */
+    if (!dest->started) {
+        bool_t insideComment = false;
+        if (CO_fifo_trimSpaces(src, &insideComment) || insideComment) {
+            /* command delimiter found without string, this is an error */
+            st |= CO_fifo_st_errTok;
+        }
+        dest->started = true;
+        step = 0;
+        dword = 0;
+    }
+    else {
+        /* get memorized variables from previous function calls */
+        step = (uint8_t)(dest->aux >> 24);
+        dword = dest->aux & 0xFFFFFF;
+    }
+
+    /* repeat until destination space available and no error and not finished
+     * and source characters available */
+    while (destSpace >= 3 && (st & CO_fifo_st_errMask) == 0 && !finished) {
+        char c;
+        if (!CO_fifo_getc(src, &c)) {
+            break;
+        }
+
+        if (step >= 5) {
+            /* String token is finished, waiting for command delimiter */
+            bool_t insideComment = step > 5;
+            if (c == DELIM_COMMAND || CO_fifo_trimSpaces(src, &insideComment)) {
+                st |= CO_fifo_st_closed;
+                finished = true;
+            }
+            else if (insideComment) {
+                step = 6;
+            }
+            else if (isgraph((int)c) != 0 && c != '=') {
+                if (c == '#') /* comment start */
+                    step = 6;
+                else /* syntax error */
+                    st |= CO_fifo_st_errTok;
+            }
+            continue;
+        }
+
+        char code = base64DecTable[c & 0x7F];
+
+        if (c < 0 || code < 0) {
+            st |= CO_fifo_st_errTok;
+        }
+        else if (code >= 64 /* '=' (pad) or DELIM_COMMAND or space */) {
+            /* base64 string finished, write remaining bytes */
+            switch (step) {
+                case 2:
+                    CO_fifo_putc(dest, (char)(dword >> 4));
+                    destSpace --;
+                    break;
+                case 3:
+                    CO_fifo_putc(dest, (char)(dword >> 10));
+                    CO_fifo_putc(dest, (char)(dword >> 2));
+                    destSpace -= 2;
+                    break;
+            }
+
+            bool_t insideComment = false;
+            if (c == DELIM_COMMAND || CO_fifo_trimSpaces(src, &insideComment)) {
+                st |= CO_fifo_st_closed;
+                finished = true;
+            }
+            else {
+                step = insideComment ? 6 : 5;
+            }
+        }
+        else {
+            dword = (dword << 6) | code;
+            if (step++ == 3) {
+                CO_fifo_putc(dest, (char)((dword >> 16) & 0xFF));
+                CO_fifo_putc(dest, (char)((dword >> 8) & 0xFF));
+                CO_fifo_putc(dest, (char)(dword & 0xFF));
+                destSpace -= 3;
+                dword = 0;
+                step = 0;
+            }
+        }
+    } /* while ... */
+
+    if (!finished) {
+        st |= CO_fifo_st_partial;
+        /* memorize variables for next iteration */
+        dest->aux = (uint32_t)step << 24 | (dword & 0xFFFFFF);
+    }
+
+    if (status != NULL) *status = st;
+
+    return destSpaceStart - destSpace;
+}
+
+#endif /* (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_DATATYPES */
+
+#endif /* (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ENABLE */

controller_yy_app/middleware/CANopenNode/301/CO_fifo.h

@@ -0,0 +1,541 @@
+/**
+ * FIFO circular buffer
+ *
+ * @file        CO_fifo.h
+ * @ingroup     CO_CANopen_301_fifo
+ * @author      Janez Paternoster
+ * @copyright   2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CO_FIFO_H
+#define CO_FIFO_H
+
+#include "301/CO_driver.h"
+
+/* default configuration, see CO_config.h */
+#ifndef CO_CONFIG_FIFO
+#define CO_CONFIG_FIFO (0)
+#endif
+
+#if ((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ENABLE) || defined CO_DOXYGEN
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_CANopen_301_fifo FIFO circular buffer
+ * @ingroup CO_CANopen_301
+ * @{
+ *
+ * FIFO is organized as circular buffer with predefined capacity. It must be
+ * initialized by CO_fifo_init(). Functions are not not thread safe.
+ *
+ * It can be used as general purpose FIFO circular buffer for any data. Data can
+ * be written by CO_fifo_write() and read by CO_fifo_read() functions.
+ *
+ * Buffer has additional functions for usage with CiA309-3 standard. It acts as
+ * circular buffer for storing ascii commands and fetching tokens from them.
+ */
+
+/**
+ * Fifo object
+ */
+typedef struct {
+    /** Buffer of size bufSize. Initialized by CO_fifo_init() */
+    char *buf;
+    /** Initialized by CO_fifo_init() */
+    size_t bufSize;
+    /** Location in the buffer, which will be next written. */
+    size_t writePtr;
+    /** Location in the buffer, which will be next read. */
+    size_t readPtr;
+#if ((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ALT_READ) || defined CO_DOXYGEN
+    /** Location in the buffer, which will be next read. */
+    size_t altReadPtr;
+#endif
+#if ((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_DATATYPES) || defined CO_DOXYGEN
+    /** helper variable, set to false in CO_fifo_reset(), used in some
+     * functions. */
+    bool_t started;
+    /** auxiliary variable, used in some functions. */
+    uint32_t aux;
+#endif
+} CO_fifo_t;
+
+
+
+/**
+ * Initialize fifo object
+ *
+ * @param fifo This object will be initialized
+ * @param buf Pointer to externally defined buffer
+ * @param bufSize Size of the above buffer. Usable size of the buffer will be
+ * one byte less than bufSize, it is used for operation of the circular buffer.
+ */
+void CO_fifo_init(CO_fifo_t *fifo, char *buf, size_t bufSize);
+
+
+/**
+ * Reset fifo object, make it empty
+ *
+ * @param fifo This object
+ */
+static inline void CO_fifo_reset(CO_fifo_t *fifo) {
+    if (fifo != NULL) {
+        fifo->readPtr = 0;
+        fifo->writePtr = 0;
+#if (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_DATATYPES
+        fifo->started = false;
+#endif
+    }
+
+    return;
+}
+
+
+/**
+ * Purge all data in fifo object, keep other properties
+ *
+ * @param fifo This object
+ *
+ * @return true, if data were purged or false, if fifo were already empty
+ */
+static inline bool_t CO_fifo_purge(CO_fifo_t *fifo) {
+    if (fifo != NULL && fifo->readPtr != fifo->writePtr) {
+        fifo->readPtr = 0;
+        fifo->writePtr = 0;
+        return true;
+    }
+
+    return false;
+}
+
+
+/**
+ * Get free buffer space in CO_fifo_t object
+ *
+ * @param fifo This object
+ *
+ * @return number of available bytes
+ */
+static inline size_t CO_fifo_getSpace(CO_fifo_t *fifo) {
+    int sizeLeft = (int)fifo->readPtr - fifo->writePtr - 1;
+    if (sizeLeft < 0) {
+        sizeLeft += fifo->bufSize;
+    }
+
+    return (size_t) sizeLeft;
+}
+
+
+/**
+ * Get size of data inside CO_fifo_t buffer object
+ *
+ * @param fifo This object
+ *
+ * @return number of occupied bytes
+ */
+static inline size_t CO_fifo_getOccupied(CO_fifo_t *fifo) {
+    int sizeOccupied = (int)fifo->writePtr - fifo->readPtr;
+    if (sizeOccupied < 0) {
+        sizeOccupied += fifo->bufSize;
+    }
+
+    return (size_t) sizeOccupied;
+}
+
+
+/**
+ * Put one character into CO_fifo_t buffer object
+ *
+ * @param fifo This object
+ * @param c Character to put
+ *
+ * @return true, if write was successful (enough space in fifo buffer)
+ */
+static inline bool_t CO_fifo_putc(CO_fifo_t *fifo, const char c) {
+    if (fifo != NULL && fifo->buf != NULL) {
+        size_t writePtrNext = fifo->writePtr + 1;
+        if (writePtrNext != fifo->readPtr &&
+            !(writePtrNext == fifo->bufSize && fifo->readPtr == 0))
+        {
+            fifo->buf[fifo->writePtr] = c;
+            fifo->writePtr = (writePtrNext == fifo->bufSize) ? 0 : writePtrNext;
+            return true;
+        }
+    }
+    return false;
+}
+
+
+/**
+ * Put one character into CO_fifo_t buffer object
+ *
+ * Overwrite old characters, if run out of space
+ *
+ * @param fifo This object
+ * @param c Character to put
+ */
+static inline void CO_fifo_putc_ov(CO_fifo_t *fifo, const char c) {
+    if (fifo != NULL && fifo->buf != NULL) {
+        fifo->buf[fifo->writePtr] = c;
+
+        if (++fifo->writePtr == fifo->bufSize) fifo->writePtr = 0;
+        if (fifo->readPtr == fifo->writePtr) {
+            if (++fifo->readPtr == fifo->bufSize) fifo->readPtr = 0;
+        }
+    }
+}
+
+
+/**
+ * Get one character from CO_fifo_t buffer object
+ *
+ * @param fifo This object
+ * @param buf Buffer of length one byte, where character will be copied
+ *
+ * @return true, if read was successful (non-empty fifo buffer)
+ */
+static inline bool_t CO_fifo_getc(CO_fifo_t *fifo, char *buf) {
+    if (fifo != NULL && buf != NULL && fifo->readPtr != fifo->writePtr) {
+        *buf = fifo->buf[fifo->readPtr];
+        if (++fifo->readPtr == fifo->bufSize) {
+            fifo->readPtr = 0;
+        }
+        return true;
+    }
+    return false;
+}
+
+
+/**
+ * Write data into CO_fifo_t object.
+ *
+ * This function copies data from buf into internal buffer of CO_fifo_t
+ * object. Function returns number of bytes successfully copied.
+ * If there is not enough space in destination, not all bytes will be copied.
+ *
+ * @param fifo This object
+ * @param buf Buffer which will be copied
+ * @param count Number of bytes in buf
+ * @param [in,out] crc Externally defined variable for CRC checksum, ignored if
+ * NULL
+ *
+ * @return number of bytes actually written.
+ */
+size_t CO_fifo_write(CO_fifo_t *fifo,
+                     const char *buf,
+                     size_t count,
+                     uint16_t *crc);
+
+
+/**
+ * Read data from CO_fifo_t object.
+ *
+ * This function copies data from internal buffer of CO_fifo_t object into
+ * buf. Function returns number of bytes successfully copied. Function also
+ * writes true into eof argument, if command delimiter character is reached.
+ *
+ * @param fifo This object
+ * @param buf Buffer into which data will be copied
+ * @param count Copy up to count bytes into buffer
+ * @param [out] eof If different than NULL, then search for delimiter character.
+ * If found, then read up to (including) that character and set *eof to true.
+ * Otherwise set *eof to false.
+ *
+ * @return number of bytes actually read.
+ */
+size_t CO_fifo_read(CO_fifo_t *fifo, char *buf, size_t count, bool_t *eof);
+
+
+#if ((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ALT_READ) || defined CO_DOXYGEN
+/**
+ * Initializes alternate read with #CO_fifo_altRead
+ *
+ * @param fifo This object
+ * @param offset Offset in bytes from original read pointer
+ *
+ * @return same as offset or lower, if there is not enough data.
+ */
+size_t CO_fifo_altBegin(CO_fifo_t *fifo, size_t offset);
+
+
+/**
+ * Ends alternate read with #CO_fifo_altRead and calculate crc checksum
+ *
+ * @param fifo This object
+ * @param [in,out] crc Externally defined variable for CRC checksum, ignored if
+ * NULL
+ */
+void CO_fifo_altFinish(CO_fifo_t *fifo, uint16_t *crc);
+
+
+/**
+ * Get alternate size of remaining data, see #CO_fifo_altRead
+ *
+ * @param fifo This object
+ *
+ * @return number of occupied bytes.
+ */
+static inline size_t CO_fifo_altGetOccupied(CO_fifo_t *fifo) {
+    int sizeOccupied = (int)fifo->writePtr - fifo->altReadPtr;
+    if (sizeOccupied < 0) {
+        sizeOccupied += fifo->bufSize;
+    }
+
+    return (size_t) sizeOccupied;
+}
+
+
+/**
+ * Alternate read data from CO_fifo_t object.
+ *
+ * This function is similar as CO_fifo_read(), but uses alternate read pointer
+ * inside circular buffer. It reads data from the buffer and data remains in it.
+ * This function uses alternate read pointer and keeps original read pointer
+ * unchanged. Before using this function, alternate read pointer must be
+ * initialized with CO_fifo_altBegin(). CO_fifo_altFinish() sets original read
+ * pointer to alternate read pointer and so empties the buffer.
+ *
+ * @param fifo This object
+ * @param buf Buffer into which data will be copied
+ * @param count Copy up to count bytes into buffer
+ *
+ * @return number of bytes actually read.
+ */
+size_t CO_fifo_altRead(CO_fifo_t *fifo, char *buf, size_t count);
+#endif /* #if (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ALT_READ */
+
+
+#if ((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_COMMANDS) || defined CO_DOXYGEN
+/**
+ * Search command inside FIFO
+ *
+ * If there are some data inside the FIFO, then search for command delimiter.
+ *
+ * If command is long, then in the buffer may not be enough space for it.
+ * In that case buffer is full and no delimiter is present. Function then
+ * returns true and command should be processed for the starting tokens.
+ * Buffer can later be refilled multiple times, until command is closed by
+ * command delimiter.
+ *
+ * If this function returns different than 0, then buffer is usually read
+ * by multiple CO_fifo_readToken() calls. If reads was successful, then
+ * delimiter is reached and fifo->readPtr is set after the command. If any
+ * CO_fifo_readToken() returns nonzero *err, then there is an error and command
+ * should be cleared. All this procedure must be implemented inside single
+ * function call.
+ *
+ * @param fifo This object.
+ * @param clear If true, then command will be cleared from the
+ * buffer. If there is no delimiter, buffer will be cleared entirely.
+ *
+ * @return true if command with delimiter found or buffer full.
+ */
+bool_t CO_fifo_CommSearch(CO_fifo_t *fifo, bool_t clear);
+
+
+/**
+ * Trim spaces inside FIFO
+ *
+ * Function removes all non graphical characters and comments from fifo
+ * buffer. It stops on first graphical character or on command delimiter (later
+ * is also removed).
+ *
+ * @param fifo This object.
+ * @param [in, out] insideComment if set to true as input, it skips all
+ * characters and searches only for delimiter. As output it is set to true, if
+ * fifo is empty, is inside comment and command delimiter is not found.
+ *
+ * @return true if command delimiter was found.
+ */
+bool_t CO_fifo_trimSpaces(CO_fifo_t *fifo, bool_t *insideComment);
+
+
+/**
+ * Get token from FIFO buffer
+ *
+ * Function search FIFO buffer for token. Token is string of only graphical
+ * characters. Graphical character is any printable character except space with
+ * acsii code within limits: 0x20 < code < 0x7F (see isgraph() function).
+ *
+ * If token is found, then copy it to the buf, if count is large enough. On
+ * success also set readPtr to point to the next graphical character.
+ *
+ * Each token must have at least one empty space after it (space, command
+ * delimiter, '\0', etc.). Delimiter must not be graphical character.
+ *
+ * If comment delimiter (delimComment, see #CO_fifo_init) character is found,
+ * then all string till command delimiter (delimCommand, see #CO_fifo_init) will
+ * be erased from the buffer.
+ *
+ * See also #CO_fifo_CommSearch().
+ *
+ * @param fifo This object.
+ * @param buf Buffer into which data will be copied. Will be terminated by '\0'.
+ * @param count Copy up to count bytes into buffer
+ * @param [in,out] closed This is input/output variable. Not used if NULL.
+ * - As output variable it is set to 1, if command delimiter (delimCommand,
+ *   see #CO_fifo_init) is found after the token and set to 0 otherwise.
+ * - As input variable it is used for verifying error condition:
+ *  - *closed = 0: Set *err to true if token is empty or command delimiter
+ *                 is found.
+ *  - *closed = 1: Set *err to true if token is empty or command delimiter
+ *                 is NOT found.
+ *  - *closed = any other value: No checking of token size or command delimiter.
+ * @param [out] err If not NULL, it is set to true if token is larger than buf
+ * or in matching combination in 'closed' argument. If it is already true, then
+ * function returns immediately.
+ *
+ * @return Number of bytes read.
+ */
+size_t CO_fifo_readToken(CO_fifo_t *fifo,
+                         char *buf,
+                         size_t count,
+                         char *closed,
+                         bool_t *err);
+#endif /* (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_COMMANDS */
+
+
+#if ((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_DATATYPES) || defined CO_DOXYGEN
+/**
+ * Read uint8_t variable from fifo and output as ascii string.
+ *
+ * @param fifo This object.
+ * @param buf Buffer into which ascii string will be copied.
+ * @param count Available count of bytes inside the buf.
+ * @param end True indicates, that fifo contains last bytes of data.
+ *
+ * @return Number of ascii bytes written into buf.
+ */
+size_t CO_fifo_readU82a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+/** Read uint16_t variable from fifo as ascii string, see CO_fifo_readU82a */
+size_t CO_fifo_readU162a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+/** Read uint32_t variable from fifo as ascii string, see CO_fifo_readU82a */
+size_t CO_fifo_readU322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+/** Read uint64_t variable from fifo as ascii string, see CO_fifo_readU82a */
+size_t CO_fifo_readU642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+/** Read uint8_t variable from fifo as ascii string, see CO_fifo_readU82a */
+size_t CO_fifo_readX82a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+/** Read uint16_t variable from fifo as ascii string, see CO_fifo_readU82a */
+size_t CO_fifo_readX162a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+/** Read uint32_t variable from fifo as ascii string, see CO_fifo_readU82a */
+size_t CO_fifo_readX322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+/** Read uint64_t variable from fifo as ascii string, see CO_fifo_readU82a */
+size_t CO_fifo_readX642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+/** Read int8_t variable from fifo as ascii string, see CO_fifo_readU82a */
+size_t CO_fifo_readI82a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+/** Read int16_t variable from fifo as ascii string, see CO_fifo_readU82a */
+size_t CO_fifo_readI162a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+/** Read int32_t variable from fifo as ascii string, see CO_fifo_readU82a */
+size_t CO_fifo_readI322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+/** Read int64_t variable from fifo as ascii string, see CO_fifo_readU82a */
+size_t CO_fifo_readI642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+/** Read float32_t variable from fifo as ascii string, see CO_fifo_readU82a */
+size_t CO_fifo_readR322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+/** Read float64_t variable from fifo as ascii string, see CO_fifo_readU82a */
+size_t CO_fifo_readR642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+/** Read data from fifo and output as space separated two digit ascii string,
+ * see also CO_fifo_readU82a */
+size_t CO_fifo_readHex2a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+/** Read data from fifo and output as visible string. A visible string is
+ * enclosed with double quotes. If a double quote is used within the string,
+ * the quotes are escaped by a second quotes, e.g. “Hello “”World””, CANopen
+ * is great”. UTF-8 characters and also line breaks works with this function.
+ * Function removes all NULL and CR characters from output string.
+ * See also CO_fifo_readU82a */
+size_t CO_fifo_readVs2a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+/** Read data from fifo and output as mime-base64 encoded string. Encoding is as
+ * specified in RFC 2045, without CR-LF, but one long string. See also
+ * CO_fifo_readU82a */
+size_t CO_fifo_readB642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+
+
+/** Bitfields for status argument from CO_fifo_cpyTok2U8 function and similar */
+typedef enum {
+    /** Bit is set, if command delimiter is reached in src */
+    CO_fifo_st_closed  = 0x01U,
+    /** Bit is set, if copy was partial and more data are available. If unset
+     * and no error, then all data was successfully copied. */
+    CO_fifo_st_partial = 0x02U,
+    /** Bit is set, if no valid token found */
+    CO_fifo_st_errTok  = 0x10U,
+    /** Bit is set, if value is not valid or out of limits */
+    CO_fifo_st_errVal  = 0x20U,
+    /** Bit is set, if destination buffer is to small */
+    CO_fifo_st_errBuf  = 0x40U,
+    /** Bit is set, if internal error */
+    CO_fifo_st_errInt  = 0x80U,
+    /** Bitmask for error bits */
+    CO_fifo_st_errMask = 0xF0U
+} CO_fifo_st;
+
+/**
+ * Read ascii string from src fifo and copy as uint8_t variable to dest fifo.
+ *
+ * @param dest destination fifo buffer object.
+ * @param src source fifo buffer object.
+ * @param [out] status bitfield of the CO_fifo_st type.
+ *
+ * @return Number of bytes written into dest.
+ */
+size_t CO_fifo_cpyTok2U8 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+/** Copy ascii string to uint16_t variable, see CO_fifo_cpyTok2U8 */
+size_t CO_fifo_cpyTok2U16(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+/** Copy ascii string to uint32_t variable, see CO_fifo_cpyTok2U8 */
+size_t CO_fifo_cpyTok2U32(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+/** Copy ascii string to uint64_t variable, see CO_fifo_cpyTok2U8 */
+size_t CO_fifo_cpyTok2U64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+/** Copy ascii string to int8_t variable, see CO_fifo_cpyTok2U8 */
+size_t CO_fifo_cpyTok2I8 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+/** Copy ascii string to int16_t variable, see CO_fifo_cpyTok2U8 */
+size_t CO_fifo_cpyTok2I16(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+/** Copy ascii string to int32_t variable, see CO_fifo_cpyTok2U8 */
+size_t CO_fifo_cpyTok2I32(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+/** Copy ascii string to int64_t variable, see CO_fifo_cpyTok2U8 */
+size_t CO_fifo_cpyTok2I64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+/** Copy ascii string to float32_t variable, see CO_fifo_cpyTok2U8 */
+size_t CO_fifo_cpyTok2R32(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+/** Copy ascii string to float64_t variable, see CO_fifo_cpyTok2U8 */
+size_t CO_fifo_cpyTok2R64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+/** Copy bytes written as two hex digits into to data. Bytes may be space
+ * separated. See CO_fifo_cpyTok2U8 for parameters. */
+size_t CO_fifo_cpyTok2Hex(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+/** Copy visible string to data. A visible string must be enclosed with double
+ * quotes, if it contains space. If a double quote is used within the string,
+ * the quotes are escaped by a second quotes. Input string can not contain
+ * newline characters. See CO_fifo_cpyTok2U8 */
+size_t CO_fifo_cpyTok2Vs(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+/** Read ascii mime-base64 encoded string from src fifo and copy as binary data
+ * to dest fifo. Encoding is as specified in RFC 2045, without CR-LF, but one
+ * long string in single line. See also CO_fifo_readU82a */
+size_t CO_fifo_cpyTok2B64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+
+#endif /* (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_DATATYPES */
+
+/** @} */ /* CO_CANopen_301_fifo */
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+#endif /* (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ENABLE */
+
+#endif /* CO_FIFO_H */

controller_yy_app/middleware/CANopenNode/301/crc16-ccitt.c

@@ -0,0 +1,106 @@
+/*
+ * Calculation of CRC 16 CCITT polynomial, x^16 + x^12 + x^5 + 1.
+ *
+ * @file        crc16-ccitt.c
+ * @ingroup     crc16-ccitt
+ * @author      Janez Paternoster
+ * @copyright   2012 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "301/crc16-ccitt.h"
+
+#if (CO_CONFIG_CRC16) & CO_CONFIG_CRC16_ENABLE
+#if !((CO_CONFIG_CRC16) & CO_CONFIG_CRC16_EXTERNAL)
+
+/*
+ * CRC table calculated by the following algorithm:
+ *
+ * void crc16_ccitt_table_init(void){
+ *     uint16_t i, j;
+ *     for(i=0; i<256; i++){
+ *         uint16_t crc = 0;
+ *         uint16_t c = i << 8;
+ *         for(j=0; j<8; j++){
+ *             if ((crc ^ c) & 0x8000) crc = (crc << 1) ^ 0x1021;
+ *             else                   crc = crc << 1;
+ *             c = c << 1;
+ *         }
+ *         crc16_ccitt_table[i] = crc;
+ *     }
+ * }
+ */
+static const uint16_t crc16_ccitt_table[256] = {
+    0x0000U, 0x1021U, 0x2042U, 0x3063U, 0x4084U, 0x50A5U, 0x60C6U, 0x70E7U,
+    0x8108U, 0x9129U, 0xA14AU, 0xB16BU, 0xC18CU, 0xD1ADU, 0xE1CEU, 0xF1EFU,
+    0x1231U, 0x0210U, 0x3273U, 0x2252U, 0x52B5U, 0x4294U, 0x72F7U, 0x62D6U,
+    0x9339U, 0x8318U, 0xB37BU, 0xA35AU, 0xD3BDU, 0xC39CU, 0xF3FFU, 0xE3DEU,
+    0x2462U, 0x3443U, 0x0420U, 0x1401U, 0x64E6U, 0x74C7U, 0x44A4U, 0x5485U,
+    0xA56AU, 0xB54BU, 0x8528U, 0x9509U, 0xE5EEU, 0xF5CFU, 0xC5ACU, 0xD58DU,
+    0x3653U, 0x2672U, 0x1611U, 0x0630U, 0x76D7U, 0x66F6U, 0x5695U, 0x46B4U,
+    0xB75BU, 0xA77AU, 0x9719U, 0x8738U, 0xF7DFU, 0xE7FEU, 0xD79DU, 0xC7BCU,
+    0x48C4U, 0x58E5U, 0x6886U, 0x78A7U, 0x0840U, 0x1861U, 0x2802U, 0x3823U,
+    0xC9CCU, 0xD9EDU, 0xE98EU, 0xF9AFU, 0x8948U, 0x9969U, 0xA90AU, 0xB92BU,
+    0x5AF5U, 0x4AD4U, 0x7AB7U, 0x6A96U, 0x1A71U, 0x0A50U, 0x3A33U, 0x2A12U,
+    0xDBFDU, 0xCBDCU, 0xFBBFU, 0xEB9EU, 0x9B79U, 0x8B58U, 0xBB3BU, 0xAB1AU,
+    0x6CA6U, 0x7C87U, 0x4CE4U, 0x5CC5U, 0x2C22U, 0x3C03U, 0x0C60U, 0x1C41U,
+    0xEDAEU, 0xFD8FU, 0xCDECU, 0xDDCDU, 0xAD2AU, 0xBD0BU, 0x8D68U, 0x9D49U,
+    0x7E97U, 0x6EB6U, 0x5ED5U, 0x4EF4U, 0x3E13U, 0x2E32U, 0x1E51U, 0x0E70U,
+    0xFF9FU, 0xEFBEU, 0xDFDDU, 0xCFFCU, 0xBF1BU, 0xAF3AU, 0x9F59U, 0x8F78U,
+    0x9188U, 0x81A9U, 0xB1CAU, 0xA1EBU, 0xD10CU, 0xC12DU, 0xF14EU, 0xE16FU,
+    0x1080U, 0x00A1U, 0x30C2U, 0x20E3U, 0x5004U, 0x4025U, 0x7046U, 0x6067U,
+    0x83B9U, 0x9398U, 0xA3FBU, 0xB3DAU, 0xC33DU, 0xD31CU, 0xE37FU, 0xF35EU,
+    0x02B1U, 0x1290U, 0x22F3U, 0x32D2U, 0x4235U, 0x5214U, 0x6277U, 0x7256U,
+    0xB5EAU, 0xA5CBU, 0x95A8U, 0x8589U, 0xF56EU, 0xE54FU, 0xD52CU, 0xC50DU,
+    0x34E2U, 0x24C3U, 0x14A0U, 0x0481U, 0x7466U, 0x6447U, 0x5424U, 0x4405U,
+    0xA7DBU, 0xB7FAU, 0x8799U, 0x97B8U, 0xE75FU, 0xF77EU, 0xC71DU, 0xD73CU,
+    0x26D3U, 0x36F2U, 0x0691U, 0x16B0U, 0x6657U, 0x7676U, 0x4615U, 0x5634U,
+    0xD94CU, 0xC96DU, 0xF90EU, 0xE92FU, 0x99C8U, 0x89E9U, 0xB98AU, 0xA9ABU,
+    0x5844U, 0x4865U, 0x7806U, 0x6827U, 0x18C0U, 0x08E1U, 0x3882U, 0x28A3U,
+    0xCB7DU, 0xDB5CU, 0xEB3FU, 0xFB1EU, 0x8BF9U, 0x9BD8U, 0xABBBU, 0xBB9AU,
+    0x4A75U, 0x5A54U, 0x6A37U, 0x7A16U, 0x0AF1U, 0x1AD0U, 0x2AB3U, 0x3A92U,
+    0xFD2EU, 0xED0FU, 0xDD6CU, 0xCD4DU, 0xBDAAU, 0xAD8BU, 0x9DE8U, 0x8DC9U,
+    0x7C26U, 0x6C07U, 0x5C64U, 0x4C45U, 0x3CA2U, 0x2C83U, 0x1CE0U, 0x0CC1U,
+    0xEF1FU, 0xFF3EU, 0xCF5DU, 0xDF7CU, 0xAF9BU, 0xBFBAU, 0x8FD9U, 0x9FF8U,
+    0x6E17U, 0x7E36U, 0x4E55U, 0x5E74U, 0x2E93U, 0x3EB2U, 0x0ED1U, 0x1EF0U
+};
+
+
+/******************************************************************************/
+void crc16_ccitt_single(uint16_t *crc, const uint8_t chr) {
+    uint8_t tmp = (uint8_t)(*crc >> 8U) ^ chr;
+    *crc = (*crc << 8U) ^ crc16_ccitt_table[tmp];
+}
+
+
+/******************************************************************************/
+uint16_t crc16_ccitt(const uint8_t block[],
+                     size_t blockLength,
+                     uint16_t crc)
+{
+    size_t i;
+
+    for (i = 0U; i < blockLength; i++) {
+        uint8_t tmp = (uint8_t)(crc >> 8U) ^ block[i];
+        crc = (crc << 8U) ^ crc16_ccitt_table[tmp];
+    }
+    return crc;
+}
+
+#endif /* !((CO_CONFIG_CRC16) & CO_CONFIG_CRC16_EXTERNAL) */
+#endif /* (CO_CONFIG_CRC16) & CO_CONFIG_CRC16_ENABLE */

controller_yy_app/middleware/CANopenNode/301/crc16-ccitt.h

@@ -0,0 +1,92 @@
+/**
+ * Calculation of CRC 16 CCITT polynomial.
+ *
+ * @file        crc16-ccitt.h
+ * @ingroup     CO_crc16_ccitt
+ * @author      Lammert Bies
+ * @author      Janez Paternoster
+ * @copyright   2012 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CRC16_CCITT_H
+#define CRC16_CCITT_H
+
+#include "301/CO_driver.h"
+
+/* default configuration, see CO_config.h */
+#ifndef CO_CONFIG_CRC16
+#define CO_CONFIG_CRC16 (0)
+#endif
+
+#if ((CO_CONFIG_CRC16) & CO_CONFIG_CRC16_ENABLE) || defined CO_DOXYGEN
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_crc16_ccitt CRC 16 CCITT
+ * @ingroup CO_CANopen_301
+ * @{
+ *
+ * Calculation of CRC 16 CCITT polynomial.
+ *
+ * Equation:
+ *
+ * `x^16 + x^12 + x^5 + 1`
+ */
+
+
+/**
+ * Update crc16_ccitt variable with one data byte
+ *
+ * This function updates crc variable for one data byte using crc16 ccitt
+ * algorithm.
+ *
+ * @param [in,out] crc Externally defined variable for CRC checksum. Before
+ * start of new CRC calculation, variable must be initialized (zero for xmodem).
+ * @param chr One byte of data
+ */
+void crc16_ccitt_single(uint16_t *crc, const uint8_t chr);
+
+
+/**
+ * Calculate CRC sum on block of data.
+ *
+ * @param block Pointer to block of data.
+ * @param blockLength Length of data in bytes;
+ * @param crc Initial value (zero for xmodem). If block is split into
+ * multiple segments, previous CRC is used as initial.
+ *
+ * @return Calculated CRC.
+ */
+uint16_t crc16_ccitt(const uint8_t block[],
+                     size_t blockLength,
+                     uint16_t crc);
+
+
+/** @} */ /* CO_crc16_ccitt */
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+#endif /* (CO_CONFIG_CRC16) & CO_CONFIG_CRC16_ENABLE */
+
+#endif /* CRC16_CCITT_H */

controller_yy_app/middleware/CANopenNode/303/CMakeLists.txt

@@ -0,0 +1,5 @@
+# Copyright (c) 2021-2024 HPMicro
+# SPDX-License-Identifier: BSD-3-Clause
+
+sdk_inc(.)
+sdk_src(CO_LEDs.c)

controller_yy_app/middleware/CANopenNode/303/CO_LEDs.c

@@ -0,0 +1,155 @@
+/*
+ * CANopen Indicator specification (CiA 303-3 v1.4.0)
+ *
+ * @file        CO_LEDs.h
+ * @ingroup     CO_LEDs
+ * @author      Janez Paternoster
+ * @copyright   2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "303/CO_LEDs.h"
+
+#if (CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE
+
+/******************************************************************************/
+CO_ReturnError_t CO_LEDs_init(CO_LEDs_t *LEDs) {
+    CO_ReturnError_t ret = CO_ERROR_NO;
+
+    /* verify arguments */
+    if (LEDs == NULL) {
+        return CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    /* clear the object */
+    memset(LEDs, 0, sizeof(CO_LEDs_t));
+
+    return ret;
+}
+
+
+/******************************************************************************/
+void CO_LEDs_process(CO_LEDs_t *LEDs,
+                     uint32_t timeDifference_us,
+                     CO_NMT_internalState_t NMTstate,
+                     bool_t LSSconfig,
+                     bool_t ErrCANbusOff,
+                     bool_t ErrCANbusWarn,
+                     bool_t ErrRpdo,
+                     bool_t ErrSync,
+                     bool_t ErrHbCons,
+                     bool_t ErrOther,
+                     bool_t firmwareDownload,
+                     uint32_t *timerNext_us)
+{
+    (void)timerNext_us; /* may be unused */
+
+    uint8_t rd = 0;
+    uint8_t gr = 0;
+    bool_t tick = false;
+
+    LEDs->LEDtmr50ms += timeDifference_us;
+    while (LEDs->LEDtmr50ms >= 50000) {
+        bool_t rdFlickerNext = (LEDs->LEDred & CO_LED_flicker) == 0;
+
+        tick = true;
+        LEDs->LEDtmr50ms -= 50000;
+
+        if (++LEDs->LEDtmr200ms > 3) {
+            /* calculate 2,5Hz blinking and flashing */
+            LEDs->LEDtmr200ms = 0;
+            rd = gr = 0;
+
+            if ((LEDs->LEDred & CO_LED_blink) == 0) rd |= CO_LED_blink;
+            else                                    gr |= CO_LED_blink;
+
+            switch (++LEDs->LEDtmrflash_1) {
+                case 1: rd |= CO_LED_flash_1; break;
+                case 2: gr |= CO_LED_flash_1; break;
+                case 6: LEDs->LEDtmrflash_1 = 0; break;
+                default: break;
+            }
+            switch (++LEDs->LEDtmrflash_2) {
+                case 1: case 3: rd |= CO_LED_flash_2; break;
+                case 2: case 4: gr |= CO_LED_flash_2; break;
+                case 8: LEDs->LEDtmrflash_2 = 0; break;
+                default: break;
+            }
+            switch (++LEDs->LEDtmrflash_3) {
+                case 1: case 3: case 5: rd |= CO_LED_flash_3; break;
+                case 2: case 4: case 6: gr |= CO_LED_flash_3; break;
+                case 10: LEDs->LEDtmrflash_3 = 0; break;
+                default: break;
+            }
+            switch (++LEDs->LEDtmrflash_4) {
+                case 1: case 3: case 5: case 7: rd |= CO_LED_flash_4; break;
+                case 2: case 4: case 6: case 8: gr |= CO_LED_flash_4; break;
+                case 12: LEDs->LEDtmrflash_4 = 0; break;
+                default: break;
+            }
+        }
+        else {
+            /* clear flicker and CANopen bits, keep others */
+            rd = LEDs->LEDred & (0xFF ^ (CO_LED_flicker | CO_LED_CANopen));
+            gr = LEDs->LEDgreen & (0xFF ^ (CO_LED_flicker | CO_LED_CANopen));
+        }
+
+        /* calculate 10Hz flickering */
+        if (rdFlickerNext) rd |= CO_LED_flicker;
+        else               gr |= CO_LED_flicker;
+
+    } /* while (LEDs->LEDtmr50ms >= 50000) */
+
+    if (tick) {
+        uint8_t rd_co, gr_co;
+
+        /* CANopen red ERROR LED */
+        if      (ErrCANbusOff)                      rd_co = 1;
+        else if (NMTstate == CO_NMT_INITIALIZING)   rd_co = rd & CO_LED_flicker;
+        else if (ErrRpdo)                           rd_co = rd & CO_LED_flash_4;
+        else if (ErrSync)                           rd_co = rd & CO_LED_flash_3;
+        else if (ErrHbCons)                         rd_co = rd & CO_LED_flash_2;
+        else if (ErrCANbusWarn)                     rd_co = rd & CO_LED_flash_1;
+        else if (ErrOther)                          rd_co = rd & CO_LED_blink;
+        else                                        rd_co = 0;
+
+        /* CANopen green RUN LED */
+        if      (LSSconfig)                         gr_co = gr & CO_LED_flicker;
+        else if (firmwareDownload)                  gr_co = gr & CO_LED_flash_3;
+        else if (NMTstate == CO_NMT_STOPPED)        gr_co = gr & CO_LED_flash_1;
+        else if (NMTstate == CO_NMT_PRE_OPERATIONAL)gr_co = gr & CO_LED_blink;
+        else if (NMTstate == CO_NMT_OPERATIONAL)    gr_co = 1;
+        else                                        gr_co = 0;
+
+        if (rd_co != 0) rd |= CO_LED_CANopen;
+        if (gr_co != 0) gr |= CO_LED_CANopen;
+        LEDs->LEDred = rd;
+        LEDs->LEDgreen = gr;
+    } /* if (tick) */
+
+#if (CO_CONFIG_LEDS) & CO_CONFIG_FLAG_TIMERNEXT
+    if (timerNext_us != NULL) {
+        uint32_t diff = 50000 - LEDs->LEDtmr50ms;
+        if (*timerNext_us > diff) {
+            *timerNext_us = diff;
+        }
+    }
+#endif
+}
+
+#endif /* (CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE */

controller_yy_app/middleware/CANopenNode/303/CO_LEDs.h

@@ -0,0 +1,157 @@
+/**
+ * CANopen Indicator specification (CiA 303-3 v1.4.0)
+ *
+ * @file        CO_LEDs.h
+ * @ingroup     CO_LEDs
+ * @author      Janez Paternoster
+ * @copyright   2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CO_LEDS_H
+#define CO_LEDS_H
+
+#include "301/CO_driver.h"
+#include "301/CO_NMT_Heartbeat.h"
+
+/* default configuration, see CO_config.h */
+#ifndef CO_CONFIG_LEDS
+#define CO_CONFIG_LEDS (CO_CONFIG_LEDS_ENABLE)
+#endif
+
+#if ((CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE) || defined CO_DOXYGEN
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_LEDs LED indicators
+ * @ingroup CO_CANopen_303
+ * @{
+ *
+ * CIA 303-3 standard specifies indicator LED diodes, which reflects state of
+ * the CANopen device. Green and red leds or bi-color led can be used.
+ *
+ * CANopen green led - run led:
+ * - flickering: LSS configuration state is active
+ * - blinking: device is in NMT pre-operational state
+ * - single flash: device is in NMT stopped state
+ * - triple flash: a software download is running in the device
+ * - on: device is in NMT operational state
+ *
+ * CANopen red led - error led:
+ * - off: no error
+ * - flickering: LSS node id is not configured, CANopen is not initialized
+ * - blinking: invalid configuration, general error
+ * - single flash: CAN warning limit reached
+ * - double flash: heartbeat consumer - error in remote monitored node
+ * - triple flash: sync message reception timeout
+ * - quadruple flash: PDO has not been received before the event timer elapsed
+ * - on: CAN bus off
+ *
+ * To apply on/off state to led diode, use #CO_LED_RED and #CO_LED_GREEN macros.
+ * For CANopen leds use CO_LED_BITFIELD_t CO_LED_CANopen. Other bitfields are
+ * available for implementing custom leds.
+ */
+
+/** Bitfield for combining with red or green led */
+typedef enum {
+    CO_LED_flicker = 0x01,  /**< LED flickering 10Hz */
+    CO_LED_blink   = 0x02,  /**< LED blinking 2,5Hz */
+    CO_LED_flash_1 = 0x04,  /**< LED single flash */
+    CO_LED_flash_2 = 0x08,  /**< LED double flash */
+    CO_LED_flash_3 = 0x10,  /**< LED triple flash */
+    CO_LED_flash_4 = 0x20,  /**< LED quadruple flash */
+    CO_LED_CANopen = 0x80   /**< LED CANopen according to CiA 303-3 */
+} CO_LED_BITFIELD_t;
+
+/** Get on/off state for green led for specified bitfield */
+#define CO_LED_RED(LEDs, BITFIELD) (((LEDs)->LEDred & BITFIELD) ? 1 : 0)
+/** Get on/off state for green led for specified bitfield */
+#define CO_LED_GREEN(LEDs, BITFIELD) (((LEDs)->LEDgreen & BITFIELD) ? 1 : 0)
+
+
+/**
+ * LEDs object, initialized by CO_LEDs_init()
+ */
+typedef struct{
+    uint32_t            LEDtmr50ms;     /**< 50ms led timer */
+    uint8_t             LEDtmr200ms;    /**< 200ms led timer */
+    uint8_t             LEDtmrflash_1;  /**< single flash led timer */
+    uint8_t             LEDtmrflash_2;  /**< double flash led timer */
+    uint8_t             LEDtmrflash_3;  /**< triple flash led timer */
+    uint8_t             LEDtmrflash_4;  /**< quadruple flash led timer */
+    uint8_t             LEDred;         /**< red led #CO_LED_BITFIELD_t */
+    uint8_t             LEDgreen;       /**< green led #CO_LED_BITFIELD_t */
+} CO_LEDs_t;
+
+
+/**
+ * Initialize LEDs object.
+ *
+ * Function must be called in the communication reset section.
+ *
+ * @param LEDs This object will be initialized.
+ *
+ * @return #CO_ReturnError_t CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ */
+CO_ReturnError_t CO_LEDs_init(CO_LEDs_t *LEDs);
+
+
+/**
+ * Process indicator states
+ *
+ * Function must be called cyclically.
+ *
+ * @param LEDs This object.
+ * @param timeDifference_us Time difference from previous function call in
+ *                          [microseconds].
+ * @param NMTstate NMT operating state.
+ * @param LSSconfig Node is in LSS configuration state indication.
+ * @param ErrCANbusOff CAN bus off indication (highest priority).
+ * @param ErrCANbusWarn CAN error warning limit reached indication.
+ * @param ErrRpdo RPDO event timer timeout indication.
+ * @param ErrSync Sync receive timeout indication.
+ * @param ErrHbCons Heartbeat consumer error (remote node) indication.
+ * @param ErrOther Other error indication (lowest priority).
+ * @param firmwareDownload Firmware download is in progress indication.
+ * @param [out] timerNext_us info to OS - see CO_process().
+ */
+void CO_LEDs_process(CO_LEDs_t *LEDs,
+                     uint32_t timeDifference_us,
+                     CO_NMT_internalState_t NMTstate,
+                     bool_t LSSconfig,
+                     bool_t ErrCANbusOff,
+                     bool_t ErrCANbusWarn,
+                     bool_t ErrRpdo,
+                     bool_t ErrSync,
+                     bool_t ErrHbCons,
+                     bool_t ErrOther,
+                     bool_t firmwareDownload,
+                     uint32_t *timerNext_us);
+
+/** @} */ /* CO_LEDs */
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+#endif /* (CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE */
+
+#endif /* CO_LEDS_H */

controller_yy_app/middleware/CANopenNode/304/CMakeLists.txt

@@ -0,0 +1,6 @@
+# Copyright (c) 2021-2024 HPMicro
+# SPDX-License-Identifier: BSD-3-Clause
+
+sdk_inc(.)
+sdk_src(CO_GFC.c)
+sdk_src(CO_SRDO.c)

controller_yy_app/middleware/CANopenNode/304/CO_GFC.c

@@ -0,0 +1,131 @@
+/**
+ * CANopen Global fail-safe command protocol.
+ *
+ * @file        CO_GFC.c
+ * @ingroup     CO_GFC
+ * @author      Robert Grüning
+ * @copyright   2020 - 2020 Robert Grüning
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "304/CO_GFC.h"
+
+#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_ENABLE
+
+#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_CONSUMER
+
+static void CO_GFC_receive(void *object, void *msg)
+{
+    CO_GFC_t *GFC;
+    uint8_t DLC = CO_CANrxMsg_readDLC(msg);
+
+    GFC = (CO_GFC_t *)
+        object; /* this is the correct pointer type of the first argument */
+
+    if ((*GFC->valid == 0x01) && (DLC == 0)) {
+
+#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_CONSUMER
+        /* Optional signal to RTOS, which can resume task, which handles SRDO.
+         */
+        if (GFC->pFunctSignalSafe != NULL) {
+            GFC->pFunctSignalSafe(GFC->functSignalObjectSafe);
+        }
+#endif
+    }
+}
+
+void CO_GFC_initCallbackEnterSafeState(CO_GFC_t *GFC,
+                                       void *object,
+                                       void (*pFunctSignalSafe)(void *object))
+{
+    if (GFC != NULL) {
+        GFC->functSignalObjectSafe = object;
+        GFC->pFunctSignalSafe = pFunctSignalSafe;
+    }
+}
+#endif
+
+CO_ReturnError_t CO_GFC_init(CO_GFC_t *GFC,
+                             uint8_t *valid,
+                             CO_CANmodule_t *GFC_CANdevRx,
+                             uint16_t GFC_rxIdx,
+                             uint16_t CANidRxGFC,
+                             CO_CANmodule_t *GFC_CANdevTx,
+                             uint16_t GFC_txIdx,
+                             uint16_t CANidTxGFC)
+{
+    if (GFC == NULL || valid == NULL || GFC_CANdevRx == NULL ||
+        GFC_CANdevTx == NULL) {
+        return CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+    GFC->valid = valid;
+#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_PRODUCER
+    GFC->CANdevTx = GFC_CANdevTx;
+#endif
+#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_CONSUMER
+    GFC->functSignalObjectSafe = NULL;
+    GFC->pFunctSignalSafe = NULL;
+#endif
+
+#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_PRODUCER
+    GFC->CANtxBuff = CO_CANtxBufferInit(
+        GFC->CANdevTx, /* CAN device */
+        GFC_txIdx,     /* index of specific buffer inside CAN module */
+        CANidTxGFC,    /* CAN identifier */
+        0,             /* rtr */
+        0,             /* number of data bytes */
+        0);            /* synchronous message flag bit */
+
+    if (GFC->CANtxBuff == NULL) {
+        return CO_ERROR_TX_UNCONFIGURED;
+    }
+#else
+    (void)GFC_txIdx;    /* unused */
+    (void)CANidTxGFC;   /* unused */
+#endif
+#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_CONSUMER
+    const CO_ReturnError_t r = CO_CANrxBufferInit(
+        GFC_CANdevRx,    /* CAN device */
+        GFC_rxIdx,       /* rx buffer index */
+        CANidRxGFC,      /* CAN identifier */
+        0x7FF,           /* mask */
+        0,               /* rtr */
+        (void *)GFC,     /* object passed to receive function */
+        CO_GFC_receive); /* this function will process received message */
+    if (r != CO_ERROR_NO) {
+        return r;
+    }
+#else
+    (void)GFC_rxIdx;    /* unused */
+    (void)CANidRxGFC;   /* unused */
+#endif
+
+    return CO_ERROR_NO;
+}
+
+#if (CO_CONFIG_GFC) & CO_CONFIG_GFC_PRODUCER
+
+CO_ReturnError_t CO_GFCsend(CO_GFC_t *GFC)
+{
+    if (*GFC->valid == 0x01)
+        return CO_CANsend(GFC->CANdevTx, GFC->CANtxBuff);
+    return CO_ERROR_NO;
+}
+#endif
+
+#endif /* (CO_CONFIG_GFC) & CO_CONFIG_GFC_ENABLE */

controller_yy_app/middleware/CANopenNode/304/CO_GFC.h

@@ -0,0 +1,138 @@
+/**
+ * CANopen Global fail-safe command protocol.
+ *
+ * @file        CO_GFC.h
+ * @ingroup     CO_GFC
+ * @author      Robert Grüning
+ * @copyright   2020 - 2020 Robert Grüning
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CO_GFC_H
+#define CO_GFC_H
+
+#include "301/CO_driver.h"
+
+/* default configuration, see CO_config.h */
+#ifndef CO_CONFIG_GFC
+#define CO_CONFIG_GFC (0)
+#endif
+
+#if ((CO_CONFIG_GFC) & CO_CONFIG_GFC_ENABLE) || defined CO_DOXYGEN
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_GFC GFC
+ * @ingroup CO_CANopen_304
+ * @{
+ *
+ * Global fail-safe command protocol.
+ *
+ * Very simple consumer/producer protocol.
+ * A net can have multiple GFC producer and multiple GFC consumer.
+ * On a safety-relevant the producer can send a GFC message (ID 0, DLC 0).
+ * The consumer can use this message to start the transition to a safe state.
+ * The GFC is optional for the security protocol and is not monitored (timed).
+ */
+
+
+/**
+ * GFC object.
+ */
+typedef struct {
+    uint8_t *valid; /**< From CO_GFC_init() */
+#if ((CO_CONFIG_GFC)&CO_CONFIG_GFC_PRODUCER) || defined CO_DOXYGEN
+    CO_CANmodule_t *CANdevTx; /**< From CO_GFC_init() */
+    CO_CANtx_t *CANtxBuff;    /**< CAN transmit buffer inside CANdevTx */
+#endif
+#if ((CO_CONFIG_GFC)&CO_CONFIG_GFC_CONSUMER) || defined CO_DOXYGEN
+    /** From CO_GFC_initCallbackEnterSafeState() or NULL */
+    void (*pFunctSignalSafe)(void *object);
+    /** From CO_GFC_initCallbackEnterSafeState() or NULL */
+    void *functSignalObjectSafe;
+#endif
+} CO_GFC_t;
+
+/**
+ * Initialize GFC object.
+ *
+ * Function must be called in the communication reset section.
+ *
+ * @param GFC This object will be initialized.
+ * @param valid pointer to the valid flag in OD (0x1300)
+ * @param GFC_CANdevRx  CAN device used for SRDO reception.
+ * @param GFC_rxIdx Index of receive buffer in the above CAN device.
+ * @param CANidRxGFC GFC CAN ID for reception
+ * @param GFC_CANdevTx AN device used for SRDO transmission.
+ * @param GFC_txIdx Index of transmit buffer in the above CAN device.
+ * @param CANidTxGFC GFC CAN ID for transmission
+ *
+ * @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ */
+CO_ReturnError_t CO_GFC_init(CO_GFC_t *GFC,
+                             uint8_t *valid,
+                             CO_CANmodule_t *GFC_CANdevRx,
+                             uint16_t GFC_rxIdx,
+                             uint16_t CANidRxGFC,
+                             CO_CANmodule_t *GFC_CANdevTx,
+                             uint16_t GFC_txIdx,
+                             uint16_t CANidTxGFC);
+
+#if ((CO_CONFIG_GFC)&CO_CONFIG_GFC_CONSUMER) || defined CO_DOXYGEN
+/**
+ * Initialize GFC callback function.
+ *
+ * Function initializes optional callback function, that is called when GFC is
+ * received. Callback is called from receive function (interrupt).
+ *
+ * @param GFC This object.
+ * @param object Pointer to object, which will be passed to pFunctSignalSafe().
+ * Can be NULL
+ * @param pFunctSignalSafe Pointer to the callback function. Not called if NULL.
+ */
+void CO_GFC_initCallbackEnterSafeState(CO_GFC_t *GFC,
+                                       void *object,
+                                       void (*pFunctSignalSafe)(void *object));
+#endif
+
+#if ((CO_CONFIG_GFC)&CO_CONFIG_GFC_PRODUCER) || defined CO_DOXYGEN
+/**
+ * Send GFC message.
+ *
+ * It should be called by application, for example after a safety-relevant
+ * change.
+ *
+ * @param GFC GFC object.
+ *
+ * @return Same as CO_CANsend().
+ */
+CO_ReturnError_t CO_GFCsend(CO_GFC_t *GFC);
+#endif
+
+/** @} */ /* CO_GFC */
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+#endif /* (CO_CONFIG_GFC) & CO_CONFIG_GFC_ENABLE */
+
+#endif /* CO_GFC_H */

controller_yy_app/middleware/CANopenNode/304/CO_SRDO.c

@@ -0,0 +1,847 @@
+/**
+ * CANopen Safety Related Data Object protocol.
+ *
+ * @file        CO_SRDO.c
+ * @ingroup     CO_SRDO
+ * @author      Robert Grüning
+ * @copyright   2020 - 2020 Robert Grüning
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "304/CO_SRDO.h"
+
+#if (CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE
+
+#include "301/crc16-ccitt.h"
+
+/* verify configuration */
+#if !((CO_CONFIG_CRC16) & CO_CONFIG_CRC16_ENABLE)
+ #error CO_CONFIG_CRC16_ENABLE must be enabled.
+#endif
+
+#define CO_SRDO_INVALID          (0U)
+#define CO_SRDO_TX               (1U)
+#define CO_SRDO_RX               (2U)
+
+#define CO_SRDO_VALID_MAGIC    (0xA5)
+
+
+static void CO_SRDO_receive_normal(void *object, void *msg){
+    CO_SRDO_t *SRDO;
+    uint8_t DLC = CO_CANrxMsg_readDLC(msg);
+    uint8_t *data = CO_CANrxMsg_readData(msg);
+
+    SRDO = (CO_SRDO_t*)object;   /* this is the correct pointer type of the first argument */
+
+    if ( (SRDO->valid == CO_SRDO_RX) &&
+        (DLC >= SRDO->dataLength) && !CO_FLAG_READ(SRDO->CANrxNew[1]))
+    {
+        /* copy data into appropriate buffer and set 'new message' flag */
+        memcpy(SRDO->CANrxData[0], data, sizeof(SRDO->CANrxData[0]));
+        CO_FLAG_SET(SRDO->CANrxNew[0]);
+
+#if (CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE
+        /* Optional signal to RTOS, which can resume task, which handles SRDO. */
+        if (SRDO->pFunctSignalPre != NULL) {
+            SRDO->pFunctSignalPre(SRDO->functSignalObjectPre);
+        }
+#endif
+    }
+}
+
+static void CO_SRDO_receive_inverted(void *object, void *msg){
+    CO_SRDO_t *SRDO;
+    uint8_t DLC = CO_CANrxMsg_readDLC(msg);
+    uint8_t *data = CO_CANrxMsg_readData(msg);
+
+    SRDO = (CO_SRDO_t*)object;   /* this is the correct pointer type of the first argument */
+
+    if ( (SRDO->valid == CO_SRDO_RX) &&
+        (DLC >= SRDO->dataLength) && CO_FLAG_READ(SRDO->CANrxNew[0]))
+    {
+        /* copy data into appropriate buffer and set 'new message' flag */
+        memcpy(SRDO->CANrxData[1], data, sizeof(SRDO->CANrxData[1]));
+        CO_FLAG_SET(SRDO->CANrxNew[1]);
+
+#if (CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE
+        /* Optional signal to RTOS, which can resume task, which handles SRDO. */
+        if (SRDO->pFunctSignalPre != NULL) {
+            SRDO->pFunctSignalPre(SRDO->functSignalObjectPre);
+        }
+#endif
+    }
+}
+
+static void CO_SRDOconfigCom(CO_SRDO_t* SRDO, uint32_t COB_IDnormal, uint32_t COB_IDinverted){
+    uint16_t IDs[2][2] = {0};
+    uint16_t* ID;
+    uint16_t successCount = 0;
+
+    int16_t i;
+
+    uint32_t COB_ID[2];
+    COB_ID[0] = COB_IDnormal;
+    COB_ID[1] = COB_IDinverted;
+
+    SRDO->valid = CO_SRDO_INVALID;
+
+    /* is SRDO used? */
+    if (*SRDO->SRDOGuard->configurationValid == CO_SRDO_VALID_MAGIC && (SRDO->SRDOCommPar->informationDirection == CO_SRDO_TX || SRDO->SRDOCommPar->informationDirection == CO_SRDO_RX) &&
+        SRDO->dataLength){
+        ID = &IDs[SRDO->SRDOCommPar->informationDirection - 1][0];
+        /* is used default COB-ID? */
+        for(i = 0; i < 2; i++){
+            if (!(COB_ID[i] & 0xBFFFF800L)){
+                ID[i] = (uint16_t)COB_ID[i] & 0x7FF;
+
+                if (ID[i] == SRDO->defaultCOB_ID[i] && SRDO->nodeId <= 64){
+                    ID[i] += 2*SRDO->nodeId;
+                }
+                if (0x101 <= ID[i] && ID[i] <= 0x180 && ((ID[i] & 1) != i )){
+                    successCount++;
+                }
+            }
+        }
+    }
+    /* all ids are ok*/
+    if (successCount == 2){
+        SRDO->valid = SRDO->SRDOCommPar->informationDirection;
+
+        if (SRDO->valid == CO_SRDO_TX){
+            SRDO->timer = 500 * SRDO->nodeId; /* 0.5ms * node-ID delay*/
+        }
+        else if (SRDO->valid == CO_SRDO_RX){
+            SRDO->timer = SRDO->SRDOCommPar->safetyCycleTime * 1000U;
+        }
+    }
+    else{
+        memset(IDs, 0, sizeof(IDs));
+        CO_FLAG_CLEAR(SRDO->CANrxNew[0]);
+        CO_FLAG_CLEAR(SRDO->CANrxNew[1]);
+        SRDO->valid = CO_SRDO_INVALID;
+    }
+
+
+    for(i = 0; i < 2; i++){
+        CO_ReturnError_t r;
+        SRDO->CANtxBuff[i] = CO_CANtxBufferInit(
+            SRDO->CANdevTx,            /* CAN device */
+            SRDO->CANdevTxIdx[i],      /* index of specific buffer inside CAN module */
+            IDs[0][i],                   /* CAN identifier */
+            0,                         /* rtr */
+            SRDO->dataLength,          /* number of data bytes */
+            0);                 /* synchronous message flag bit */
+
+        if (SRDO->CANtxBuff[i] == 0){
+            SRDO->valid = CO_SRDO_INVALID;
+        }
+
+        r = CO_CANrxBufferInit(
+                SRDO->CANdevRx,         /* CAN device */
+                SRDO->CANdevRxIdx[i],   /* rx buffer index */
+                IDs[1][i],                /* CAN identifier */
+                0x7FF,                  /* mask */
+                0,                      /* rtr */
+                (void*)SRDO,            /* object passed to receive function */
+                i ? CO_SRDO_receive_inverted : CO_SRDO_receive_normal);        /* this function will process received message */
+        if (r != CO_ERROR_NO){
+            SRDO->valid = CO_SRDO_INVALID;
+            CO_FLAG_CLEAR(SRDO->CANrxNew[i]);
+        }
+    }
+}
+
+static uint32_t CO_SRDOfindMap(
+        CO_SDO_t               *SDO,
+        uint32_t                map,
+        uint8_t                 R_T,
+        uint8_t               **ppData,
+        uint8_t                *pLength,
+        uint8_t                *pSendIfCOSFlags,
+        uint8_t                *pIsMultibyteVar)
+{
+    uint16_t entryNo;
+    uint16_t index;
+    uint8_t subIndex;
+    uint8_t dataLen;
+    uint8_t objectLen;
+    uint8_t attr;
+
+    index = (uint16_t)(map>>16);
+    subIndex = (uint8_t)(map>>8);
+    dataLen = (uint8_t) map;   /* data length in bits */
+
+    /* data length must be byte aligned */
+    if (dataLen&0x07) return CO_SDO_AB_NO_MAP;   /* Object cannot be mapped to the PDO. */
+
+    dataLen >>= 3;    /* new data length is in bytes */
+    *pLength += dataLen;
+
+    /* total PDO length can not be more than 8 bytes */
+    if (*pLength > 8) return CO_SDO_AB_MAP_LEN;  /* The number and length of the objects to be mapped would exceed PDO length. */
+
+    /* is there a reference to dummy entries */
+    if (index <=7 && subIndex == 0){
+        static uint32_t dummyTX = 0;
+        static uint32_t dummyRX;
+        uint8_t dummySize = 4;
+
+        if (index<2) dummySize = 0;
+        else if (index==2 || index==5) dummySize = 1;
+        else if (index==3 || index==6) dummySize = 2;
+
+        /* is size of variable big enough for map */
+        if (dummySize < dataLen) return CO_SDO_AB_NO_MAP;   /* Object cannot be mapped to the PDO. */
+
+        /* Data and ODE pointer */
+        if (R_T == 0) *ppData = (uint8_t*) &dummyRX;
+        else         *ppData = (uint8_t*) &dummyTX;
+
+        return 0;
+    }
+
+    /* find object in Object Dictionary */
+    entryNo = CO_OD_find(SDO, index);
+
+    /* Does object exist in OD? */
+    if (entryNo == 0xFFFF || subIndex > SDO->OD[entryNo].maxSubIndex)
+        return CO_SDO_AB_NOT_EXIST;   /* Object does not exist in the object dictionary. */
+
+    attr = CO_OD_getAttribute(SDO, entryNo, subIndex);
+    /* Is object Mappable for RPDO? */
+    if (R_T==0 && !((attr&CO_ODA_RPDO_MAPABLE) && (attr&CO_ODA_WRITEABLE))) return CO_SDO_AB_NO_MAP;   /* Object cannot be mapped to the PDO. */
+    /* Is object Mappable for TPDO? */
+    if (R_T!=0 && !((attr&CO_ODA_TPDO_MAPABLE) && (attr&CO_ODA_READABLE))) return CO_SDO_AB_NO_MAP;   /* Object cannot be mapped to the PDO. */
+
+    /* is size of variable big enough for map */
+    objectLen = CO_OD_getLength(SDO, entryNo, subIndex);
+    if (objectLen < dataLen) return CO_SDO_AB_NO_MAP;   /* Object cannot be mapped to the PDO. */
+
+    /* mark multibyte variable */
+    *pIsMultibyteVar = (attr&CO_ODA_MB_VALUE) ? 1 : 0;
+
+    /* pointer to data */
+    *ppData = (uint8_t*) CO_OD_getDataPointer(SDO, entryNo, subIndex);
+#ifdef CO_BIG_ENDIAN
+    /* skip unused MSB bytes */
+    if (*pIsMultibyteVar){
+        *ppData += objectLen - dataLen;
+    }
+#endif
+
+    /* setup change of state flags */
+    if (attr&CO_ODA_TPDO_DETECT_COS){
+        int16_t i;
+        for(i=*pLength-dataLen; i<*pLength; i++){
+            *pSendIfCOSFlags |= 1<<i;
+        }
+    }
+
+    return 0;
+}
+
+static uint32_t CO_SRDOconfigMap(CO_SRDO_t* SRDO, uint8_t noOfMappedObjects){
+    int16_t i;
+    uint8_t lengths[2] = {0};
+    uint32_t ret = 0;
+    const uint32_t* pMap = &SRDO->SRDOMapPar->mappedObjects[0];
+
+
+    for(i=noOfMappedObjects; i>0; i--){
+        int16_t j;
+        uint8_t* pData;
+        uint8_t dummy = 0;
+        uint8_t* length = &lengths[i%2];
+        uint8_t** mapPointer = SRDO->mapPointer[i%2];
+        uint8_t prevLength = *length;
+        uint8_t MBvar;
+        uint32_t map = *(pMap++);
+
+        /* function do much checking of errors in map */
+        ret = CO_SRDOfindMap(
+                SRDO->SDO,
+                map,
+                SRDO->SRDOCommPar->informationDirection == CO_SRDO_TX,
+                &pData,
+                length,
+                &dummy,
+                &MBvar);
+        if (ret){
+            *length = 0;
+            CO_errorReport(SRDO->em, CO_EM_PDO_WRONG_MAPPING, CO_EMC_PROTOCOL_ERROR, map);
+            break;
+        }
+
+        /* write SRDO data pointers */
+#ifdef CO_BIG_ENDIAN
+        if (MBvar){
+            for(j=*length-1; j>=prevLength; j--)
+                mapPointer[j] = pData++;
+        }
+        else{
+            for(j=prevLength; j<*length; j++)
+                mapPointer[j] = pData++;
+        }
+#else
+        for(j=prevLength; j<*length; j++){
+            mapPointer[j] = pData++;
+        }
+#endif
+
+    }
+    if (lengths[0] == lengths[1])
+        SRDO->dataLength = lengths[0];
+    else{
+        SRDO->dataLength = 0;
+        CO_errorReport(SRDO->em, CO_EM_PDO_WRONG_MAPPING, CO_EMC_PROTOCOL_ERROR, 0);
+        ret = CO_SDO_AB_MAP_LEN;
+    }
+
+    return ret;
+}
+
+static uint16_t CO_SRDOcalcCrc(const CO_SRDO_t *SRDO){
+    uint16_t i;
+    uint16_t result = 0x0000;
+    uint8_t buffer[4];
+    uint32_t cob;
+
+    const CO_SRDOCommPar_t *com = SRDO->SRDOCommPar;
+    const CO_SRDOMapPar_t *map = SRDO->SRDOMapPar;
+
+    result = crc16_ccitt(&com->informationDirection, 1, result);
+    CO_memcpySwap2(&buffer[0], &com->safetyCycleTime);
+    result = crc16_ccitt(&buffer[0], 2, result);
+    result = crc16_ccitt(&com->safetyRelatedValidationTime, 1, result);
+
+    /* adjust COB-ID if the default is used
+    Caution: if the node id changes and you are using the default COB-ID you have to recalculate the checksum
+    This behaviour is controversial and could be changed or made optional.
+    */
+    cob = com->COB_ID1_normal;
+    if (((cob)&0x7FF) == SRDO->defaultCOB_ID[0] && SRDO->nodeId <= 64)
+        cob += SRDO->nodeId;
+    CO_memcpySwap4(&buffer[0], &cob);
+    result = crc16_ccitt(&buffer[0], 4, result);
+
+    cob = com->COB_ID2_inverted;
+    if (((cob)&0x7FF) == SRDO->defaultCOB_ID[1] && SRDO->nodeId <= 64)
+        cob += SRDO->nodeId;
+    CO_memcpySwap4(&buffer[0], &cob);
+    result = crc16_ccitt(&buffer[0], 4, result);
+
+    result = crc16_ccitt(&map->numberOfMappedObjects, 1, result);
+    for(i = 0; i < map->numberOfMappedObjects;){
+        uint8_t subindex = i + 1;
+        result = crc16_ccitt(&subindex, 1, result);
+        CO_memcpySwap4(&buffer[0], &map->mappedObjects[i]);
+        result = crc16_ccitt(&buffer[0], 4, result);
+        i = subindex;
+    }
+    return result;
+}
+
+static CO_SDO_abortCode_t CO_ODF_SRDOcom(CO_ODF_arg_t *ODF_arg){
+    CO_SRDO_t *SRDO;
+
+    SRDO = (CO_SRDO_t*) ODF_arg->object;
+
+    /* Reading Object Dictionary variable */
+    if (ODF_arg->reading){
+        if (ODF_arg->subIndex == 5 || ODF_arg->subIndex == 6){
+            uint32_t value = CO_getUint32(ODF_arg->data);
+            uint16_t index = ODF_arg->subIndex - 5;
+
+            /* if default COB ID is used, write default value here */
+            if (((value)&0x7FF) == SRDO->defaultCOB_ID[index] && SRDO->nodeId <= 64)
+                value += SRDO->nodeId;
+
+            /* If SRDO is not valid, set bit 31. but I do not think this applies to SRDO ?! */
+            /* if (!SRDO->valid) value |= 0x80000000L; */
+
+            CO_setUint32(ODF_arg->data, value);
+        }
+        return CO_SDO_AB_NONE;
+    }
+
+    /* Writing Object Dictionary variable */
+    if (*SRDO->SRDOGuard->operatingState == CO_NMT_OPERATIONAL)
+        return CO_SDO_AB_DATA_DEV_STATE;   /* Data cannot be transferred or stored to the application because of the present device state. */
+
+    if (ODF_arg->subIndex == 1){
+        uint8_t value = ODF_arg->data[0];
+        if (value > 2)
+            return CO_SDO_AB_INVALID_VALUE;
+    }
+    else if (ODF_arg->subIndex == 2){
+        uint16_t value = CO_getUint16(ODF_arg->data);
+        if (value == 0)
+            return CO_SDO_AB_INVALID_VALUE;
+    }
+    else if (ODF_arg->subIndex == 3){
+        uint8_t value = ODF_arg->data[0];
+        if (value == 0)
+            return CO_SDO_AB_INVALID_VALUE;
+    }
+    else if (ODF_arg->subIndex == 4){   /* Transmission_type */
+        uint8_t *value = (uint8_t*) ODF_arg->data;
+        if (*value != 254)
+            return CO_SDO_AB_INVALID_VALUE;  /* Invalid value for parameter (download only). */
+    }
+   else if (ODF_arg->subIndex == 5 || ODF_arg->subIndex == 6){   /* COB_ID */
+        uint32_t value = CO_getUint32(ODF_arg->data);
+        uint16_t index = ODF_arg->subIndex - 5;
+
+        /* check value range, the spec does not specify if COB-ID flags are allowed */
+        if (value < 0x101 || value > 0x180 || (value & 1) == index)
+            return CO_SDO_AB_INVALID_VALUE;  /* Invalid value for parameter (download only). */
+
+        /* if default COB-ID is being written, write defaultCOB_ID without nodeId */
+        if (SRDO->nodeId <= 64 && value == (SRDO->defaultCOB_ID[index] + SRDO->nodeId)){
+            value = SRDO->defaultCOB_ID[index];
+            CO_setUint32(ODF_arg->data, value);
+        }
+    }
+
+    *SRDO->SRDOGuard->configurationValid = CO_SRDO_INVALID;
+
+    return CO_SDO_AB_NONE;
+}
+
+static CO_SDO_abortCode_t CO_ODF_SRDOmap(CO_ODF_arg_t *ODF_arg){
+    CO_SRDO_t *SRDO;
+
+    SRDO = (CO_SRDO_t*) ODF_arg->object;
+    if (ODF_arg->reading)
+        return CO_SDO_AB_NONE;
+
+    /* Writing Object Dictionary variable */
+
+    if (*SRDO->SRDOGuard->operatingState == CO_NMT_OPERATIONAL)
+        return CO_SDO_AB_DATA_DEV_STATE;   /* Data cannot be transferred or stored to the application because of the present device state. */
+    if (SRDO->SRDOCommPar->informationDirection) /* SRDO must be deleted */
+        return CO_SDO_AB_UNSUPPORTED_ACCESS;  /* Unsupported access to an object. */
+
+    /* numberOfMappedObjects */
+    if (ODF_arg->subIndex == 0){
+        uint8_t *value = (uint8_t*) ODF_arg->data;
+
+        if (*value > 16 || *value & 1) /*only odd numbers are allowed*/
+            return CO_SDO_AB_MAP_LEN;  /* Number and length of object to be mapped exceeds SRDO length. */
+    }
+    else{
+        if (SRDO->SRDOMapPar->numberOfMappedObjects != 0)
+            return CO_SDO_AB_UNSUPPORTED_ACCESS;
+    }
+    *SRDO->SRDOGuard->configurationValid = CO_SRDO_INVALID;
+    return CO_SDO_AB_NONE;
+}
+
+static CO_SDO_abortCode_t CO_ODF_SRDOcrc(CO_ODF_arg_t *ODF_arg){
+    CO_SRDOGuard_t *SRDOGuard;
+
+    SRDOGuard = (CO_SRDOGuard_t*) ODF_arg->object;
+
+    if (!ODF_arg->reading){
+        if (*SRDOGuard->operatingState == CO_NMT_OPERATIONAL)
+            return CO_SDO_AB_DATA_DEV_STATE;   /* Data cannot be transferred or stored to the application because of the present device state. */
+        *SRDOGuard->configurationValid = CO_SRDO_INVALID;
+    }
+    return CO_SDO_AB_NONE;
+}
+
+static CO_SDO_abortCode_t CO_ODF_SRDOvalid(CO_ODF_arg_t *ODF_arg){
+    CO_SRDOGuard_t *SRDOGuard;
+
+    SRDOGuard = (CO_SRDOGuard_t*) ODF_arg->object;
+
+    if (!ODF_arg->reading){
+        if (*SRDOGuard->operatingState == CO_NMT_OPERATIONAL)
+            return CO_SDO_AB_DATA_DEV_STATE;   /* Data cannot be transferred or stored to the application because of the present device state. */
+        SRDOGuard->checkCRC = ODF_arg->data[0] == CO_SRDO_VALID_MAGIC;
+    }
+    return CO_SDO_AB_NONE;
+}
+
+CO_ReturnError_t CO_SRDOGuard_init(
+        CO_SRDOGuard_t         *SRDOGuard,
+        CO_SDO_t               *SDO,
+        CO_NMT_internalState_t *operatingState,
+        uint8_t                *configurationValid,
+        uint16_t                idx_SRDOvalid,
+        uint16_t                idx_SRDOcrc)
+{
+    /* verify arguments */
+    if (SRDOGuard==NULL || SDO==NULL || operatingState==NULL || configurationValid==NULL){
+        return CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    SRDOGuard->operatingState = operatingState;
+    SRDOGuard->operatingStatePrev = CO_NMT_INITIALIZING;
+    SRDOGuard->configurationValid = configurationValid;
+    SRDOGuard->checkCRC = *configurationValid == CO_SRDO_VALID_MAGIC;
+
+
+    /* Configure Object dictionary entry at index 0x13FE and 0x13FF */
+    CO_OD_configure(SDO, idx_SRDOvalid, CO_ODF_SRDOvalid, (void*)SRDOGuard, 0, 0);
+    CO_OD_configure(SDO, idx_SRDOcrc, CO_ODF_SRDOcrc, (void*)SRDOGuard, 0, 0);
+
+    return CO_ERROR_NO;
+}
+
+uint8_t CO_SRDOGuard_process(
+        CO_SRDOGuard_t         *SRDOGuard)
+{
+    uint8_t result = 0;
+    CO_NMT_internalState_t operatingState = *SRDOGuard->operatingState;
+    if (operatingState != SRDOGuard->operatingStatePrev){
+        SRDOGuard->operatingStatePrev = operatingState;
+        if (operatingState == CO_NMT_OPERATIONAL)
+            result |= 1 << 0;
+    }
+
+    if (SRDOGuard->checkCRC){
+        result |= 1 << 1;
+        SRDOGuard->checkCRC = 0;
+    }
+    return result;
+}
+
+#if (CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE
+/******************************************************************************/
+void CO_SRDO_initCallbackPre(
+        CO_SRDO_t              *SRDO,
+        void                   *object,
+        void                  (*pFunctSignalPre)(void *object))
+{
+    if (SRDO != NULL){
+        SRDO->functSignalObjectPre = object;
+        SRDO->pFunctSignalPre = pFunctSignalPre;
+    }
+}
+#endif
+
+/******************************************************************************/
+void CO_SRDO_initCallbackEnterSafeState(
+        CO_SRDO_t              *SRDO,
+        void                   *object,
+        void                  (*pFunctSignalSafe)(void *object))
+{
+    if (SRDO != NULL){
+        SRDO->functSignalObjectSafe = object;
+        SRDO->pFunctSignalSafe = pFunctSignalSafe;
+    }
+}
+
+CO_ReturnError_t CO_SRDO_init(
+        CO_SRDO_t              *SRDO,
+        CO_SRDOGuard_t         *SRDOGuard,
+        CO_EM_t                *em,
+        CO_SDO_t               *SDO,
+        uint8_t                 nodeId,
+        uint16_t                defaultCOB_ID,
+        const CO_SRDOCommPar_t *SRDOCommPar,
+        const CO_SRDOMapPar_t  *SRDOMapPar,
+        const uint16_t         *checksum,
+        uint16_t                idx_SRDOCommPar,
+        uint16_t                idx_SRDOMapPar,
+        CO_CANmodule_t         *CANdevRx,
+        uint16_t                CANdevRxIdxNormal,
+        uint16_t                CANdevRxIdxInverted,
+        CO_CANmodule_t         *CANdevTx,
+        uint16_t                CANdevTxIdxNormal,
+        uint16_t                CANdevTxIdxInverted)
+{
+        /* verify arguments */
+    if (SRDO==NULL || SRDOGuard==NULL || em==NULL || SDO==NULL || checksum==NULL ||
+        SRDOCommPar==NULL || SRDOMapPar==NULL || CANdevRx==NULL || CANdevTx==NULL){
+        return CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    SRDO->SRDOGuard = SRDOGuard;
+    SRDO->em = em;
+    SRDO->SDO = SDO;
+    SRDO->SRDOCommPar = SRDOCommPar;
+    SRDO->SRDOMapPar = SRDOMapPar;
+    SRDO->checksum = checksum;
+    SRDO->CANdevRx = CANdevRx;
+    SRDO->CANdevRxIdx[0] = CANdevRxIdxNormal;
+    SRDO->CANdevRxIdx[1] = CANdevRxIdxInverted;
+    SRDO->CANdevTx = CANdevTx;
+    SRDO->CANdevTxIdx[0] = CANdevTxIdxNormal;
+    SRDO->CANdevTxIdx[1] = CANdevTxIdxInverted;
+    SRDO->nodeId = nodeId;
+    SRDO->defaultCOB_ID[0] = defaultCOB_ID;
+    SRDO->defaultCOB_ID[1] = defaultCOB_ID + 1;
+    SRDO->valid = CO_SRDO_INVALID;
+    SRDO->toogle = 0;
+    SRDO->timer = 0;
+    SRDO->pFunctSignalSafe = NULL;
+    SRDO->functSignalObjectSafe = NULL;
+#if (CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE
+    SRDO->pFunctSignalPre = NULL;
+    SRDO->functSignalObjectPre = NULL;
+#endif
+
+    /* Configure Object dictionary entry at index 0x1301+ and 0x1381+ */
+    CO_OD_configure(SDO, idx_SRDOCommPar, CO_ODF_SRDOcom, (void*)SRDO, 0, 0);
+    CO_OD_configure(SDO, idx_SRDOMapPar, CO_ODF_SRDOmap, (void*)SRDO, 0, 0);
+
+    return CO_ERROR_NO;
+}
+
+CO_ReturnError_t CO_SRDO_requestSend(
+        CO_SRDO_t              *SRDO)
+{
+    if (*SRDO->SRDOGuard->operatingState != CO_NMT_OPERATIONAL)
+        return CO_ERROR_WRONG_NMT_STATE;
+
+    if (SRDO->valid != CO_SRDO_TX)
+        return CO_ERROR_TX_UNCONFIGURED;
+
+    if (SRDO->toogle != 0)
+        return CO_ERROR_TX_BUSY;
+
+    SRDO->timer = 0;
+    return CO_ERROR_NO;
+}
+
+void CO_SRDO_process(
+        CO_SRDO_t              *SRDO,
+        uint8_t                 commands,
+        uint32_t                timeDifference_us,
+        uint32_t               *timerNext_us)
+{
+    (void)timerNext_us; /* may be unused */
+
+    if (commands & (1<<1)){
+        uint16_t crcValue = CO_SRDOcalcCrc(SRDO);
+        if (*SRDO->checksum != crcValue)
+            *SRDO->SRDOGuard->configurationValid = 0;
+    }
+
+    if ((commands & (1<<0)) && *SRDO->SRDOGuard->configurationValid == CO_SRDO_VALID_MAGIC){
+        if (CO_SRDOconfigMap(SRDO, SRDO->SRDOMapPar->numberOfMappedObjects) == 0){
+            CO_SRDOconfigCom(SRDO, SRDO->SRDOCommPar->COB_ID1_normal, SRDO->SRDOCommPar->COB_ID2_inverted);
+        }
+        else{
+            SRDO->valid = CO_SRDO_INVALID;
+        }
+    }
+
+    if (SRDO->valid && *SRDO->SRDOGuard->operatingState == CO_NMT_OPERATIONAL){
+        SRDO->timer = (SRDO->timer > timeDifference_us) ? (SRDO->timer - timeDifference_us) : 0;
+        if (SRDO->valid == CO_SRDO_TX){
+            if (SRDO->timer == 0){
+                if (SRDO->toogle){
+                    CO_CANsend(SRDO->CANdevTx, SRDO->CANtxBuff[1]);
+                    SRDO->timer = SRDO->SRDOCommPar->safetyCycleTime * 1000U - CO_CONFIG_SRDO_MINIMUM_DELAY;
+                }
+                else{
+
+                    int16_t i;
+                    uint8_t* pSRDOdataByte_normal;
+                    uint8_t* pSRDOdataByte_inverted;
+
+                    uint8_t** ppODdataByte_normal;
+                    uint8_t** ppODdataByte_inverted;
+
+                    bool_t data_ok = true;
+#if (CO_CONFIG_SRDO) & CO_CONFIG_TSRDO_CALLS_EXTENSION
+                    if (SRDO->SDO->ODExtensions){
+                        /* for each mapped OD, check mapping to see if an OD extension is available, and call it if it is */
+                        const uint32_t* pMap = &SRDO->SRDOMapPar->mappedObjects[0];
+                        CO_SDO_t *pSDO = SRDO->SDO;
+
+                        for(i=SRDO->SRDOMapPar->numberOfMappedObjects; i>0; i--){
+                            uint32_t map = *(pMap++);
+                            uint16_t index = (uint16_t)(map>>16);
+                            uint8_t subIndex = (uint8_t)(map>>8);
+                            uint16_t entryNo = CO_OD_find(pSDO, index);
+                            if ( entryNo != 0xFFFF )
+                            {
+                                CO_OD_extension_t *ext = &pSDO->ODExtensions[entryNo];
+                                if ( ext->pODFunc != NULL)
+                                {
+                                    CO_ODF_arg_t ODF_arg;
+                                    memset((void*)&ODF_arg, 0, sizeof(CO_ODF_arg_t));
+                                    ODF_arg.reading = true;
+                                    ODF_arg.index = index;
+                                    ODF_arg.subIndex = subIndex;
+                                    ODF_arg.object = ext->object;
+                                    ODF_arg.attribute = CO_OD_getAttribute(pSDO, entryNo, subIndex);
+                                    ODF_arg.pFlags = CO_OD_getFlagsPointer(pSDO, entryNo, subIndex);
+                                    ODF_arg.data = CO_OD_getDataPointer(pSDO, entryNo, subIndex); /* https://github.com/CANopenNode/CANopenNode/issues/100 */
+                                    ODF_arg.dataLength = CO_OD_getLength(pSDO, entryNo, subIndex);
+                                    ext->pODFunc(&ODF_arg);
+                                }
+                            }
+                        }
+                    }
+#endif
+                    pSRDOdataByte_normal = &SRDO->CANtxBuff[0]->data[0];
+                    ppODdataByte_normal = &SRDO->mapPointer[0][0];
+
+                    pSRDOdataByte_inverted = &SRDO->CANtxBuff[1]->data[0];
+                    ppODdataByte_inverted = &SRDO->mapPointer[1][0];
+
+#if (CO_CONFIG_SRDO) & CO_CONFIG_SRDO_CHECK_TX
+                    /* check data before sending (optional) */
+                    for(i = 0; i<SRDO->dataLength; i++){
+                        uint8_t invert = ~*(ppODdataByte_inverted[i]);
+                        if (*(ppODdataByte_normal[i]) != invert)
+                        {
+                            data_ok = false;
+                            break;
+                        }
+                    }
+#endif
+                    if (data_ok){
+                        /* Copy data from Object dictionary. */
+                        for(i = 0; i<SRDO->dataLength; i++){
+                            pSRDOdataByte_normal[i] = *(ppODdataByte_normal[i]);
+                            pSRDOdataByte_inverted[i] = *(ppODdataByte_inverted[i]);
+                        }
+
+                        CO_CANsend(SRDO->CANdevTx, SRDO->CANtxBuff[0]);
+
+                        SRDO->timer = CO_CONFIG_SRDO_MINIMUM_DELAY;
+                    }
+                    else{
+                        SRDO->toogle = 1;
+                        /* save state */
+                        if (SRDO->pFunctSignalSafe != NULL){
+                            SRDO->pFunctSignalSafe(SRDO->functSignalObjectSafe);
+                        }
+                    }
+                }
+                SRDO->toogle = !SRDO->toogle;
+            }
+#if (CO_CONFIG_SRDO) & CO_CONFIG_FLAG_TIMERNEXT
+            if (timerNext_us != NULL){
+                if (*timerNext_us > SRDO->timer){
+                    *timerNext_us = SRDO->timer; /* Schedule for next message timer */
+                }
+            }
+#endif
+        }
+        else if (SRDO->valid == CO_SRDO_RX){
+            if (CO_FLAG_READ(SRDO->CANrxNew[SRDO->toogle])){
+
+                if (SRDO->toogle){
+                    int16_t i;
+                    uint8_t* pSRDOdataByte_normal;
+                    uint8_t* pSRDOdataByte_inverted;
+
+                    uint8_t** ppODdataByte_normal;
+                    uint8_t** ppODdataByte_inverted;
+                    bool_t data_ok = true;
+
+                    pSRDOdataByte_normal = &SRDO->CANrxData[0][0];
+                    pSRDOdataByte_inverted = &SRDO->CANrxData[1][0];
+                    for(i = 0; i<SRDO->dataLength; i++){
+                        uint8_t invert = ~pSRDOdataByte_inverted[i];
+                        if (pSRDOdataByte_normal[i] != invert){
+                            data_ok = false;
+                            break;
+                        }
+                    }
+                    if (data_ok){
+                        ppODdataByte_normal = &SRDO->mapPointer[0][0];
+                        ppODdataByte_inverted = &SRDO->mapPointer[1][0];
+
+                        /* Copy data to Object dictionary. If between the copy operation CANrxNew
+                        * is set to true by receive thread, then copy the latest data again. */
+
+                        for(i = 0; i<SRDO->dataLength; i++){
+                            *(ppODdataByte_normal[i]) = pSRDOdataByte_normal[i];
+                            *(ppODdataByte_inverted[i]) = pSRDOdataByte_inverted[i];
+                        }
+                        CO_FLAG_CLEAR(SRDO->CANrxNew[0]);
+                        CO_FLAG_CLEAR(SRDO->CANrxNew[1]);
+#if (CO_CONFIG_SRDO) & CO_CONFIG_RSRDO_CALLS_EXTENSION
+                        if (SRDO->SDO->ODExtensions){
+                            int16_t i;
+                            /* for each mapped OD, check mapping to see if an OD extension is available, and call it if it is */
+                            const uint32_t* pMap = &SRDO->SRDOMapPar->mappedObjects[0];
+                            CO_SDO_t *pSDO = SRDO->SDO;
+
+                            for(i=SRDO->SRDOMapPar->numberOfMappedObjects; i>0; i--){
+                                uint32_t map = *(pMap++);
+                                uint16_t index = (uint16_t)(map>>16);
+                                uint8_t subIndex = (uint8_t)(map>>8);
+                                uint16_t entryNo = CO_OD_find(pSDO, index);
+                                if ( entryNo != 0xFFFF )
+                                {
+                                    CO_OD_extension_t *ext = &pSDO->ODExtensions[entryNo];
+                                    if ( ext->pODFunc != NULL)
+                                    {
+                                        CO_ODF_arg_t ODF_arg;
+                                        memset((void*)&ODF_arg, 0, sizeof(CO_ODF_arg_t));
+                                        ODF_arg.reading = false;
+                                        ODF_arg.index = index;
+                                        ODF_arg.subIndex = subIndex;
+                                        ODF_arg.object = ext->object;
+                                        ODF_arg.attribute = CO_OD_getAttribute(pSDO, entryNo, subIndex);
+                                        ODF_arg.pFlags = CO_OD_getFlagsPointer(pSDO, entryNo, subIndex);
+                                        ODF_arg.data = CO_OD_getDataPointer(pSDO, entryNo, subIndex); /* https://github.com/CANopenNode/CANopenNode/issues/100 */
+                                        ODF_arg.dataLength = CO_OD_getLength(pSDO, entryNo, subIndex);
+                                        ext->pODFunc(&ODF_arg);
+                                    }
+                                }
+                            }
+                        }
+#endif
+                    }
+                    else{
+                        CO_FLAG_CLEAR(SRDO->CANrxNew[0]);
+                        CO_FLAG_CLEAR(SRDO->CANrxNew[1]);
+                        /* save state */
+                        if (SRDO->pFunctSignalSafe != NULL){
+                            SRDO->pFunctSignalSafe(SRDO->functSignalObjectSafe);
+                        }
+                    }
+
+                    SRDO->timer = SRDO->SRDOCommPar->safetyCycleTime * 1000U;
+                }
+                else{
+                    SRDO->timer = SRDO->SRDOCommPar->safetyRelatedValidationTime * 1000U;
+                }
+                SRDO->toogle = !SRDO->toogle;
+            }
+
+            if (SRDO->timer == 0){
+                SRDO->toogle = 0;
+                SRDO->timer = SRDO->SRDOCommPar->safetyRelatedValidationTime * 1000U;
+                CO_FLAG_CLEAR(SRDO->CANrxNew[0]);
+                CO_FLAG_CLEAR(SRDO->CANrxNew[1]);
+                /* save state */
+                if (SRDO->pFunctSignalSafe != NULL){
+                    SRDO->pFunctSignalSafe(SRDO->functSignalObjectSafe);
+                }
+            }
+        }
+    }
+    else{
+        SRDO->valid = CO_SRDO_INVALID;
+        CO_FLAG_CLEAR(SRDO->CANrxNew[0]);
+        CO_FLAG_CLEAR(SRDO->CANrxNew[1]);
+    }
+}
+
+#endif /* (CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE */

controller_yy_app/middleware/CANopenNode/304/CO_SRDO.h

@@ -0,0 +1,314 @@
+/**
+ * CANopen Safety Related Data Object protocol.
+ *
+ * @file        CO_SRDO.h
+ * @ingroup     CO_SRDO
+ * @author      Robert Grüning
+ * @copyright   2020 - 2020 Robert Grüning
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CO_SRDO_H
+#define CO_SRDO_H
+
+#include "301/CO_driver.h"
+#include "301/CO_SDOserver.h"
+#include "301/CO_Emergency.h"
+#include "301/CO_NMT_Heartbeat.h"
+
+/* default configuration, see CO_config.h */
+#ifndef CO_CONFIG_SRDO
+#define CO_CONFIG_SRDO (0)
+#endif
+#ifndef CO_CONFIG_SRDO_MINIMUM_DELAY
+#define CO_CONFIG_SRDO_MINIMUM_DELAY 0
+#endif
+
+#if ((CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE) || defined CO_DOXYGEN
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_SRDO SRDO
+ * @ingroup CO_CANopen_304
+ * @{
+ *
+ * CANopen Safety Related Data Object protocol.
+ *
+ * The functionality is very similar to that of the PDOs.
+ * The main differences is every message is send and received twice.
+ * The second message must be bitwise inverted. The delay between the two messages and between each message pair is monitored.
+ * The distinction between sending and receiving SRDO is made at runtime (for PDO it is compile time).
+ * If the security protocol is used, at least one SRDO is mandatory.
+ */
+
+
+/**
+ * SRDO communication parameter. The same as record from Object dictionary (index 0x1301-0x1340).
+ */
+typedef struct{
+    uint8_t             maxSubIndex;    /**< Equal to 6 */
+    /** Direction of the SRDO. Values:
+        - 0: SRDO is invalid (deleted)
+        - 1: SRDO is transmiting data
+        - 2: SRDO is receive data */
+    uint8_t             informationDirection;
+    /** Refresh-time / SCT
+        - in tx mode (Refresh-time): transmission interval
+        - in rx mode (SCT): receive timeout between two SRDO */
+    uint16_t            safetyCycleTime;
+    /** SRVT
+        - in tx mode: unsed
+        - in rx mode: receive timeout between first and second SRDO message */
+    uint8_t             safetyRelatedValidationTime;
+    /** Transmission type. Values:
+        - 254:     Manufacturer specific.*/
+    uint8_t             transmissionType;
+    /** Communication object identifier for message received. Meaning of the specific bits:
+        - Bit  0-10: COB-ID for SRDO
+        - Bit 11-31: set to 0 for 11 bit COB-ID. */
+    uint32_t            COB_ID1_normal;
+    /** Communication object identifier for message received. Meaning of the specific bits:
+        - Bit  0-10: COB-ID for SRDO
+        - Bit 11-31: set to 0 for 11 bit COB-ID. */
+    uint32_t            COB_ID2_inverted;
+}CO_SRDOCommPar_t;
+
+
+typedef struct{
+    /** Actual number of mapped objects from 0 to 16. Only even numbers are allowed. To change mapped object,
+    this value must be 0. */
+    uint8_t             numberOfMappedObjects;
+    /** Location and size of the mapped object.
+        Even index is the normal object. Odd index is the inverted object. Bit meanings `0xIIIISSLL`:
+        - Bit  0-7:  Data Length in bits.
+        - Bit 8-15:  Subindex from object distionary.
+        - Bit 16-31: Index from object distionary. */
+    uint32_t            mappedObjects[16];
+}CO_SRDOMapPar_t;
+
+/**
+ * Gurad Object for SRDO
+ * monitors:
+ * - access to CRC objects
+ * - access configuration valid flag
+ * - change in operation state
+ */
+typedef struct{
+    CO_NMT_internalState_t *operatingState;     /**< pointer to current operation state */
+    CO_NMT_internalState_t  operatingStatePrev; /**< last operation state */
+    uint8_t                *configurationValid; /**< pointer to the configuration valid flag in OD */
+    uint8_t                 checkCRC;           /**< specifies whether a CRC check should be performed */
+}CO_SRDOGuard_t;
+
+/**
+ * SRDO object.
+ */
+typedef struct{
+    CO_EM_t                *em;                  /**< From CO_SRDO_init() */
+    CO_SDO_t               *SDO;                 /**< From CO_SRDO_init() */
+    CO_SRDOGuard_t         *SRDOGuard;           /**< From CO_SRDO_init() */
+    /** Pointers to 2*8 data objects, where SRDO will be copied */
+    uint8_t                *mapPointer[2][8];
+    /** Data length of the received SRDO message. Calculated from mapping */
+    uint8_t                 dataLength;
+    uint8_t                 nodeId;              /**< From CO_SRDO_init() */
+    uint16_t                defaultCOB_ID[2];    /**< From CO_SRDO_init() */
+    /** 0 - invalid, 1 - tx, 2 - rx */
+    uint8_t                 valid;
+    const CO_SRDOCommPar_t *SRDOCommPar;         /**< From CO_SRDO_init() */
+    const CO_SRDOMapPar_t  *SRDOMapPar;          /**< From CO_SRDO_init() */
+    const uint16_t         *checksum;            /**< From CO_SRDO_init() */
+    CO_CANmodule_t         *CANdevRx;            /**< From CO_SRDO_init() */
+    CO_CANmodule_t         *CANdevTx;            /**< From CO_SRDO_init() */
+    CO_CANtx_t             *CANtxBuff[2];        /**< CAN transmit buffer inside CANdevTx */
+    uint16_t                CANdevRxIdx[2];      /**< From CO_SRDO_init() */
+    uint16_t                CANdevTxIdx[2];      /**< From CO_SRDO_init() */
+    uint8_t                 toogle;              /**< defines the current state */
+    uint32_t                timer;               /**< transmit timer and receive timeout */
+    /** Variable indicates, if new SRDO message received from CAN bus. */
+    volatile void          *CANrxNew[2];
+    /** 2*8 data bytes of the received message. */
+    uint8_t                 CANrxData[2][8];
+    /** From CO_SRDO_initCallbackEnterSafeState() or NULL */
+    void                  (*pFunctSignalSafe)(void *object);
+    /** From CO_SRDO_initCallbackEnterSafeState() or NULL */
+    void                   *functSignalObjectSafe;
+#if ((CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+    /** From CO_SRDO_initCallbackPre() or NULL */
+    void                  (*pFunctSignalPre)(void *object);
+    /** From CO_SRDO_initCallbackPre() or NULL */
+    void                   *functSignalObjectPre;
+#endif
+}CO_SRDO_t;
+
+/**
+ * Initialize SRDOGuard object.
+ *
+ * Function must be called in the communication reset section.
+ *
+ * @param SRDOGuard This object will be initialized.
+ * @param SDO SDO object.
+ * @param operatingState Pointer to variable indicating CANopen device NMT internal state.
+ * @param configurationValid Pointer to variable with the SR valid flag
+ * @param idx_SRDOvalid Index in Object Dictionary
+ * @param idx_SRDOcrc Index in Object Dictionary
+ *
+ * @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ */
+CO_ReturnError_t CO_SRDOGuard_init(
+        CO_SRDOGuard_t         *SRDOGuard,
+        CO_SDO_t               *SDO,
+        CO_NMT_internalState_t *operatingState,
+        uint8_t                *configurationValid,
+        uint16_t                idx_SRDOvalid,
+        uint16_t                idx_SRDOcrc);
+
+/**
+ * Process operation and valid state changes.
+ *
+ * @param SRDOGuard This object.
+ * @return uint8_t command for CO_SRDO_process().
+ * - bit 0 entered operational
+ * - bit 1 validate checksum
+ */
+uint8_t CO_SRDOGuard_process(
+        CO_SRDOGuard_t         *SRDOGuard);
+
+/**
+ * Initialize SRDO object.
+ *
+ * Function must be called in the communication reset section.
+ *
+ * @param SRDO This object will be initialized.
+ * @param SRDOGuard SRDOGuard object.
+ * @param em Emergency object.
+ * @param SDO SDO object.
+ * @param nodeId CANopen Node ID of this device. If default COB_ID is used, value will be added.
+ * @param defaultCOB_ID Default COB ID for this SRDO (without NodeId).
+ * @param SRDOCommPar Pointer to _SRDO communication parameter_ record from Object
+ * dictionary (index 0x1301+).
+ * @param SRDOMapPar Pointer to _SRDO mapping parameter_ record from Object
+ * dictionary (index 0x1381+).
+ * @param checksum
+ * @param idx_SRDOCommPar Index in Object Dictionary
+ * @param idx_SRDOMapPar Index in Object Dictionary
+ * @param CANdevRx CAN device used for SRDO reception.
+ * @param CANdevRxIdxNormal Index of receive buffer in the above CAN device.
+ * @param CANdevRxIdxInverted Index of receive buffer in the above CAN device.
+ * @param CANdevTx CAN device used for SRDO transmission.
+ * @param CANdevTxIdxNormal Index of transmit buffer in the above CAN device.
+ * @param CANdevTxIdxInverted Index of transmit buffer in the above CAN device.
+ *
+ * @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ */
+CO_ReturnError_t CO_SRDO_init(
+        CO_SRDO_t              *SRDO,
+        CO_SRDOGuard_t         *SRDOGuard,
+        CO_EM_t                *em,
+        CO_SDO_t               *SDO,
+        uint8_t                 nodeId,
+        uint16_t                defaultCOB_ID,
+        const CO_SRDOCommPar_t *SRDOCommPar,
+        const CO_SRDOMapPar_t  *SRDOMapPar,
+        const uint16_t         *checksum,
+        uint16_t                idx_SRDOCommPar,
+        uint16_t                idx_SRDOMapPar,
+        CO_CANmodule_t         *CANdevRx,
+        uint16_t                CANdevRxIdxNormal,
+        uint16_t                CANdevRxIdxInverted,
+        CO_CANmodule_t         *CANdevTx,
+        uint16_t                CANdevTxIdxNormal,
+        uint16_t                CANdevTxIdxInverted);
+
+#if ((CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+/**
+ * Initialize SRDO callback function.
+ *
+ * Function initializes optional callback function, which should immediately
+ * start processing of CO_SRDO_process() function.
+ * Callback is called after SRDO message is received from the CAN bus.
+ *
+ * @param SRDO This object.
+ * @param object Pointer to object, which will be passed to pFunctSignalPre(). Can be NULL
+ * @param pFunctSignalPre Pointer to the callback function. Not called if NULL.
+ */
+void CO_SRDO_initCallbackPre(
+        CO_SRDO_t              *SRDO,
+        void                   *object,
+        void                  (*pFunctSignalPre)(void *object));
+#endif
+
+/**
+ * Initialize SRDO callback function.
+ *
+ * Function initializes optional callback function, that is called when SRDO enters a safe state.
+ * This happens when a timeout is reached or the data is inconsistent. The safe state itself is not further defined.
+ * One measure, for example, would be to go back to the pre-operational state
+ * Callback is called from CO_SRDO_process().
+ *
+ * @param SRDO This object.
+ * @param object Pointer to object, which will be passed to pFunctSignalSafe(). Can be NULL
+ * @param pFunctSignalSafe Pointer to the callback function. Not called if NULL.
+ */
+void CO_SRDO_initCallbackEnterSafeState(
+        CO_SRDO_t              *SRDO,
+        void                   *object,
+        void                  (*pFunctSignalSafe)(void *object));
+
+
+/**
+ * Send SRDO on event
+ *
+ * Sends SRDO before the next refresh timer tiggers. The message itself is send in CO_SRDO_process().
+ * After the transmission the timer is reset to the full refresh time.
+ *
+ * @param SRDO This object.
+ * @return CO_ReturnError_t CO_ERROR_NO if request is granted
+ */
+CO_ReturnError_t CO_SRDO_requestSend(
+        CO_SRDO_t              *SRDO);
+
+/**
+ * Process transmitting/receiving SRDO messages.
+ *
+ *  This function verifies the checksum on demand.
+ *  This function also configures the SRDO on operation state change to operational
+ *
+ * @param SRDO This object.
+ * @param commands result from CO_SRDOGuard_process().
+ * @param timeDifference_us Time difference from previous function call in [microseconds].
+ * @param [out] timerNext_us info to OS.
+ */
+void CO_SRDO_process(
+        CO_SRDO_t              *SRDO,
+        uint8_t                 commands,
+        uint32_t                timeDifference_us,
+        uint32_t               *timerNext_us);
+
+/** @} */ /* CO_SRDO */
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+#endif /* (CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE */
+
+#endif /* CO_SRDO_H */

controller_yy_app/middleware/CANopenNode/305/CMakeLists.txt

@@ -0,0 +1,6 @@
+# Copyright (c) 2021-2024 HPMicro
+# SPDX-License-Identifier: BSD-3-Clause
+
+sdk_inc(.)
+sdk_src(CO_LSSmaster.c)
+sdk_src(CO_LSSslave.c)

controller_yy_app/middleware/CANopenNode/305/CO_LSS.h

@@ -0,0 +1,251 @@
+/**
+ * CANopen Layer Setting Services protocol (common).
+ *
+ * @file        CO_LSS.h
+ * @ingroup     CO_LSS
+ * @author      Martin Wagner
+ * @copyright   2017 - 2020 Neuberger Gebaeudeautomation GmbH
+ *
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CO_LSS_H
+#define CO_LSS_H
+
+#include "301/CO_driver.h"
+
+/* default configuration, see CO_config.h */
+#ifndef CO_CONFIG_LSS
+#define CO_CONFIG_LSS (CO_CONFIG_LSS_SLAVE)
+#endif
+
+#if ((CO_CONFIG_LSS) & (CO_CONFIG_LSS_SLAVE | CO_CONFIG_LSS_MASTER)) || defined CO_DOXYGEN
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_LSS LSS
+ * @ingroup CO_CANopen_305
+ * @{
+ *
+ * CANopen Layer Setting Services protocol (common).
+ *
+ * LSS protocol is according to CiA DSP 305 V3.0.0.
+ *
+ * LSS services and protocols are used to inquire or to change the settings
+ * of three parameters of the physical layer, data link layer, and application
+ * layer on a CANopen device with LSS slave capability by a CANopen device
+ * with LSS master capability via the CAN network.
+ *
+ * The following parameters may be inquired or changed:
+ * - Node-ID of the CANopen device
+ * - Bit timing parameters of the physical layer (bit rate)
+ * - LSS address compliant to the identity object (1018h)
+ *
+ * The connection is established in one of two ways:
+ * - addressing a node by it's 128 bit LSS address. This requires that the
+ *   master already knows the node's LSS address.
+ * - scanning the network for unknown nodes (Fastscan). Using this method,
+ *   unknown devices can be found and configured one by one.
+ *
+ * Be aware that changing the bit rate is a critical step for the network. A
+ * failure will render the network unusable!
+ *
+ * Using this implementation, only master or slave can be included in one
+ * node at a time.
+ *
+ * For CAN identifiers see #CO_Default_CAN_ID_t
+ */
+
+/**
+ * LSS protocol command specifiers
+ *
+ * The LSS protocols are executed between the LSS master device and the LSS
+ * slave device(s) to implement the LSS services. Some LSS protocols require
+ * a sequence of CAN messages.
+ *
+ * As identifying method only "LSS fastscan" is supported.
+ */
+typedef enum {
+    CO_LSS_SWITCH_STATE_GLOBAL      = 0x04U, /**< Switch state global protocol */
+    CO_LSS_SWITCH_STATE_SEL_VENDOR  = 0x40U, /**< Switch state selective protocol - Vendor ID */
+    CO_LSS_SWITCH_STATE_SEL_PRODUCT = 0x41U, /**< Switch state selective protocol - Product code */
+    CO_LSS_SWITCH_STATE_SEL_REV     = 0x42U, /**< Switch state selective protocol - Revision number */
+    CO_LSS_SWITCH_STATE_SEL_SERIAL  = 0x43U, /**< Switch state selective protocol - Serial number */
+    CO_LSS_SWITCH_STATE_SEL         = 0x44U, /**< Switch state selective protocol - Slave response */
+    CO_LSS_CFG_NODE_ID              = 0x11U, /**< Configure node ID protocol */
+    CO_LSS_CFG_BIT_TIMING           = 0x13U, /**< Configure bit timing parameter protocol */
+    CO_LSS_CFG_ACTIVATE_BIT_TIMING  = 0x15U, /**< Activate bit timing parameter protocol */
+    CO_LSS_CFG_STORE                = 0x17U, /**< Store configuration protocol */
+    CO_LSS_IDENT_SLAVE              = 0x4FU, /**< LSS Fastscan response */
+    CO_LSS_IDENT_FASTSCAN           = 0x51U, /**< LSS Fastscan protocol */
+    CO_LSS_INQUIRE_VENDOR           = 0x5AU, /**< Inquire identity vendor-ID protocol */
+    CO_LSS_INQUIRE_PRODUCT          = 0x5BU, /**< Inquire identity product-code protocol */
+    CO_LSS_INQUIRE_REV              = 0x5CU, /**< Inquire identity revision-number protocol */
+    CO_LSS_INQUIRE_SERIAL           = 0x5DU, /**< Inquire identity serial-number protocol */
+    CO_LSS_INQUIRE_NODE_ID          = 0x5EU, /**< Inquire node-ID protocol */
+} CO_LSS_cs_t;
+
+/**
+ * Error codes for Configure node ID protocol
+ */
+typedef enum {
+    CO_LSS_CFG_NODE_ID_OK           = 0x00U,/**< Protocol successfully completed */
+    CO_LSS_CFG_NODE_ID_OUT_OF_RANGE = 0x01U,/**< NID out of range */
+    CO_LSS_CFG_NODE_ID_MANUFACTURER = 0xFFU /**< Manufacturer specific error. No further support */
+} CO_LSS_cfgNodeId_t;
+
+/**
+ * Error codes for Configure bit timing parameters protocol
+ */
+typedef enum {
+    CO_LSS_CFG_BIT_TIMING_OK           = 0x00U,/**< Protocol successfully completed */
+    CO_LSS_CFG_BIT_TIMING_OUT_OF_RANGE = 0x01U,/**< Bit timing / Bit rate not supported */
+    CO_LSS_CFG_BIT_TIMING_MANUFACTURER = 0xFFU /**< Manufacturer specific error. No further support */
+} CO_LSS_cfgBitTiming_t;
+
+/**
+ * Error codes for Store configuration protocol
+ */
+typedef enum {
+    CO_LSS_CFG_STORE_OK            = 0x00U, /**< Protocol successfully completed */
+    CO_LSS_CFG_STORE_NOT_SUPPORTED = 0x01U, /**< Store configuration not supported */
+    CO_LSS_CFG_STORE_FAILED        = 0x02U, /**< Storage media access error */
+    CO_LSS_CFG_STORE_MANUFACTURER  = 0xFFU  /**< Manufacturer specific error. No further support */
+} CO_LSS_cfgStore_t;
+
+/**
+ * Fastscan BitCheck. BIT0 means all bits are checked for equality by slave.
+ */
+typedef enum {
+    CO_LSS_FASTSCAN_BIT0    = 0x00U, /**< Least significant bit of IDnumbners bit area to be checked */
+    /* ... */
+    CO_LSS_FASTSCAN_BIT31   = 0x1FU, /**< dito */
+    CO_LSS_FASTSCAN_CONFIRM = 0x80U  /**< All LSS slaves waiting for scan respond and previous scan is reset */
+} CO_LSS_fastscan_bitcheck;
+
+#define CO_LSS_FASTSCAN_BITCHECK_VALID(bit) ((bit>=CO_LSS_FASTSCAN_BIT0 && bit<=CO_LSS_FASTSCAN_BIT31) || bit==CO_LSS_FASTSCAN_CONFIRM)
+
+/**
+ * Fastscan LSSsub, LSSnext
+ */
+typedef enum {
+    CO_LSS_FASTSCAN_VENDOR_ID = 0, /**< Vendor ID */
+    CO_LSS_FASTSCAN_PRODUCT   = 1, /**< Product code */
+    CO_LSS_FASTSCAN_REV       = 2, /**< Revision number */
+    CO_LSS_FASTSCAN_SERIAL    = 3  /**< Serial number */
+} CO_LSS_fastscan_lss_sub_next;
+
+#define CO_LSS_FASTSCAN_LSS_SUB_NEXT_VALID(index) (index>=CO_LSS_FASTSCAN_VENDOR_ID && index<=CO_LSS_FASTSCAN_SERIAL)
+
+/**
+ * The LSS address is a 128 bit number, uniquely identifying each node. It
+ * consists of the values in object 0x1018.
+ */
+typedef union {
+    uint32_t addr[4];
+    struct {
+        uint32_t vendorID;
+        uint32_t productCode;
+        uint32_t revisionNumber;
+        uint32_t serialNumber;
+    } identity;
+} CO_LSS_address_t;
+
+/**
+ * LSS finite state automaton
+ *
+ * The LSS FSA shall provide the following states:
+ * - Initial: Pseudo state, indicating the activation of the FSA.
+ * - LSS waiting: In this state, the LSS slave device waits for requests.
+ * - LSS configuration: In this state variables may be configured in the LSS slave.
+ * - Final: Pseudo state, indicating the deactivation of the FSA.
+ */
+typedef enum {
+    CO_LSS_STATE_WAITING = 0,                /**< LSS FSA waiting for requests*/
+    CO_LSS_STATE_CONFIGURATION = 1,          /**< LSS FSA waiting for configuration*/
+} CO_LSS_state_t;
+
+/**
+ * Definition of table_index for /CiA301/ bit timing table
+ */
+typedef enum {
+    CO_LSS_BIT_TIMING_1000 = 0,              /**< 1000kbit/s */
+    CO_LSS_BIT_TIMING_800  = 1,              /**< 800kbit/s */
+    CO_LSS_BIT_TIMING_500  = 2,              /**< 500kbit/s */
+    CO_LSS_BIT_TIMING_250  = 3,              /**< 250kbit/s */
+    CO_LSS_BIT_TIMING_125  = 4,              /**< 125kbit/s */
+    /* reserved            = 5 */
+    CO_LSS_BIT_TIMING_50   = 6,              /**< 50kbit/s */
+    CO_LSS_BIT_TIMING_20   = 7,              /**< 20kbit/s */
+    CO_LSS_BIT_TIMING_10   = 8,              /**< 10kbit/s */
+    CO_LSS_BIT_TIMING_AUTO = 9,              /**< Automatic bit rate detection */
+} CO_LSS_bitTimingTable_t;
+
+/**
+ * Lookup table for conversion between bit timing table and numerical
+ * bit rate
+ */
+static const uint16_t CO_LSS_bitTimingTableLookup[]  = {
+     1000,
+     800,
+     500,
+     250,
+     125,
+     0,
+     50,
+     20,
+     10,
+     0
+};
+
+/**
+ * Macro to check if index contains valid bit timing
+ */
+#define CO_LSS_BIT_TIMING_VALID(index) (index != 5 && (index >= CO_LSS_BIT_TIMING_1000 && index <= CO_LSS_BIT_TIMING_AUTO))
+
+/**
+ * Invalid node ID triggers node ID assignment
+ */
+#define CO_LSS_NODE_ID_ASSIGNMENT 0xFFU
+
+/**
+ * Macro to check if node id is valid
+ */
+#define CO_LSS_NODE_ID_VALID(nid) ((nid >= 1 && nid <= 0x7F) || nid == CO_LSS_NODE_ID_ASSIGNMENT)
+
+/**
+ * Macro to check if two LSS addresses are equal
+ */
+#define CO_LSS_ADDRESS_EQUAL(/*CO_LSS_address_t*/ a1, /*CO_LSS_address_t*/ a2) \
+     (a1.identity.productCode == a2.identity.productCode &&         \
+      a1.identity.revisionNumber == a2.identity.revisionNumber &&   \
+      a1.identity.serialNumber == a2.identity.serialNumber &&       \
+      a1.identity.vendorID == a2.identity.vendorID)
+
+/** @} */ /*@defgroup CO_LSS*/
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+#endif /* (CO_CONFIG_LSS) & (CO_CONFIG_LSS_SLAVE | CO_CONFIG_LSS_MASTER) */
+
+#endif /*CO_LSS_H*/

controller_yy_app/middleware/CANopenNode/305/CO_LSSmaster.c

@@ -0,0 +1,1116 @@
+/*
+ * CANopen LSS Master protocol.
+ *
+ * @file        CO_LSSmaster.c
+ * @ingroup     CO_LSS
+ * @author      Martin Wagner
+ * @copyright   2017 - 2020 Neuberger Gebaeudeautomation GmbH
+ *
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "305/CO_LSSmaster.h"
+
+#if (CO_CONFIG_LSS) & CO_CONFIG_LSS_MASTER
+
+#include <string.h>
+
+/*
+ * LSS master slave select state machine. Compared to #CO_LSS_state_t this
+ * has information if we currently have selected one or all slaves. This
+ * allows for some basic error checking.
+ */
+typedef enum {
+  CO_LSSmaster_STATE_WAITING = 0,
+  CO_LSSmaster_STATE_CFG_SLECTIVE,
+  CO_LSSmaster_STATE_CFG_GLOBAL,
+} CO_LSSmaster_state_t;
+
+/*
+ * LSS master slave command state machine
+ */
+typedef enum {
+  CO_LSSmaster_COMMAND_WAITING = 0,
+  CO_LSSmaster_COMMAND_SWITCH_STATE,
+  CO_LSSmaster_COMMAND_CFG_BIT_TIMING,
+  CO_LSSmaster_COMMAND_CFG_NODE_ID,
+  CO_LSSmaster_COMMAND_CFG_STORE,
+  CO_LSSmaster_COMMAND_INQUIRE_VENDOR,
+  CO_LSSmaster_COMMAND_INQUIRE_PRODUCT,
+  CO_LSSmaster_COMMAND_INQUIRE_REV,
+  CO_LSSmaster_COMMAND_INQUIRE_SERIAL,
+  CO_LSSmaster_COMMAND_INQUIRE,
+  CO_LSSmaster_COMMAND_IDENTIFY_FASTSCAN,
+} CO_LSSmaster_command_t;
+
+/*
+ * LSS master fastscan state machine
+ */
+typedef enum {
+  CO_LSSmaster_FS_STATE_CHECK,
+  CO_LSSmaster_FS_STATE_SCAN,
+  CO_LSSmaster_FS_STATE_VERIFY
+} CO_LSSmaster_fs_t;
+
+/*
+ * Read received message from CAN module.
+ *
+ * Function will be called (by CAN receive interrupt) every time, when CAN
+ * message with correct identifier will be received. For more information and
+ * description of parameters see file CO_driver.h.
+ */
+static void CO_LSSmaster_receive(void *object, void *msg)
+{
+    CO_LSSmaster_t *LSSmaster;
+    uint8_t DLC = CO_CANrxMsg_readDLC(msg);
+    uint8_t *data = CO_CANrxMsg_readData(msg);
+
+    LSSmaster = (CO_LSSmaster_t*)object;   /* this is the correct pointer type of the first argument */
+
+    /* verify message length and message overflow (previous message was not processed yet) */
+    if (DLC==8 && !CO_FLAG_READ(LSSmaster->CANrxNew) &&
+       LSSmaster->command!=CO_LSSmaster_COMMAND_WAITING){
+
+        /* copy data and set 'new message' flag */
+        memcpy(LSSmaster->CANrxData, data, sizeof(LSSmaster->CANrxData));
+
+        CO_FLAG_SET(LSSmaster->CANrxNew);
+
+#if (CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE
+        /* Optional signal to RTOS, which can resume task, which handles further processing. */
+        if (LSSmaster->pFunctSignal != NULL) {
+            LSSmaster->pFunctSignal(LSSmaster->functSignalObject);
+        }
+#endif
+    }
+}
+
+/*
+ * Check LSS timeout.
+ *
+ * Generally, we do not really care if the message has been received before
+ * or after the timeout expired. Only if no message has been received we have
+ * to check for timeouts
+ */
+static inline CO_LSSmaster_return_t CO_LSSmaster_check_timeout(
+        CO_LSSmaster_t         *LSSmaster,
+        uint32_t                timeDifference_us)
+{
+    CO_LSSmaster_return_t ret = CO_LSSmaster_WAIT_SLAVE;
+
+    LSSmaster->timeoutTimer += timeDifference_us;
+    if (LSSmaster->timeoutTimer >= LSSmaster->timeout_us) {
+        LSSmaster->timeoutTimer = 0;
+        ret = CO_LSSmaster_TIMEOUT;
+    }
+
+    return ret;
+}
+
+
+/******************************************************************************/
+CO_ReturnError_t CO_LSSmaster_init(
+        CO_LSSmaster_t         *LSSmaster,
+        uint16_t                timeout_ms,
+        CO_CANmodule_t         *CANdevRx,
+        uint16_t                CANdevRxIdx,
+        uint32_t                CANidLssSlave,
+        CO_CANmodule_t         *CANdevTx,
+        uint16_t                CANdevTxIdx,
+        uint32_t                CANidLssMaster)
+{
+    CO_ReturnError_t ret = CO_ERROR_NO;
+
+    /* verify arguments */
+    if (LSSmaster==NULL || CANdevRx==NULL || CANdevTx==NULL){
+        return CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    LSSmaster->timeout_us = (uint32_t)timeout_ms * 1000;
+    LSSmaster->state = CO_LSSmaster_STATE_WAITING;
+    LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
+    LSSmaster->timeoutTimer = 0;
+    CO_FLAG_CLEAR(LSSmaster->CANrxNew);
+    memset(LSSmaster->CANrxData, 0, sizeof(LSSmaster->CANrxData));
+#if (CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE
+    LSSmaster->pFunctSignal = NULL;
+    LSSmaster->functSignalObject = NULL;
+#endif
+
+    /* configure LSS CAN Slave response message reception */
+    ret = CO_CANrxBufferInit(
+            CANdevRx,             /* CAN device */
+            CANdevRxIdx,          /* rx buffer index */
+            CANidLssSlave,        /* CAN identifier */
+            0x7FF,                /* mask */
+            0,                    /* rtr */
+            (void*)LSSmaster,     /* object passed to receive function */
+            CO_LSSmaster_receive);/* this function will process received message */
+
+    /* configure LSS CAN Master message transmission */
+    LSSmaster->CANdevTx = CANdevTx;
+    LSSmaster->TXbuff = CO_CANtxBufferInit(
+            CANdevTx,             /* CAN device */
+            CANdevTxIdx,          /* index of specific buffer inside CAN module */
+            CANidLssMaster,       /* CAN identifier */
+            0,                    /* rtr */
+            8,                    /* number of data bytes */
+            0);                   /* synchronous message flag bit */
+
+    if (LSSmaster->TXbuff == NULL) {
+        ret = CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    return ret;
+}
+
+
+/******************************************************************************/
+void CO_LSSmaster_changeTimeout(
+        CO_LSSmaster_t         *LSSmaster,
+        uint16_t                timeout_ms)
+{
+    if (LSSmaster != NULL) {
+        LSSmaster->timeout_us = (uint32_t)timeout_ms * 1000;
+    }
+}
+
+
+#if (CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE
+/******************************************************************************/
+void CO_LSSmaster_initCallbackPre(
+        CO_LSSmaster_t         *LSSmaster,
+        void                   *object,
+        void                  (*pFunctSignal)(void *object))
+{
+    if (LSSmaster != NULL){
+        LSSmaster->functSignalObject = object;
+        LSSmaster->pFunctSignal = pFunctSignal;
+    }
+}
+#endif
+
+
+/*
+ * Helper function - initiate switch state
+ */
+static CO_LSSmaster_return_t CO_LSSmaster_switchStateSelectInitiate(
+        CO_LSSmaster_t         *LSSmaster,
+        CO_LSS_address_t       *lssAddress)
+{
+  CO_LSSmaster_return_t ret;
+
+  if (lssAddress != NULL) {
+      /* switch state select specific using LSS address */
+      LSSmaster->state = CO_LSSmaster_STATE_CFG_SLECTIVE;
+      LSSmaster->command = CO_LSSmaster_COMMAND_SWITCH_STATE;
+      LSSmaster->timeoutTimer = 0;
+
+      CO_FLAG_CLEAR(LSSmaster->CANrxNew);
+      memset(&LSSmaster->TXbuff->data[6], 0, sizeof(LSSmaster->TXbuff->data) - 6);
+      LSSmaster->TXbuff->data[0] = CO_LSS_SWITCH_STATE_SEL_VENDOR;
+      CO_setUint32(&LSSmaster->TXbuff->data[1], lssAddress->identity.vendorID);
+      CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
+      LSSmaster->TXbuff->data[0] = CO_LSS_SWITCH_STATE_SEL_PRODUCT;
+      CO_setUint32(&LSSmaster->TXbuff->data[1], lssAddress->identity.productCode);
+      CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
+      LSSmaster->TXbuff->data[0] = CO_LSS_SWITCH_STATE_SEL_REV;
+      CO_setUint32(&LSSmaster->TXbuff->data[1], lssAddress->identity.revisionNumber);
+      CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
+      LSSmaster->TXbuff->data[0] = CO_LSS_SWITCH_STATE_SEL_SERIAL;
+      CO_setUint32(&LSSmaster->TXbuff->data[1], lssAddress->identity.serialNumber);
+      CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
+
+      ret = CO_LSSmaster_WAIT_SLAVE;
+  }
+  else {
+      /* switch state global */
+      LSSmaster->state = CO_LSSmaster_STATE_CFG_GLOBAL;
+
+      CO_FLAG_CLEAR(LSSmaster->CANrxNew);
+      LSSmaster->TXbuff->data[0] = CO_LSS_SWITCH_STATE_GLOBAL;
+      LSSmaster->TXbuff->data[1] = CO_LSS_STATE_CONFIGURATION;
+      memset(&LSSmaster->TXbuff->data[2], 0, sizeof(LSSmaster->TXbuff->data) - 2);
+      CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
+
+      /* This is non-confirmed service! */
+      ret = CO_LSSmaster_OK;
+  }
+  return ret;
+}
+
+/*
+ * Helper function - wait for confirmation
+ */
+static CO_LSSmaster_return_t CO_LSSmaster_switchStateSelectWait(
+        CO_LSSmaster_t         *LSSmaster,
+        uint32_t                timeDifference_us)
+{
+    CO_LSSmaster_return_t ret;
+
+    if (CO_FLAG_READ(LSSmaster->CANrxNew)) {
+        uint8_t cs = LSSmaster->CANrxData[0];
+        CO_FLAG_CLEAR(LSSmaster->CANrxNew);
+
+        if (cs == CO_LSS_SWITCH_STATE_SEL) {
+            /* confirmation received */
+            ret = CO_LSSmaster_OK;
+        }
+        else {
+            ret = CO_LSSmaster_check_timeout(LSSmaster, timeDifference_us);
+        }
+    }
+    else {
+        ret = CO_LSSmaster_check_timeout(LSSmaster, timeDifference_us);
+    }
+
+    return ret;
+}
+
+/******************************************************************************/
+CO_LSSmaster_return_t CO_LSSmaster_switchStateSelect(
+        CO_LSSmaster_t         *LSSmaster,
+        uint32_t                timeDifference_us,
+        CO_LSS_address_t       *lssAddress)
+{
+    CO_LSSmaster_return_t ret = CO_LSSmaster_INVALID_STATE;
+
+    if (LSSmaster == NULL){
+        return CO_LSSmaster_ILLEGAL_ARGUMENT;
+    }
+
+    /* Initiate select */
+    if (LSSmaster->state==CO_LSSmaster_STATE_WAITING &&
+        LSSmaster->command==CO_LSSmaster_COMMAND_WAITING){
+
+        ret = CO_LSSmaster_switchStateSelectInitiate(LSSmaster, lssAddress);
+    }
+    /* Wait for confirmation */
+    else if (LSSmaster->command == CO_LSSmaster_COMMAND_SWITCH_STATE) {
+        ret = CO_LSSmaster_switchStateSelectWait(LSSmaster, timeDifference_us);
+    }
+
+    if (ret!=CO_LSSmaster_INVALID_STATE && ret!=CO_LSSmaster_WAIT_SLAVE) {
+        /* finished */
+        LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
+    }
+    if (ret < CO_LSSmaster_OK) {
+        /* switching failed, go back to waiting */
+        LSSmaster->state=CO_LSSmaster_STATE_WAITING;
+        LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
+    }
+    return ret;
+}
+
+
+/******************************************************************************/
+CO_LSSmaster_return_t CO_LSSmaster_switchStateDeselect(
+        CO_LSSmaster_t         *LSSmaster)
+{
+    CO_LSSmaster_return_t ret = CO_LSSmaster_INVALID_STATE;
+
+    if (LSSmaster == NULL){
+        return CO_LSSmaster_ILLEGAL_ARGUMENT;
+    }
+
+    /* We can always send this command to get into a clean state on the network.
+     * If no slave is selected, this command is ignored. */
+    LSSmaster->state = CO_LSSmaster_STATE_WAITING;
+    LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
+    LSSmaster->timeoutTimer = 0;
+
+    /* switch state global */
+    CO_FLAG_CLEAR(LSSmaster->CANrxNew);
+    LSSmaster->TXbuff->data[0] = CO_LSS_SWITCH_STATE_GLOBAL;
+    LSSmaster->TXbuff->data[1] = CO_LSS_STATE_WAITING;
+    memset(&LSSmaster->TXbuff->data[2], 0, sizeof(LSSmaster->TXbuff->data) - 2);
+    CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
+
+    /* This is non-confirmed service! */
+    ret = CO_LSSmaster_OK;
+
+    return ret;
+}
+
+
+/*
+ * Helper function - wait for confirmation, check for returned error code
+ *
+ * This uses the nature of the configure confirmation message design:
+ * - byte 0 -> cs
+ * - byte 1 -> Error Code, where
+ *    - 0  = OK
+ *    - 1 .. FE = Values defined by CiA. All currently defined values
+ *                are slave rejects. No further distinction on why the
+ *                slave did reject the request.
+ *    - FF = Manufacturer Error Code in byte 2
+ * - byte 2 -> Manufacturer Error, currently not used
+ *
+ * enums for the errorCode are
+ * - CO_LSS_cfgNodeId_t
+ * - CO_LSS_cfgBitTiming_t
+ * - CO_LSS_cfgStore_t
+ */
+static CO_LSSmaster_return_t CO_LSSmaster_configureCheckWait(
+        CO_LSSmaster_t         *LSSmaster,
+        uint32_t                timeDifference_us,
+        uint8_t                 csWait)
+{
+    CO_LSSmaster_return_t ret;
+
+    if (CO_FLAG_READ(LSSmaster->CANrxNew)) {
+        uint8_t cs = LSSmaster->CANrxData[0];
+        uint8_t errorCode = LSSmaster->CANrxData[1];
+        CO_FLAG_CLEAR(LSSmaster->CANrxNew);
+
+        if (cs == csWait) {
+            if (errorCode == 0) {
+                ret = CO_LSSmaster_OK;
+            }
+            else if (errorCode == 0xff) {
+                ret = CO_LSSmaster_OK_MANUFACTURER;
+            }
+            else {
+                ret = CO_LSSmaster_OK_ILLEGAL_ARGUMENT;
+            }
+        }
+        else {
+            ret = CO_LSSmaster_check_timeout(LSSmaster, timeDifference_us);
+        }
+    }
+    else {
+        ret = CO_LSSmaster_check_timeout(LSSmaster, timeDifference_us);
+    }
+
+    if (ret!=CO_LSSmaster_INVALID_STATE && ret!=CO_LSSmaster_WAIT_SLAVE) {
+        /* finished */
+        LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
+    }
+    return ret;
+}
+
+
+/******************************************************************************/
+CO_LSSmaster_return_t CO_LSSmaster_configureBitTiming(
+        CO_LSSmaster_t         *LSSmaster,
+        uint32_t                timeDifference_us,
+        uint16_t                bit)
+{
+    CO_LSSmaster_return_t ret = CO_LSSmaster_INVALID_STATE;
+    uint8_t bitTiming;
+
+    if (LSSmaster == NULL){
+        return CO_LSSmaster_ILLEGAL_ARGUMENT;
+    }
+
+    switch (bit) {
+        case 1000:  bitTiming = CO_LSS_BIT_TIMING_1000; break;
+        case 800:   bitTiming = CO_LSS_BIT_TIMING_800;  break;
+        case 500:   bitTiming = CO_LSS_BIT_TIMING_500;  break;
+        case 250:   bitTiming = CO_LSS_BIT_TIMING_250;  break;
+        case 125:   bitTiming = CO_LSS_BIT_TIMING_125;  break;
+        case 50:    bitTiming = CO_LSS_BIT_TIMING_50;   break;
+        case 20:    bitTiming = CO_LSS_BIT_TIMING_20;   break;
+        case 10:    bitTiming = CO_LSS_BIT_TIMING_10;   break;
+        case 0:     bitTiming = CO_LSS_BIT_TIMING_AUTO; break;
+        default:    return CO_LSSmaster_ILLEGAL_ARGUMENT;
+    }
+
+    /* Initiate config bit */
+    if (LSSmaster->state==CO_LSSmaster_STATE_CFG_SLECTIVE &&
+        LSSmaster->command==CO_LSSmaster_COMMAND_WAITING){
+
+        LSSmaster->command = CO_LSSmaster_COMMAND_CFG_BIT_TIMING;
+        LSSmaster->timeoutTimer = 0;
+
+        CO_FLAG_CLEAR(LSSmaster->CANrxNew);
+        LSSmaster->TXbuff->data[0] = CO_LSS_CFG_BIT_TIMING;
+        LSSmaster->TXbuff->data[1] = 0;
+        LSSmaster->TXbuff->data[2] = bitTiming;
+        memset(&LSSmaster->TXbuff->data[3], 0, sizeof(LSSmaster->TXbuff->data) - 3);
+        CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
+
+        ret = CO_LSSmaster_WAIT_SLAVE;
+    }
+    /* Wait for confirmation */
+    else if (LSSmaster->command == CO_LSSmaster_COMMAND_CFG_BIT_TIMING) {
+
+        ret = CO_LSSmaster_configureCheckWait(LSSmaster, timeDifference_us,
+                CO_LSS_CFG_BIT_TIMING);
+    }
+
+    if (ret!=CO_LSSmaster_INVALID_STATE && ret!=CO_LSSmaster_WAIT_SLAVE) {
+        /* finished */
+        LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
+    }
+    return ret;
+}
+
+
+/******************************************************************************/
+CO_LSSmaster_return_t CO_LSSmaster_configureNodeId(
+        CO_LSSmaster_t         *LSSmaster,
+        uint32_t                timeDifference_us,
+        uint8_t                 nodeId)
+{
+    CO_LSSmaster_return_t ret = CO_LSSmaster_INVALID_STATE;
+
+    if (LSSmaster==NULL || !CO_LSS_NODE_ID_VALID(nodeId)){
+        return CO_LSSmaster_ILLEGAL_ARGUMENT;
+    }
+
+    /* Initiate config node ID */
+    if ((LSSmaster->state==CO_LSSmaster_STATE_CFG_SLECTIVE ||
+        /* Let un-config node ID also be run in global mode for unconfiguring all nodes */
+        (LSSmaster->state==CO_LSSmaster_STATE_CFG_GLOBAL &&
+         nodeId == CO_LSS_NODE_ID_ASSIGNMENT)) &&
+         LSSmaster->command==CO_LSSmaster_COMMAND_WAITING) {
+
+        LSSmaster->command = CO_LSSmaster_COMMAND_CFG_NODE_ID;
+        LSSmaster->timeoutTimer = 0;
+
+        CO_FLAG_CLEAR(LSSmaster->CANrxNew);
+        LSSmaster->TXbuff->data[0] = CO_LSS_CFG_NODE_ID;
+        LSSmaster->TXbuff->data[1] = nodeId;
+        memset(&LSSmaster->TXbuff->data[2], 0, sizeof(LSSmaster->TXbuff->data) - 2);
+        CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
+
+        ret = CO_LSSmaster_WAIT_SLAVE;
+    }
+    /* Wait for confirmation */
+    else if (LSSmaster->command == CO_LSSmaster_COMMAND_CFG_NODE_ID) {
+
+        ret = CO_LSSmaster_configureCheckWait(LSSmaster, timeDifference_us,
+                CO_LSS_CFG_NODE_ID);
+    }
+
+    if (ret!=CO_LSSmaster_INVALID_STATE && ret!=CO_LSSmaster_WAIT_SLAVE) {
+        /* finished */
+        LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
+    }
+    return ret;
+}
+
+
+/******************************************************************************/
+CO_LSSmaster_return_t CO_LSSmaster_configureStore(
+        CO_LSSmaster_t         *LSSmaster,
+        uint32_t                timeDifference_us)
+{
+    CO_LSSmaster_return_t ret = CO_LSSmaster_INVALID_STATE;
+
+    if (LSSmaster == NULL){
+        return CO_LSSmaster_ILLEGAL_ARGUMENT;
+    }
+
+    /* Initiate config store */
+    if (LSSmaster->state==CO_LSSmaster_STATE_CFG_SLECTIVE &&
+        LSSmaster->command==CO_LSSmaster_COMMAND_WAITING){
+
+        LSSmaster->command = CO_LSSmaster_COMMAND_CFG_STORE;
+        LSSmaster->timeoutTimer = 0;
+
+        CO_FLAG_CLEAR(LSSmaster->CANrxNew);
+        LSSmaster->TXbuff->data[0] = CO_LSS_CFG_STORE;
+        memset(&LSSmaster->TXbuff->data[1], 0, sizeof(LSSmaster->TXbuff->data) - 1);
+        CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
+
+        ret = CO_LSSmaster_WAIT_SLAVE;
+    }
+    /* Wait for confirmation */
+    else if (LSSmaster->command == CO_LSSmaster_COMMAND_CFG_STORE) {
+
+        ret = CO_LSSmaster_configureCheckWait(LSSmaster, timeDifference_us,
+                CO_LSS_CFG_STORE);
+    }
+
+    if (ret!=CO_LSSmaster_INVALID_STATE && ret!=CO_LSSmaster_WAIT_SLAVE) {
+        /* finished */
+        LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
+    }
+    return ret;
+}
+
+
+/******************************************************************************/
+CO_LSSmaster_return_t CO_LSSmaster_ActivateBit(
+        CO_LSSmaster_t         *LSSmaster,
+        uint16_t                switchDelay_ms)
+{
+    CO_LSSmaster_return_t ret = CO_LSSmaster_INVALID_STATE;
+
+    if (LSSmaster == NULL){
+        return CO_LSSmaster_ILLEGAL_ARGUMENT;
+    }
+
+    /* for activating bit timing, we need to have all slaves set to config
+     * state. This check makes it a bit harder to shoot ourselves in the foot */
+    if (LSSmaster->state==CO_LSSmaster_STATE_CFG_GLOBAL &&
+        LSSmaster->command==CO_LSSmaster_COMMAND_WAITING){
+
+        CO_FLAG_CLEAR(LSSmaster->CANrxNew);
+        LSSmaster->TXbuff->data[0] = CO_LSS_CFG_ACTIVATE_BIT_TIMING;
+        CO_setUint16(&LSSmaster->TXbuff->data[1], switchDelay_ms);
+        memset(&LSSmaster->TXbuff->data[3], 0, sizeof(LSSmaster->TXbuff->data) - 3);
+        CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
+
+        /* This is non-confirmed service! */
+        ret = CO_LSSmaster_OK;
+    }
+
+    return ret;
+}
+
+/*
+ * Helper function - send request
+ */
+static CO_LSSmaster_return_t CO_LSSmaster_inquireInitiate(
+        CO_LSSmaster_t         *LSSmaster,
+        uint8_t                 cs)
+{
+    CO_FLAG_CLEAR(LSSmaster->CANrxNew);
+    LSSmaster->TXbuff->data[0] = cs;
+    memset(&LSSmaster->TXbuff->data[1], 0, sizeof(LSSmaster->TXbuff->data) - 1);
+    CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
+
+    return CO_LSSmaster_WAIT_SLAVE;
+}
+
+/*
+ * Helper function - wait for confirmation
+ */
+static CO_LSSmaster_return_t CO_LSSmaster_inquireCheckWait(
+        CO_LSSmaster_t         *LSSmaster,
+        uint32_t                timeDifference_us,
+        uint8_t                 csWait,
+        uint32_t               *value)
+{
+    CO_LSSmaster_return_t ret;
+
+    if (CO_FLAG_READ(LSSmaster->CANrxNew)) {
+        uint8_t cs = LSSmaster->CANrxData[0];
+        *value = CO_getUint32(&LSSmaster->CANrxData[1]);
+        CO_FLAG_CLEAR(LSSmaster->CANrxNew);
+
+        if (cs == csWait) {
+            ret = CO_LSSmaster_OK;
+        }
+        else {
+            ret = CO_LSSmaster_check_timeout(LSSmaster, timeDifference_us);
+        }
+    }
+    else {
+        ret = CO_LSSmaster_check_timeout(LSSmaster, timeDifference_us);
+    }
+
+    return ret;
+}
+
+/******************************************************************************/
+CO_LSSmaster_return_t CO_LSSmaster_InquireLssAddress(
+        CO_LSSmaster_t         *LSSmaster,
+        uint32_t                timeDifference_us,
+        CO_LSS_address_t       *lssAddress)
+{
+    CO_LSSmaster_return_t ret = CO_LSSmaster_INVALID_STATE;
+    CO_LSSmaster_command_t next = CO_LSSmaster_COMMAND_WAITING;
+
+    if (LSSmaster==NULL || lssAddress==NULL){
+        return CO_LSSmaster_ILLEGAL_ARGUMENT;
+    }
+
+    /* Check for reply */
+    if (LSSmaster->command == CO_LSSmaster_COMMAND_INQUIRE_VENDOR) {
+
+        ret = CO_LSSmaster_inquireCheckWait(LSSmaster, timeDifference_us,
+                CO_LSS_INQUIRE_VENDOR, &lssAddress->identity.vendorID);
+        if (ret == CO_LSSmaster_OK) {
+            /* Start next request */
+            next = CO_LSSmaster_COMMAND_INQUIRE_PRODUCT;
+            ret = CO_LSSmaster_WAIT_SLAVE;
+        }
+    }
+    else if (LSSmaster->command == CO_LSSmaster_COMMAND_INQUIRE_PRODUCT) {
+
+        ret = CO_LSSmaster_inquireCheckWait(LSSmaster, timeDifference_us,
+                CO_LSS_INQUIRE_PRODUCT, &lssAddress->identity.productCode);
+        if (ret == CO_LSSmaster_OK) {
+            /* Start next request */
+            next = CO_LSSmaster_COMMAND_INQUIRE_REV;
+            ret = CO_LSSmaster_WAIT_SLAVE;
+        }
+    }
+    else if (LSSmaster->command == CO_LSSmaster_COMMAND_INQUIRE_REV) {
+
+        ret = CO_LSSmaster_inquireCheckWait(LSSmaster, timeDifference_us,
+                CO_LSS_INQUIRE_REV, &lssAddress->identity.revisionNumber);
+        if (ret == CO_LSSmaster_OK) {
+            /* Start next request */
+            next = CO_LSSmaster_COMMAND_INQUIRE_SERIAL;
+            ret = CO_LSSmaster_WAIT_SLAVE;
+        }
+    }
+    else if (LSSmaster->command == CO_LSSmaster_COMMAND_INQUIRE_SERIAL) {
+
+        ret = CO_LSSmaster_inquireCheckWait(LSSmaster, timeDifference_us,
+                CO_LSS_INQUIRE_SERIAL, &lssAddress->identity.serialNumber);
+    }
+    /* Check for next request */
+    if (LSSmaster->state == CO_LSSmaster_STATE_CFG_SLECTIVE ||
+        LSSmaster->state == CO_LSSmaster_STATE_CFG_GLOBAL) {
+        if (LSSmaster->command == CO_LSSmaster_COMMAND_WAITING) {
+
+            LSSmaster->command = CO_LSSmaster_COMMAND_INQUIRE_VENDOR;
+            LSSmaster->timeoutTimer = 0;
+
+            ret = CO_LSSmaster_inquireInitiate(LSSmaster, CO_LSS_INQUIRE_VENDOR);
+        }
+        else if (next == CO_LSSmaster_COMMAND_INQUIRE_PRODUCT) {
+            LSSmaster->command = CO_LSSmaster_COMMAND_INQUIRE_PRODUCT;
+            LSSmaster->timeoutTimer = 0;
+
+            ret = CO_LSSmaster_inquireInitiate(LSSmaster, CO_LSS_INQUIRE_PRODUCT);
+        }
+        else if (next == CO_LSSmaster_COMMAND_INQUIRE_REV) {
+            LSSmaster->command = CO_LSSmaster_COMMAND_INQUIRE_REV;
+            LSSmaster->timeoutTimer = 0;
+
+            ret = CO_LSSmaster_inquireInitiate(LSSmaster, CO_LSS_INQUIRE_REV);
+        }
+        else if (next == CO_LSSmaster_COMMAND_INQUIRE_SERIAL) {
+            LSSmaster->command = CO_LSSmaster_COMMAND_INQUIRE_SERIAL;
+            LSSmaster->timeoutTimer = 0;
+
+            ret = CO_LSSmaster_inquireInitiate(LSSmaster, CO_LSS_INQUIRE_SERIAL);
+        }
+    }
+
+    if (ret!=CO_LSSmaster_INVALID_STATE && ret!=CO_LSSmaster_WAIT_SLAVE) {
+        /* finished */
+        LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
+    }
+    return ret;
+}
+
+
+/******************************************************************************/
+CO_LSSmaster_return_t CO_LSSmaster_Inquire(
+        CO_LSSmaster_t         *LSSmaster,
+        uint32_t                timeDifference_us,
+        CO_LSS_cs_t             lssInquireCs,
+        uint32_t               *value)
+{
+  CO_LSSmaster_return_t ret = CO_LSSmaster_INVALID_STATE;
+
+  if (LSSmaster==NULL || value==NULL){
+      return CO_LSSmaster_ILLEGAL_ARGUMENT;
+  }
+
+  /* send request */
+  if ((LSSmaster->state==CO_LSSmaster_STATE_CFG_SLECTIVE ||
+       LSSmaster->state==CO_LSSmaster_STATE_CFG_GLOBAL) &&
+       LSSmaster->command == CO_LSSmaster_COMMAND_WAITING) {
+
+      LSSmaster->command = CO_LSSmaster_COMMAND_INQUIRE;
+      LSSmaster->timeoutTimer = 0;
+
+      ret = CO_LSSmaster_inquireInitiate(LSSmaster, lssInquireCs);
+  }
+  /* Check for reply */
+  else if (LSSmaster->command == CO_LSSmaster_COMMAND_INQUIRE) {
+      ret = CO_LSSmaster_inquireCheckWait(LSSmaster, timeDifference_us,
+                                          lssInquireCs, value);
+  }
+
+  if (ret != CO_LSSmaster_WAIT_SLAVE) {
+      LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
+  }
+  return ret;
+}
+
+/*
+ * Helper function - send request
+ */
+static void CO_LSSmaster_FsSendMsg(
+        CO_LSSmaster_t         *LSSmaster,
+        uint32_t                idNumber,
+        uint8_t                 bitCheck,
+        uint8_t                 lssSub,
+        uint8_t                 lssNext)
+{
+    LSSmaster->timeoutTimer = 0;
+
+    CO_FLAG_CLEAR(LSSmaster->CANrxNew);
+    LSSmaster->TXbuff->data[0] = CO_LSS_IDENT_FASTSCAN;
+    CO_setUint32(&LSSmaster->TXbuff->data[1], idNumber);
+    LSSmaster->TXbuff->data[5] = bitCheck;
+    LSSmaster->TXbuff->data[6] = lssSub;
+    LSSmaster->TXbuff->data[7] = lssNext;
+
+    CO_CANsend(LSSmaster->CANdevTx, LSSmaster->TXbuff);
+}
+
+/*
+ * Helper function - wait for confirmation
+ */
+static CO_LSSmaster_return_t CO_LSSmaster_FsCheckWait(
+        CO_LSSmaster_t         *LSSmaster,
+        uint32_t                timeDifference_us)
+{
+    CO_LSSmaster_return_t ret;
+
+    ret = CO_LSSmaster_check_timeout(LSSmaster, timeDifference_us);
+    if (ret == CO_LSSmaster_TIMEOUT) {
+        ret = CO_LSSmaster_SCAN_NOACK;
+
+        if (CO_FLAG_READ(LSSmaster->CANrxNew)) {
+            uint8_t cs = LSSmaster->CANrxData[0];
+            CO_FLAG_CLEAR(LSSmaster->CANrxNew);
+
+            if (cs == CO_LSS_IDENT_SLAVE) {
+                /* At least one node is waiting for fastscan */
+                ret = CO_LSSmaster_SCAN_FINISHED;
+            }
+        }
+    }
+
+    return ret;
+}
+
+/*
+ * Helper function - initiate scan for 32 bit part of LSS address
+ */
+static CO_LSSmaster_return_t CO_LSSmaster_FsScanInitiate(
+        CO_LSSmaster_t                  *LSSmaster,
+        uint32_t                         timeDifference_us,
+        CO_LSSmaster_scantype_t          scan,
+        CO_LSS_fastscan_lss_sub_next     lssSub)
+{
+    (void)timeDifference_us;    /* unused */
+
+    LSSmaster->fsLssSub = lssSub;
+    LSSmaster->fsIdNumber = 0;
+
+    switch (scan) {
+        case CO_LSSmaster_FS_SCAN:
+            break;
+        case CO_LSSmaster_FS_MATCH:
+            /* No scanning requested */
+            return CO_LSSmaster_SCAN_FINISHED;
+        case CO_LSSmaster_FS_SKIP:
+        default:
+            return CO_LSSmaster_SCAN_FAILED;
+    }
+
+    LSSmaster->fsBitChecked = CO_LSS_FASTSCAN_BIT31;
+
+    /* trigger scan procedure by sending first message */
+    CO_LSSmaster_FsSendMsg(LSSmaster, LSSmaster->fsIdNumber,
+        LSSmaster->fsBitChecked, LSSmaster->fsLssSub, LSSmaster->fsLssSub);
+
+    return CO_LSSmaster_WAIT_SLAVE;
+}
+
+/*
+ * Helper function - scan for 32 bits of LSS address, one by one
+ */
+static CO_LSSmaster_return_t CO_LSSmaster_FsScanWait(
+        CO_LSSmaster_t                  *LSSmaster,
+        uint32_t                         timeDifference_us,
+        CO_LSSmaster_scantype_t          scan)
+{
+    CO_LSSmaster_return_t ret;
+
+    switch (scan) {
+        case CO_LSSmaster_FS_SCAN:
+            break;
+        case CO_LSSmaster_FS_MATCH:
+            /* No scanning requested */
+            return CO_LSSmaster_SCAN_FINISHED;
+        case CO_LSSmaster_FS_SKIP:
+        default:
+            return CO_LSSmaster_SCAN_FAILED;
+    }
+
+    ret = CO_LSSmaster_check_timeout(LSSmaster, timeDifference_us);
+    if (ret == CO_LSSmaster_TIMEOUT) {
+
+        ret = CO_LSSmaster_WAIT_SLAVE;
+
+        if (CO_FLAG_READ(LSSmaster->CANrxNew)) {
+            uint8_t cs = LSSmaster->CANrxData[0];
+            CO_FLAG_CLEAR(LSSmaster->CANrxNew);
+
+            if (cs != CO_LSS_IDENT_SLAVE) {
+                /* wrong response received. Can not continue */
+                return CO_LSSmaster_SCAN_FAILED;
+            }
+        }
+        else {
+            /* no response received, assumption is wrong */
+            LSSmaster->fsIdNumber |= 1UL << LSSmaster->fsBitChecked;
+        }
+
+        if (LSSmaster->fsBitChecked == CO_LSS_FASTSCAN_BIT0) {
+            /* Scanning cycle is finished, we now have 32 bit address data */
+            ret = CO_LSSmaster_SCAN_FINISHED;
+        }
+        else {
+            LSSmaster->fsBitChecked --;
+
+            CO_LSSmaster_FsSendMsg(LSSmaster,
+                LSSmaster->fsIdNumber, LSSmaster->fsBitChecked,
+                LSSmaster->fsLssSub, LSSmaster->fsLssSub);
+        }
+    }
+
+    return ret;
+}
+
+/*
+ * Helper function - initiate check for 32 bit part of LSS address
+ */
+static CO_LSSmaster_return_t CO_LSSmaster_FsVerifyInitiate(
+        CO_LSSmaster_t                  *LSSmaster,
+        uint32_t                         timeDifference_us,
+        CO_LSSmaster_scantype_t          scan,
+        uint32_t                         idNumberCheck,
+        CO_LSS_fastscan_lss_sub_next     lssNext)
+{
+    (void)timeDifference_us;    /* unused */
+
+    switch (scan) {
+        case CO_LSSmaster_FS_SCAN:
+            /* ID obtained by scan */
+            break;
+        case CO_LSSmaster_FS_MATCH:
+            /* ID given by user */
+            LSSmaster->fsIdNumber = idNumberCheck;
+            break;
+        case CO_LSSmaster_FS_SKIP:
+        default:
+            return CO_LSSmaster_SCAN_FAILED;
+    }
+
+    LSSmaster->fsBitChecked = CO_LSS_FASTSCAN_BIT0;
+
+    /* send request */
+    CO_LSSmaster_FsSendMsg(LSSmaster, LSSmaster->fsIdNumber,
+        LSSmaster->fsBitChecked, LSSmaster->fsLssSub, lssNext);
+
+    return CO_LSSmaster_WAIT_SLAVE;
+}
+
+/*
+ * Helper function - verify 32 bit LSS address, request node(s) to switch
+ * their state machine to the next state
+ */
+static CO_LSSmaster_return_t CO_LSSmaster_FsVerifyWait(
+        CO_LSSmaster_t                  *LSSmaster,
+        uint32_t                         timeDifference_us,
+        CO_LSSmaster_scantype_t          scan,
+        uint32_t                        *idNumberRet)
+{
+    CO_LSSmaster_return_t ret;
+
+    if (scan == CO_LSSmaster_FS_SKIP) {
+        return CO_LSSmaster_SCAN_FAILED;
+    }
+
+    ret = CO_LSSmaster_check_timeout(LSSmaster, timeDifference_us);
+    if (ret == CO_LSSmaster_TIMEOUT) {
+
+        *idNumberRet = 0;
+        ret = CO_LSSmaster_SCAN_NOACK;
+
+        if (CO_FLAG_READ(LSSmaster->CANrxNew)) {
+            uint8_t cs = LSSmaster->CANrxData[0];
+            CO_FLAG_CLEAR(LSSmaster->CANrxNew);
+
+            if (cs == CO_LSS_IDENT_SLAVE) {
+                *idNumberRet = LSSmaster->fsIdNumber;
+                ret = CO_LSSmaster_SCAN_FINISHED;
+            } else {
+                ret = CO_LSSmaster_SCAN_FAILED;
+            }
+        }
+    }
+
+    return ret;
+}
+
+/*
+ * Helper function - check which 32 bit to scan for next, if any
+ */
+static CO_LSS_fastscan_lss_sub_next CO_LSSmaster_FsSearchNext(
+        CO_LSSmaster_t                  *LSSmaster,
+        const CO_LSSmaster_fastscan_t   *fastscan)
+{
+    int i;
+
+    /* we search for the next LSS address part to scan for, beginning with the
+     * one after the current one. If there is none remaining, scanning is
+     * finished */
+    for (i = LSSmaster->fsLssSub + 1; i <= CO_LSS_FASTSCAN_SERIAL; i++) {
+        if (fastscan->scan[i] != CO_LSSmaster_FS_SKIP) {
+            return (CO_LSS_fastscan_lss_sub_next)i;
+        }
+    }
+    /* node selection is triggered by switching node state machine back
+     * to initial state */
+    return CO_LSS_FASTSCAN_VENDOR_ID;
+}
+
+/******************************************************************************/
+CO_LSSmaster_return_t CO_LSSmaster_IdentifyFastscan(
+        CO_LSSmaster_t          *LSSmaster,
+        uint32_t                 timeDifference_us,
+        CO_LSSmaster_fastscan_t *fastscan)
+{
+    uint8_t i;
+    uint8_t count;
+    CO_LSSmaster_return_t ret = CO_LSSmaster_INVALID_STATE;
+    CO_LSS_fastscan_lss_sub_next next;
+
+    /* parameter validation */
+    if (LSSmaster==NULL || fastscan==NULL){
+        return CO_LSSmaster_ILLEGAL_ARGUMENT;
+    }
+    if (fastscan->scan[0] == CO_LSSmaster_FS_SKIP) {
+        /* vendor ID scan cannot be skipped */
+        return CO_LSSmaster_ILLEGAL_ARGUMENT;
+    }
+    count = 0;
+    for (i = 0; i < (sizeof(fastscan->scan) / sizeof(fastscan->scan[0])); i++) {
+        if (fastscan->scan[i] == CO_LSSmaster_FS_SKIP) {
+            count ++;
+        }
+        if (count > 2) {
+            /* Node selection needs the Vendor ID and at least one other value */
+            return CO_LSSmaster_ILLEGAL_ARGUMENT;
+        }
+    }
+
+    /* state machine validation */
+    if (LSSmaster->state!=CO_LSSmaster_STATE_WAITING ||
+        (LSSmaster->command!=CO_LSSmaster_COMMAND_WAITING &&
+         LSSmaster->command!=CO_LSSmaster_COMMAND_IDENTIFY_FASTSCAN)) {
+        /* state machine not ready, other command is already processed */
+        return CO_LSSmaster_INVALID_STATE;
+    }
+
+    /* evaluate LSS state machine */
+    switch (LSSmaster->command) {
+        case CO_LSSmaster_COMMAND_WAITING:
+            /* start fastscan */
+            LSSmaster->command = CO_LSSmaster_COMMAND_IDENTIFY_FASTSCAN;
+
+            /* check if any nodes are waiting, if yes fastscan is reset */
+            LSSmaster->fsState = CO_LSSmaster_FS_STATE_CHECK;
+            CO_LSSmaster_FsSendMsg(LSSmaster, 0, CO_LSS_FASTSCAN_CONFIRM, 0, 0);
+
+            return CO_LSSmaster_WAIT_SLAVE;
+        default:
+            /* continue with evaluating fastscan state machine */
+            break;
+    }
+
+    /* evaluate fastscan state machine. The state machine is evaluated as following
+     * - check for non-configured nodes
+     * - scan for vendor ID
+     * - verify vendor ID, switch node state
+     * - scan for product code
+     * - verify product code, switch node state
+     * - scan for revision number
+     * - verify revision number, switch node state
+     * - scan for serial number
+     * - verify serial number, switch node to LSS configuration mode
+     * Certain steps can be skipped as mentioned in the function description.
+     * If one step is not ack'ed by a node, the scanning process is terminated
+     * and the correspondign error is returned. */
+    switch (LSSmaster->fsState) {
+        case CO_LSSmaster_FS_STATE_CHECK:
+            ret = CO_LSSmaster_FsCheckWait(LSSmaster, timeDifference_us);
+            if (ret == CO_LSSmaster_SCAN_FINISHED) {
+                memset(&fastscan->found, 0, sizeof(fastscan->found));
+
+                /* start scanning procedure by triggering vendor ID scan */
+                CO_LSSmaster_FsScanInitiate(LSSmaster, timeDifference_us,
+                      fastscan->scan[CO_LSS_FASTSCAN_VENDOR_ID],
+                      CO_LSS_FASTSCAN_VENDOR_ID);
+                ret = CO_LSSmaster_WAIT_SLAVE;
+
+                LSSmaster->fsState = CO_LSSmaster_FS_STATE_SCAN;
+            }
+            break;
+        case CO_LSSmaster_FS_STATE_SCAN:
+            ret = CO_LSSmaster_FsScanWait(LSSmaster, timeDifference_us,
+                      fastscan->scan[LSSmaster->fsLssSub]);
+            if (ret == CO_LSSmaster_SCAN_FINISHED) {
+                /* scanning finished, initiate verifcation. The verification
+                 * message also contains the node state machine "switch to
+                 * next state" request */
+                next = CO_LSSmaster_FsSearchNext(LSSmaster, fastscan);
+                ret = CO_LSSmaster_FsVerifyInitiate(LSSmaster, timeDifference_us,
+                          fastscan->scan[LSSmaster->fsLssSub],
+                          fastscan->match.addr[LSSmaster->fsLssSub], next);
+
+                LSSmaster->fsState = CO_LSSmaster_FS_STATE_VERIFY;
+            }
+            break;
+        case CO_LSSmaster_FS_STATE_VERIFY:
+            ret = CO_LSSmaster_FsVerifyWait(LSSmaster, timeDifference_us,
+                      fastscan->scan[LSSmaster->fsLssSub],
+                      &fastscan->found.addr[LSSmaster->fsLssSub]);
+            if (ret == CO_LSSmaster_SCAN_FINISHED) {
+                /* verification successful:
+                 * - assumed node id is correct
+                 * - node state machine has switched to the requested state,
+                 *   mirror that in the local copy */
+                next = CO_LSSmaster_FsSearchNext(LSSmaster, fastscan);
+                if (next == CO_LSS_FASTSCAN_VENDOR_ID) {
+                    /* fastscan finished, one node is now in LSS configuration
+                     * mode */
+                    LSSmaster->state = CO_LSSmaster_STATE_CFG_SLECTIVE;
+                }
+                else {
+                    /* initiate scan for next part of LSS address */
+                    ret = CO_LSSmaster_FsScanInitiate(LSSmaster,
+                              timeDifference_us, fastscan->scan[next], next);
+                    if (ret == CO_LSSmaster_SCAN_FINISHED) {
+                        /* Scanning is not requested. Initiate verification
+                         * step in next function call */
+                        ret = CO_LSSmaster_WAIT_SLAVE;
+                    }
+
+                    LSSmaster->fsState = CO_LSSmaster_FS_STATE_SCAN;
+                }
+            }
+            break;
+        default:
+            break;
+    }
+
+    if (ret != CO_LSSmaster_WAIT_SLAVE) {
+        /* finished */
+        LSSmaster->command = CO_LSSmaster_COMMAND_WAITING;
+    }
+    return ret;
+}
+
+#endif /* (CO_CONFIG_LSS) & CO_CONFIG_LSS_MASTER */

controller_yy_app/middleware/CANopenNode/305/CO_LSSmaster.h

@@ -0,0 +1,480 @@
+/**
+ * CANopen Layer Setting Service - master protocol.
+ *
+ * @file        CO_LSSmaster.h
+ * @ingroup     CO_LSS
+ * @author      Martin Wagner
+ * @copyright   2017 - 2020 Neuberger Gebaeudeautomation GmbH
+ *
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CO_LSSmaster_H
+#define CO_LSSmaster_H
+
+#include "305/CO_LSS.h"
+
+#if ((CO_CONFIG_LSS) & CO_CONFIG_LSS_MASTER) || defined CO_DOXYGEN
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_LSSmaster LSS Master
+ * @ingroup CO_CANopen_305
+ * @{
+ *
+ * CANopen Layer Setting Service - master protocol.
+ *
+ * The client/master can use the following services
+ * - node selection via LSS address
+ * - node selection via LSS fastscan
+ * - Inquire LSS address of currently selected node
+ * - Inquire node ID
+ * - Configure bit timing
+ * - Configure node ID
+ * - Activate bit timing parameters
+ * - Store configuration
+ *
+ * The LSS master is initalized during the CANopenNode initialization process.
+ * Except for enabling the LSS master in the configurator, no further
+ * run-time configuration is needed for basic operation.
+ * The LSS master does basic checking of commands and command sequence.
+ *
+ * ###Usage
+ *
+ * Usage of the CANopen LSS master is demonstrated in CANopenSocket application,
+ * see CO_LSS_master.c / CO_LSS_master.h files.
+ *
+ * It essentially is always as following:
+ * - select node(s)
+ * - call master command(s)
+ * - evaluate return value
+ * - deselect nodes
+ *
+ * All commands need to be run cyclically, e.g. like this
+ * \code{.c}
+
+    interval = 0;
+    do {
+        ret = CO_LSSmaster_InquireNodeId(LSSmaster, interval, &outval);
+
+        interval = 1; ms
+        sleep(interval);
+    } while (ret == CO_LSSmaster_WAIT_SLAVE);
+
+ * \endcode
+ *
+ * A more advanced implementation can make use of the callback function to
+ * shorten waiting times.
+ */
+
+/**
+ * Return values of LSS master functions.
+ */
+typedef enum {
+    CO_LSSmaster_SCAN_FINISHED       = 2,    /**< Scanning finished successful */
+    CO_LSSmaster_WAIT_SLAVE          = 1,    /**< No response arrived from slave yet */
+    CO_LSSmaster_OK                  = 0,    /**< Success, end of communication */
+    CO_LSSmaster_TIMEOUT             = -1,   /**< No reply received */
+    CO_LSSmaster_ILLEGAL_ARGUMENT    = -2,   /**< Invalid argument */
+    CO_LSSmaster_INVALID_STATE       = -3,   /**< State machine not ready or already processing a request */
+    CO_LSSmaster_SCAN_NOACK          = -4,   /**< No node found that matches scan request */
+    CO_LSSmaster_SCAN_FAILED         = -5,   /**< An error occurred while scanning. Try again */
+    CO_LSSmaster_OK_ILLEGAL_ARGUMENT = -101, /**< LSS success, node rejected argument because of non-supported value */
+    CO_LSSmaster_OK_MANUFACTURER     = -102, /**< LSS success, node rejected argument with manufacturer error code */
+} CO_LSSmaster_return_t;
+
+
+/**
+ * LSS master object.
+ */
+typedef struct{
+    uint32_t         timeout_us;       /**< LSS response timeout in us */
+
+    uint8_t          state;            /**< Node is currently selected */
+    uint8_t          command;          /**< Active command */
+    uint32_t         timeoutTimer;     /**< Timeout timer for LSS communication */
+
+    uint8_t          fsState;          /**< Current state of fastscan master state machine */
+    uint8_t          fsLssSub;         /**< Current state of node state machine */
+    uint8_t          fsBitChecked;     /**< Current scan bit position */
+    uint32_t         fsIdNumber;       /**< Current scan result */
+
+    volatile void   *CANrxNew;         /**< Indication if new LSS message is received from CAN bus. It needs to be cleared when received message is completely processed. */
+    uint8_t          CANrxData[8];     /**< 8 data bytes of the received message */
+#if ((CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+    void           (*pFunctSignal)(void *object); /**< From CO_LSSmaster_initCallbackPre() or NULL */
+    void            *functSignalObject;/**< Pointer to object */
+#endif
+    CO_CANmodule_t  *CANdevTx;         /**< From CO_LSSmaster_init() */
+    CO_CANtx_t      *TXbuff;           /**< CAN transmit buffer */
+}CO_LSSmaster_t;
+
+
+/**
+ * Default timeout for LSS slave in ms. This is the same as for SDO. For more
+ * info about LSS timeout see #CO_LSSmaster_changeTimeout()
+ */
+#ifndef CO_LSSmaster_DEFAULT_TIMEOUT
+#define CO_LSSmaster_DEFAULT_TIMEOUT 1000U /* ms */
+#endif
+
+
+/**
+ * Initialize LSS object.
+ *
+ * Function must be called in the communication reset section.
+ *
+ * @param LSSmaster This object will be initialized.
+ * @param timeout_ms slave response timeout in ms, for more detail see
+ * #CO_LSSmaster_changeTimeout()
+ * @param CANdevRx CAN device for LSS master reception.
+ * @param CANdevRxIdx Index of receive buffer in the above CAN device.
+ * @param CANidLssSlave COB ID for reception.
+ * @param CANdevTx CAN device for LSS master transmission.
+ * @param CANdevTxIdx Index of transmit buffer in the above CAN device.
+ * @param CANidLssMaster COB ID for transmission.
+ * @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ */
+CO_ReturnError_t CO_LSSmaster_init(
+        CO_LSSmaster_t         *LSSmaster,
+        uint16_t                timeout_ms,
+        CO_CANmodule_t         *CANdevRx,
+        uint16_t                CANdevRxIdx,
+        uint32_t                CANidLssSlave,
+        CO_CANmodule_t         *CANdevTx,
+        uint16_t                CANdevTxIdx,
+        uint32_t                CANidLssMaster);
+
+/**
+ * Change LSS master timeout
+ *
+ * On LSS, a "negative ack" is signaled by the slave not answering. Because of
+ * that, a low timeout value can significantly increase protocol speed in some
+ * cases (e.g. fastscan). However, as soon as there is activity on the bus,
+ * LSS messages can be delayed because of their low CAN network priority (see
+ * #CO_Default_CAN_ID_t).
+ *
+ * @remark Be aware that a "late response" will seriously mess up LSS, so this
+ * value must be selected "as high as necessary and as low as possible". CiA does
+ * neither specify nor recommend a value.
+ *
+ * @remark This timeout is per-transfer. If a command internally needs multiple
+ * transfers to complete, this timeout is applied on each transfer.
+ *
+ * @param LSSmaster This object.
+ * @param timeout_ms timeout value in ms
+ */
+void CO_LSSmaster_changeTimeout(
+        CO_LSSmaster_t         *LSSmaster,
+        uint16_t                timeout_ms);
+
+
+#if ((CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+/**
+ * Initialize LSSmasterRx callback function.
+ *
+ * Function initializes optional callback function, which should immediately
+ * start further LSS processing. Callback is called after LSS message is
+ * received from the CAN bus. It should signal the RTOS to resume corresponding
+ * task.
+ *
+ * @param LSSmaster This object.
+ * @param object Pointer to object, which will be passed to pFunctSignal(). Can be NULL
+ * @param pFunctSignal Pointer to the callback function. Not called if NULL.
+ */
+void CO_LSSmaster_initCallbackPre(
+        CO_LSSmaster_t         *LSSmaster,
+        void                   *object,
+        void                  (*pFunctSignal)(void *object));
+#endif
+
+
+/**
+ * Request LSS switch state select
+ *
+ * This function can select one specific or all nodes.
+ *
+ * Function must be called cyclically until it returns != #CO_LSSmaster_WAIT_SLAVE
+ * Function is non-blocking.
+ *
+ * @remark Only one selection can be active at any time.
+ *
+ * @param LSSmaster This object.
+ * @param timeDifference_us Time difference from previous function call in
+ * [microseconds]. Zero when request is started.
+ * @param lssAddress LSS target address. If NULL, all nodes are selected
+ * @return #CO_LSSmaster_ILLEGAL_ARGUMENT,  #CO_LSSmaster_INVALID_STATE,
+ * #CO_LSSmaster_WAIT_SLAVE, #CO_LSSmaster_OK, #CO_LSSmaster_TIMEOUT
+ */
+CO_LSSmaster_return_t CO_LSSmaster_switchStateSelect(
+        CO_LSSmaster_t         *LSSmaster,
+        uint32_t                timeDifference_us,
+        CO_LSS_address_t       *lssAddress);
+
+
+/**
+ * Request LSS switch state deselect
+ *
+ * This function deselects all nodes, so it doesn't matter if a specific
+ * node is selected.
+ *
+ * This function also resets the LSS master state machine to a clean state
+ *
+ * @param LSSmaster This object.
+ * @return #CO_LSSmaster_ILLEGAL_ARGUMENT,  #CO_LSSmaster_INVALID_STATE,
+ * #CO_LSSmaster_OK
+ */
+CO_LSSmaster_return_t CO_LSSmaster_switchStateDeselect(
+        CO_LSSmaster_t         *LSSmaster);
+
+
+/**
+ * Request LSS configure Bit Timing
+ *
+ * The new bit rate is set as new pending value.
+ *
+ * This function needs one specific node to be selected.
+ *
+ * Function must be called cyclically until it returns != #CO_LSSmaster_WAIT_SLAVE.
+ * Function is non-blocking.
+ *
+ * @param LSSmaster This object.
+ * @param timeDifference_us Time difference from previous function call in
+ * [microseconds]. Zero when request is started.
+ * @param bit new bit rate
+ * @return #CO_LSSmaster_ILLEGAL_ARGUMENT,  #CO_LSSmaster_INVALID_STATE,
+ * #CO_LSSmaster_WAIT_SLAVE, #CO_LSSmaster_OK, #CO_LSSmaster_TIMEOUT,
+ * #CO_LSSmaster_OK_MANUFACTURER, #CO_LSSmaster_OK_ILLEGAL_ARGUMENT
+ */
+CO_LSSmaster_return_t CO_LSSmaster_configureBitTiming(
+        CO_LSSmaster_t         *LSSmaster,
+        uint32_t                timeDifference_us,
+        uint16_t                bit);
+
+
+/**
+ * Request LSS configure node ID
+ *
+ * The new node id is set as new pending node ID.
+ *
+ * This function needs one specific node to be selected.
+ *
+ * Function must be called cyclically until it returns != #CO_LSSmaster_WAIT_SLAVE.
+ * Function is non-blocking.
+ *
+ * @param LSSmaster This object.
+ * @param timeDifference_us Time difference from previous function call in
+ * [microseconds]. Zero when request is started.
+ * @param nodeId new node ID. Special value #CO_LSS_NODE_ID_ASSIGNMENT can be
+ * used to invalidate node ID.
+ * @return #CO_LSSmaster_ILLEGAL_ARGUMENT,  #CO_LSSmaster_INVALID_STATE,
+ * #CO_LSSmaster_WAIT_SLAVE, #CO_LSSmaster_OK, #CO_LSSmaster_TIMEOUT,
+ * #CO_LSSmaster_OK_MANUFACTURER, #CO_LSSmaster_OK_ILLEGAL_ARGUMENT
+ */
+CO_LSSmaster_return_t CO_LSSmaster_configureNodeId(
+        CO_LSSmaster_t         *LSSmaster,
+        uint32_t                timeDifference_us,
+        uint8_t                 nodeId);
+
+
+/**
+ * Request LSS store configuration
+ *
+ * The current "pending" values for bit rate and node ID in LSS slave are
+ * stored as "permanent" values.
+ *
+ * This function needs one specific node to be selected.
+ *
+ * Function must be called cyclically until it returns != #CO_LSSmaster_WAIT_SLAVE.
+ * Function is non-blocking.
+ *
+ * @param LSSmaster This object.
+ * @param timeDifference_us Time difference from previous function call in
+ * [microseconds]. Zero when request is started.
+ * @return #CO_LSSmaster_ILLEGAL_ARGUMENT,  #CO_LSSmaster_INVALID_STATE,
+ * #CO_LSSmaster_WAIT_SLAVE, #CO_LSSmaster_OK, #CO_LSSmaster_TIMEOUT,
+ * #CO_LSSmaster_OK_MANUFACTURER, #CO_LSSmaster_OK_ILLEGAL_ARGUMENT
+ */
+CO_LSSmaster_return_t CO_LSSmaster_configureStore(
+        CO_LSSmaster_t         *LSSmaster,
+        uint32_t                timeDifference_us);
+
+
+/**
+ * Request LSS activate bit timing
+ *
+ * The current "pending" bit rate in LSS slave is applied.
+ *
+ * Be aware that changing the bit rate is a critical step for the network. A
+ * failure will render the network unusable! Therefore, this function only
+ * should be called if the following conditions are met:
+ * - all nodes support changing bit timing
+ * - new bit timing is successfully set as "pending" in all nodes
+ * - all nodes have to activate the new bit timing roughly at the same time.
+ *   Therefore this function needs all nodes to be selected.
+ *
+ * @param LSSmaster This object.
+ * @param switchDelay_ms delay that is applied by the slave once before and
+ * once after switching in ms.
+ * @return #CO_LSSmaster_ILLEGAL_ARGUMENT,  #CO_LSSmaster_INVALID_STATE,
+ * #CO_LSSmaster_OK
+ */
+CO_LSSmaster_return_t CO_LSSmaster_ActivateBit(
+        CO_LSSmaster_t         *LSSmaster,
+        uint16_t                switchDelay_ms);
+
+
+/**
+ * Request LSS inquire LSS address
+ *
+ * The LSS address value is read from the node. This is useful when the node
+ * was selected by fastscan.
+ *
+ * This function needs one specific node to be selected.
+ *
+ * Function must be called cyclically until it returns != #CO_LSSmaster_WAIT_SLAVE.
+ * Function is non-blocking.
+ *
+ * @param LSSmaster This object.
+ * @param timeDifference_us Time difference from previous function call in
+ * [microseconds]. Zero when request is started.
+ * @param [out] lssAddress read result when function returns successfully
+ * @return #CO_LSSmaster_ILLEGAL_ARGUMENT,  #CO_LSSmaster_INVALID_STATE,
+ * #CO_LSSmaster_WAIT_SLAVE, #CO_LSSmaster_OK, #CO_LSSmaster_TIMEOUT
+ */
+CO_LSSmaster_return_t CO_LSSmaster_InquireLssAddress(
+        CO_LSSmaster_t         *LSSmaster,
+        uint32_t                timeDifference_us,
+        CO_LSS_address_t       *lssAddress);
+
+
+/**
+ * Request LSS inquire node ID or part of LSS address
+ *
+ * The node-ID, identity vendor-ID, product-code, revision-number or
+ * serial-number value is read from the node.
+ *
+ * This function needs one specific node to be selected.
+ *
+ * Function must be called cyclically until it returns != #CO_LSSmaster_WAIT_SLAVE.
+ * Function is non-blocking.
+ *
+ * @param LSSmaster This object.
+ * @param timeDifference_us Time difference from previous function call in
+ * [microseconds]. Zero when request is started.
+ * @param lssInquireCs One of CO_LSS_INQUIRE_xx commands from #CO_LSS_cs_t.
+ * @param [out] value read result when function returns successfully
+ * @return #CO_LSSmaster_ILLEGAL_ARGUMENT,  #CO_LSSmaster_INVALID_STATE,
+ * #CO_LSSmaster_WAIT_SLAVE, #CO_LSSmaster_OK, #CO_LSSmaster_TIMEOUT
+ */
+CO_LSSmaster_return_t CO_LSSmaster_Inquire(
+        CO_LSSmaster_t         *LSSmaster,
+        uint32_t                timeDifference_us,
+        CO_LSS_cs_t             lssInquireCs,
+        uint32_t               *value);
+
+
+/**
+ * Scan type for #CO_LSSmaster_fastscan_t scan
+ */
+typedef enum {
+    CO_LSSmaster_FS_SCAN  = 0,    /**< Do full 32 bit scan */
+    CO_LSSmaster_FS_SKIP  = 1,    /**< Skip this value */
+    CO_LSSmaster_FS_MATCH = 2,    /**< Full 32 bit value is given as argument, just verify */
+} CO_LSSmaster_scantype_t;
+
+/**
+ * Parameters for LSS fastscan #CO_LSSmaster_IdentifyFastscan
+ */
+typedef struct{
+    CO_LSSmaster_scantype_t scan[4];  /**< Scan type for each part of the LSS address */
+    CO_LSS_address_t        match;    /**< Value to match in case of #CO_LSSmaster_FS_MATCH */
+    CO_LSS_address_t        found;    /**< Scan result */
+} CO_LSSmaster_fastscan_t;
+
+/**
+ * Select a node by LSS identify fastscan
+ *
+ * This initiates searching for a unconfigured node by the means of LSS fastscan
+ * mechanism. When this function is finished
+ * - a (more or less) arbitrary node is selected and ready for node ID assingment
+ * - no node is selected because the given criteria do not match a node
+ * - no node is selected because all nodes are already configured
+ *
+ * There are multiple ways to scan for a node. Depending on those, the scan
+ * will take different amounts of time:
+ * - full scan
+ * - partial scan
+ * - verification
+ *
+ * Most of the time, those are used in combination. Consider the following example:
+ * - Vendor ID and product code are known
+ * - Software version doesn't matter
+ * - Serial number is unknown
+ *
+ * In this case, the fastscan structure should be set up as following:
+ * \code{.c}
+CO_LSSmaster_fastscan_t fastscan;
+fastscan.scan[CO_LSS_FASTSCAN_VENDOR_ID] = CO_LSSmaster_FS_MATCH;
+fastscan.match.vendorID = YOUR_VENDOR_ID;
+fastscan.scan[CO_LSS_FASTSCAN_PRODUCT] = CO_LSSmaster_FS_MATCH;
+fastscan.match.productCode = YOUR_PRODUCT_CODE;
+fastscan.scan[CO_LSS_FASTSCAN_REV] = CO_LSSmaster_FS_SKIP;
+fastscan.scan[CO_LSS_FASTSCAN_SERIAL] = CO_LSSmaster_FS_SCAN;
+ * \endcode
+ *
+ * This example will take 2 scan cyles for verifying vendor ID and product code
+ * and 33 scan cycles to find the serial number.
+ *
+ * For scanning, the following limitations apply:
+ * - No more than two values can be skipped
+ * - Vendor ID cannot be skipped
+ *
+ * @remark When doing partial scans, it is in the responsibility of the user
+ * that the LSS address is unique.
+ *
+ * This function needs that no node is selected when starting the scan process.
+ *
+ * Function must be called cyclically until it returns != #CO_LSSmaster_WAIT_SLAVE.
+ * Function is non-blocking.
+ *
+ * @param LSSmaster This object.
+ * @param timeDifference_us Time difference from previous function call in
+ * [microseconds]. Zero when request is started.
+ * @param fastscan struct according to #CO_LSSmaster_fastscan_t.
+ * @return #CO_LSSmaster_ILLEGAL_ARGUMENT,  #CO_LSSmaster_INVALID_STATE,
+ * #CO_LSSmaster_WAIT_SLAVE, #CO_LSSmaster_SCAN_FINISHED, #CO_LSSmaster_SCAN_NOACK,
+ * #CO_LSSmaster_SCAN_FAILED
+ */
+CO_LSSmaster_return_t CO_LSSmaster_IdentifyFastscan(
+        CO_LSSmaster_t                  *LSSmaster,
+        uint32_t                         timeDifference_us,
+        CO_LSSmaster_fastscan_t         *fastscan);
+
+/** @} */ /*@defgroup CO_LSSmaster*/
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+#endif /* (CO_CONFIG_LSS) & CO_CONFIG_LSS_MASTER */
+
+#endif /*CO_LSSmaster_H*/

controller_yy_app/middleware/CANopenNode/305/CO_LSSslave.c

@@ -0,0 +1,490 @@
+/*
+ * CANopen LSS Slave protocol.
+ *
+ * @file        CO_LSSslave.c
+ * @ingroup     CO_LSS
+ * @author      Martin Wagner
+ * @author      Janez Paternoster
+ * @copyright   2017 - 2020 Neuberger Gebaeudeautomation GmbH
+ *
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "305/CO_LSSslave.h"
+
+#if (CO_CONFIG_LSS) & CO_CONFIG_LSS_SLAVE
+
+#include <string.h>
+
+/*
+ * Read received message from CAN module.
+ *
+ * Function will be called (by CAN receive interrupt) every time, when CAN
+ * message with correct identifier will be received. For more information and
+ * description of parameters see file CO_driver.h.
+ */
+static void CO_LSSslave_receive(void *object, void *msg)
+{
+    CO_LSSslave_t *LSSslave = (CO_LSSslave_t*)object;
+    uint8_t DLC = CO_CANrxMsg_readDLC(msg);
+
+    if (DLC == 8U && !CO_FLAG_READ(LSSslave->sendResponse)) {
+        bool_t request_LSSslave_process = false;
+        uint8_t *data = CO_CANrxMsg_readData(msg);
+        CO_LSS_cs_t cs = (CO_LSS_cs_t) data[0];
+
+        if (cs == CO_LSS_SWITCH_STATE_GLOBAL) {
+            uint8_t mode = data[1];
+
+            switch (mode) {
+                case CO_LSS_STATE_WAITING:
+                    if (LSSslave->lssState == CO_LSS_STATE_CONFIGURATION &&
+                        LSSslave->activeNodeID == CO_LSS_NODE_ID_ASSIGNMENT &&
+                        *LSSslave->pendingNodeID != CO_LSS_NODE_ID_ASSIGNMENT)
+                    {
+                        /* Slave process function will request NMT Reset comm.*/
+                        LSSslave->service = cs;
+                        request_LSSslave_process = true;
+                    }
+                    LSSslave->lssState = CO_LSS_STATE_WAITING;
+                    memset(&LSSslave->lssSelect, 0,
+                           sizeof(LSSslave->lssSelect));
+                    break;
+                case CO_LSS_STATE_CONFIGURATION:
+                    LSSslave->lssState = CO_LSS_STATE_CONFIGURATION;
+                    break;
+                default:
+                    break;
+            }
+        }
+        else if (LSSslave->lssState == CO_LSS_STATE_WAITING) {
+            switch (cs) {
+            case CO_LSS_SWITCH_STATE_SEL_VENDOR: {
+                uint32_t valSw;
+                memcpy(&valSw, &data[1], sizeof(valSw));
+                LSSslave->lssSelect.identity.vendorID = CO_SWAP_32(valSw);
+                break;
+            }
+            case CO_LSS_SWITCH_STATE_SEL_PRODUCT: {
+                uint32_t valSw;
+                memcpy(&valSw, &data[1], sizeof(valSw));
+                LSSslave->lssSelect.identity.productCode = CO_SWAP_32(valSw);
+                break;
+            }
+            case CO_LSS_SWITCH_STATE_SEL_REV: {
+                uint32_t valSw;
+                memcpy(&valSw, &data[1], sizeof(valSw));
+                LSSslave->lssSelect.identity.revisionNumber = CO_SWAP_32(valSw);
+                break;
+            }
+            case CO_LSS_SWITCH_STATE_SEL_SERIAL: {
+                uint32_t valSw;
+                memcpy(&valSw, &data[1], sizeof(valSw));
+                LSSslave->lssSelect.identity.serialNumber = CO_SWAP_32(valSw);
+
+                if (CO_LSS_ADDRESS_EQUAL(LSSslave->lssAddress,
+                                         LSSslave->lssSelect)
+                ) {
+                    LSSslave->lssState = CO_LSS_STATE_CONFIGURATION;
+                    LSSslave->service = cs;
+                    request_LSSslave_process = true;
+                }
+                break;
+            }
+            case CO_LSS_IDENT_FASTSCAN: {
+                /* fastscan is only active on unconfigured nodes */
+                if (*LSSslave->pendingNodeID == CO_LSS_NODE_ID_ASSIGNMENT &&
+                    LSSslave->activeNodeID == CO_LSS_NODE_ID_ASSIGNMENT)
+                {
+                    uint8_t bitCheck = data[5];
+                    uint8_t lssSub = data[6];
+                    uint8_t lssNext = data[7];
+                    uint32_t valSw;
+                    uint32_t idNumber;
+                    bool_t ack;
+
+                    if (!CO_LSS_FASTSCAN_BITCHECK_VALID(bitCheck) ||
+                        !CO_LSS_FASTSCAN_LSS_SUB_NEXT_VALID(lssSub) ||
+                        !CO_LSS_FASTSCAN_LSS_SUB_NEXT_VALID(lssNext)) {
+                        /* Invalid request */
+                        break;
+                    }
+
+                    memcpy(&valSw, &data[1], sizeof(valSw));
+                    idNumber = CO_SWAP_32(valSw);
+                    ack = false;
+
+                    if (bitCheck == CO_LSS_FASTSCAN_CONFIRM) {
+                        /* Confirm, Reset */
+                        ack = true;
+                        LSSslave->fastscanPos = CO_LSS_FASTSCAN_VENDOR_ID;
+                        memset(&LSSslave->lssFastscan, 0,
+                                sizeof(LSSslave->lssFastscan));
+                    }
+                    else if (LSSslave->fastscanPos == lssSub) {
+                        uint32_t mask = 0xFFFFFFFF << bitCheck;
+
+                        if ((LSSslave->lssAddress.addr[lssSub] & mask)
+                            == (idNumber & mask))
+                        {
+                            /* all requested bits match */
+                            ack = true;
+                            LSSslave->fastscanPos = lssNext;
+
+                            if (bitCheck == 0 && lssNext < lssSub) {
+                                /* complete match, enter configuration state */
+                                LSSslave->lssState = CO_LSS_STATE_CONFIGURATION;
+                            }
+                        }
+                    }
+                    if (ack) {
+#if (CO_CONFIG_LSS) & CO_CONFIG_LSS_SLAVE_FASTSCAN_DIRECT_RESPOND
+                        LSSslave->TXbuff->data[0] = CO_LSS_IDENT_SLAVE;
+                        memset(&LSSslave->TXbuff->data[1], 0,
+                               sizeof(LSSslave->TXbuff->data) - 1);
+                        CO_CANsend(LSSslave->CANdevTx, LSSslave->TXbuff);
+#else
+                        LSSslave->service = cs;
+                        request_LSSslave_process = true;
+#endif
+                    }
+                }
+                break;
+            }
+            default: {
+                break;
+            }
+            }
+        }
+        else { /* LSSslave->lssState == CO_LSS_STATE_CONFIGURATION */
+            memcpy(&LSSslave->CANdata, &data[0], sizeof(LSSslave->CANdata));
+            LSSslave->service = cs;
+            request_LSSslave_process = true;
+        }
+
+        if (request_LSSslave_process) {
+            CO_FLAG_SET(LSSslave->sendResponse);
+#if (CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE
+            /* Optional signal to RTOS, which can resume task,
+             * which handles further processing. */
+            if (LSSslave->pFunctSignalPre != NULL) {
+                LSSslave->pFunctSignalPre(LSSslave->functSignalObjectPre);
+            }
+#endif
+        }
+    }
+}
+
+
+/******************************************************************************/
+CO_ReturnError_t CO_LSSslave_init(
+        CO_LSSslave_t          *LSSslave,
+        CO_LSS_address_t       *lssAddress,
+        uint16_t               *pendingBitRate,
+        uint8_t                *pendingNodeID,
+        CO_CANmodule_t         *CANdevRx,
+        uint16_t                CANdevRxIdx,
+        uint32_t                CANidLssMaster,
+        CO_CANmodule_t         *CANdevTx,
+        uint16_t                CANdevTxIdx,
+        uint32_t                CANidLssSlave)
+{
+    CO_ReturnError_t ret = CO_ERROR_NO;
+
+    /* verify arguments */
+    if (LSSslave==NULL || pendingBitRate == NULL || pendingNodeID == NULL ||
+        CANdevRx==NULL || CANdevTx==NULL ||
+        !CO_LSS_NODE_ID_VALID(*pendingNodeID)
+    ) {
+        return CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    /* Application must make sure that lssAddress is filled with data. */
+
+    /* clear the object */
+    memset(LSSslave, 0, sizeof(CO_LSSslave_t));
+
+    /* Configure object variables */
+    memcpy(&LSSslave->lssAddress, lssAddress, sizeof(LSSslave->lssAddress));
+    LSSslave->lssState = CO_LSS_STATE_WAITING;
+    LSSslave->fastscanPos = CO_LSS_FASTSCAN_VENDOR_ID;
+
+    LSSslave->pendingBitRate = pendingBitRate;
+    LSSslave->pendingNodeID = pendingNodeID;
+    LSSslave->activeNodeID = *pendingNodeID;
+    CO_FLAG_CLEAR(LSSslave->sendResponse);
+
+    /* configure LSS CAN Master message reception */
+    ret = CO_CANrxBufferInit(
+            CANdevRx,             /* CAN device */
+            CANdevRxIdx,          /* rx buffer index */
+            CANidLssMaster,       /* CAN identifier */
+            0x7FF,                /* mask */
+            0,                    /* rtr */
+            (void*)LSSslave,      /* object passed to receive function */
+            CO_LSSslave_receive); /* this function will process received message */
+
+    /* configure LSS CAN Slave response message transmission */
+    LSSslave->CANdevTx = CANdevTx;
+    LSSslave->TXbuff = CO_CANtxBufferInit(
+            CANdevTx,             /* CAN device */
+            CANdevTxIdx,          /* index of specific buffer inside CAN module */
+            CANidLssSlave,        /* CAN identifier */
+            0,                    /* rtr */
+            8,                    /* number of data bytes */
+            0);                   /* synchronous message flag bit */
+
+    if (LSSslave->TXbuff == NULL) {
+        ret = CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    return ret;
+}
+
+
+#if (CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE
+/******************************************************************************/
+void CO_LSSslave_initCallbackPre(
+        CO_LSSslave_t          *LSSslave,
+        void                   *object,
+        void                  (*pFunctSignalPre)(void *object))
+{
+    if (LSSslave != NULL){
+        LSSslave->functSignalObjectPre = object;
+        LSSslave->pFunctSignalPre = pFunctSignalPre;
+    }
+}
+#endif
+
+
+/******************************************************************************/
+void CO_LSSslave_initCheckBitRateCallback(
+        CO_LSSslave_t          *LSSslave,
+        void                   *object,
+        bool_t                (*pFunctLSScheckBitRate)(void *object, uint16_t bitRate))
+{
+    if (LSSslave != NULL){
+        LSSslave->functLSScheckBitRateObject = object;
+        LSSslave->pFunctLSScheckBitRate = pFunctLSScheckBitRate;
+    }
+}
+
+
+/******************************************************************************/
+void CO_LSSslave_initActivateBitRateCallback(
+        CO_LSSslave_t          *LSSslave,
+        void                   *object,
+        void                  (*pFunctLSSactivateBitRate)(void *object, uint16_t delay))
+{
+    if (LSSslave != NULL){
+        LSSslave->functLSSactivateBitRateObject = object;
+        LSSslave->pFunctLSSactivateBitRate = pFunctLSSactivateBitRate;
+    }
+}
+
+
+/******************************************************************************/
+void CO_LSSslave_initCfgStoreCallback(
+        CO_LSSslave_t          *LSSslave,
+        void                   *object,
+        bool_t                (*pFunctLSScfgStore)(void *object, uint8_t id, uint16_t bitRate))
+{
+    if (LSSslave != NULL){
+        LSSslave->functLSScfgStoreObject = object;
+        LSSslave->pFunctLSScfgStore = pFunctLSScfgStore;
+    }
+}
+
+
+/******************************************************************************/
+bool_t CO_LSSslave_process(CO_LSSslave_t *LSSslave) {
+    bool_t resetCommunication = false;
+
+    if (CO_FLAG_READ(LSSslave->sendResponse)) {
+        uint8_t nid;
+        uint8_t errorCode;
+        uint8_t errorCodeManuf;
+        uint8_t tableSelector;
+        uint8_t tableIndex;
+        bool_t CANsend = false;
+        uint32_t valSw;
+
+        memset(&LSSslave->TXbuff->data[0], 0, sizeof(LSSslave->TXbuff->data));
+
+        switch (LSSslave->service) {
+        case CO_LSS_SWITCH_STATE_GLOBAL: {
+            /* Node-Id was unconfigured before, now it is configured,
+             * enter the NMT Reset communication autonomously. */
+            resetCommunication = true;
+            break;
+        }
+        case CO_LSS_SWITCH_STATE_SEL_SERIAL: {
+            LSSslave->TXbuff->data[0] = CO_LSS_SWITCH_STATE_SEL;
+            CANsend = true;
+            break;
+        }
+        case CO_LSS_CFG_NODE_ID: {
+            nid = LSSslave->CANdata[1];
+            errorCode = CO_LSS_CFG_NODE_ID_OK;
+
+            if (CO_LSS_NODE_ID_VALID(nid)) {
+                *LSSslave->pendingNodeID = nid;
+            }
+            else {
+                errorCode = CO_LSS_CFG_NODE_ID_OUT_OF_RANGE;
+            }
+
+            /* send confirmation */
+            LSSslave->TXbuff->data[0] = LSSslave->service;
+            LSSslave->TXbuff->data[1] = errorCode;
+            /* we do not use spec-error, always 0 */
+            CANsend = true;
+            break;
+        }
+        case CO_LSS_CFG_BIT_TIMING: {
+            if (LSSslave->pFunctLSScheckBitRate == NULL) {
+                /* setting bit timing is not supported. Drop request */
+                break;
+            }
+
+            tableSelector = LSSslave->CANdata[1];
+            tableIndex = LSSslave->CANdata[2];
+            errorCode = CO_LSS_CFG_BIT_TIMING_OK;
+            errorCodeManuf = CO_LSS_CFG_BIT_TIMING_OK;
+
+            if (tableSelector == 0 && CO_LSS_BIT_TIMING_VALID(tableIndex)) {
+                uint16_t bit = CO_LSS_bitTimingTableLookup[tableIndex];
+                bool_t bit_rate_supported = LSSslave->pFunctLSScheckBitRate(
+                    LSSslave->functLSScheckBitRateObject, bit);
+
+                if (bit_rate_supported) {
+                    *LSSslave->pendingBitRate = bit;
+                }
+                else {
+                    errorCode = CO_LSS_CFG_BIT_TIMING_MANUFACTURER;
+                    errorCodeManuf = CO_LSS_CFG_BIT_TIMING_OUT_OF_RANGE;
+                }
+            }
+            else {
+                /* we currently only support CiA301 bit timing table */
+                errorCode = CO_LSS_CFG_BIT_TIMING_OUT_OF_RANGE;
+            }
+
+            /* send confirmation */
+            LSSslave->TXbuff->data[0] = LSSslave->service;
+            LSSslave->TXbuff->data[1] = errorCode;
+            LSSslave->TXbuff->data[2] = errorCodeManuf;
+            CANsend = true;
+            break;
+        }
+        case CO_LSS_CFG_ACTIVATE_BIT_TIMING: {
+            if (LSSslave->pFunctLSScheckBitRate == NULL) {
+                /* setting bit timing is not supported. Drop request */
+                break;
+            }
+
+            /* notify application */
+            if (LSSslave->pFunctLSSactivateBitRate != NULL) {
+                uint16_t delay = ((uint16_t) LSSslave->CANdata[2]) << 8;
+                delay |= LSSslave->CANdata[1];
+                LSSslave->pFunctLSSactivateBitRate(
+                    LSSslave->functLSSactivateBitRateObject, delay);
+            }
+            break;
+        }
+        case CO_LSS_CFG_STORE: {
+            errorCode = CO_LSS_CFG_STORE_OK;
+
+            if (LSSslave->pFunctLSScfgStore == NULL) {
+                /* storing is not supported. Reply error */
+                errorCode = CO_LSS_CFG_STORE_NOT_SUPPORTED;
+            }
+            else {
+                bool_t result;
+                /* Store "pending" to "persistent" */
+                result =
+                   LSSslave->pFunctLSScfgStore(LSSslave->functLSScfgStoreObject,
+                                               *LSSslave->pendingNodeID,
+                                               *LSSslave->pendingBitRate);
+                if (!result) {
+                    errorCode = CO_LSS_CFG_STORE_FAILED;
+                }
+            }
+
+            /* send confirmation */
+            LSSslave->TXbuff->data[0] = LSSslave->service;
+            LSSslave->TXbuff->data[1] = errorCode;
+            /* we do not use spec-error, always 0 */
+            CANsend = true;
+            break;
+        }
+        case CO_LSS_INQUIRE_VENDOR: {
+            LSSslave->TXbuff->data[0] = LSSslave->service;
+            valSw = CO_SWAP_32(LSSslave->lssAddress.identity.vendorID);
+            memcpy(&LSSslave->TXbuff->data[1], &valSw, sizeof(valSw));
+            CANsend = true;
+            break;
+        }
+        case CO_LSS_INQUIRE_PRODUCT: {
+            LSSslave->TXbuff->data[0] = LSSslave->service;
+            valSw = CO_SWAP_32(LSSslave->lssAddress.identity.productCode);
+            memcpy(&LSSslave->TXbuff->data[1], &valSw, sizeof(valSw));
+            CANsend = true;
+            break;
+        }
+        case CO_LSS_INQUIRE_REV: {
+            LSSslave->TXbuff->data[0] = LSSslave->service;
+            valSw = CO_SWAP_32(LSSslave->lssAddress.identity.revisionNumber);
+            memcpy(&LSSslave->TXbuff->data[1], &valSw, sizeof(valSw));
+            CANsend = true;
+            break;
+        }
+        case CO_LSS_INQUIRE_SERIAL: {
+            LSSslave->TXbuff->data[0] = LSSslave->service;
+            valSw = CO_SWAP_32(LSSslave->lssAddress.identity.serialNumber);
+            memcpy(&LSSslave->TXbuff->data[1], &valSw, sizeof(valSw));
+            CANsend = true;
+            break;
+        }
+        case CO_LSS_INQUIRE_NODE_ID: {
+            LSSslave->TXbuff->data[0] = LSSslave->service;
+            LSSslave->TXbuff->data[1] = LSSslave->activeNodeID;
+            CANsend = true;
+            break;
+        }
+        case CO_LSS_IDENT_FASTSCAN: {
+            LSSslave->TXbuff->data[0] = CO_LSS_IDENT_SLAVE;
+            CANsend = true;
+            break;
+        }
+        default: {
+            break;
+        }
+        }
+
+        if (CANsend) {
+            CO_CANsend(LSSslave->CANdevTx, LSSslave->TXbuff);
+        }
+
+        CO_FLAG_CLEAR(LSSslave->sendResponse);
+    }
+
+    return resetCommunication;
+}
+
+#endif /* (CO_CONFIG_LSS) & CO_CONFIG_LSS_SLAVE */

controller_yy_app/middleware/CANopenNode/305/CO_LSSslave.h

@@ -0,0 +1,282 @@
+/**
+ * CANopen Layer Setting Service - slave protocol.
+ *
+ * @file        CO_LSSslave.h
+ * @ingroup     CO_LSS
+ * @author      Martin Wagner
+ * @author      Janez Paternoster
+ * @copyright   2017 - 2020 Neuberger Gebaeudeautomation GmbH
+ *
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CO_LSSslave_H
+#define CO_LSSslave_H
+
+#include "305/CO_LSS.h"
+
+#if ((CO_CONFIG_LSS) & CO_CONFIG_LSS_SLAVE) || defined CO_DOXYGEN
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_LSSslave LSS Slave
+ * @ingroup CO_CANopen_305
+ * @{
+ *
+ * CANopen Layer Setting Service - slave protocol.
+ *
+ * The slave provides the following services
+ * - node selection via LSS address
+ * - node selection via LSS fastscan
+ * - Inquire LSS address of currently selected node
+ * - Inquire node ID
+ * - Configure bit timing
+ * - Configure node ID
+ * - Activate bit timing parameters
+ * - Store configuration (bit rate and node ID)
+ *
+ * After CAN module start, the LSS slave and NMT slave are started and then
+ * coexist alongside each other. To achieve this behaviour, the CANopen node
+ * startup process has to be controlled more detailed. Therefore, CO_LSSinit()
+ * must be invoked between CO_CANinit() and CO_CANopenInit() in the
+ * communication reset section.
+ *
+ * Moreover, the LSS slave needs to pause the NMT slave initialization in case
+ * no valid node ID is available at start up. In that case CO_CANopenInit()
+ * skips initialization of other CANopen modules and CO_process() skips
+ * processing of other modules than LSS slave automatically.
+ *
+ * Variables for CAN-bitrate and CANopen node-id must be initialized by
+ * application from non-volatile memory or dip switches. Pointers to them are
+ * passed to CO_LSSinit() function. Those variables represents pending values.
+ * If node-id is valid in the moment it enters CO_LSSinit(), it also becomes
+ * active node-id and the stack initialises normally. Otherwise, node-id must be
+ * configured by lss and after successful configuration stack passes reset
+ * communication autonomously.
+ *
+ * Device with all threads can be normally initialized and running despite that
+ * node-id is not valid. Application must take care, because CANopen is not
+ * initialized. In that case CO_CANopenInit() returns error condition
+ * CO_ERROR_NODE_ID_UNCONFIGURED_LSS which must be handled properly. Status can
+ * also be checked with CO->nodeIdUnconfigured variable.
+ *
+ * Some callback functions may be initialized by application with
+ * CO_LSSslave_initCheckBitRateCallback(),
+ * CO_LSSslave_initActivateBitRateCallback() and
+ * CO_LSSslave_initCfgStoreCallback().
+ */
+
+/**
+ * LSS slave object.
+ */
+typedef struct{
+    CO_LSS_address_t        lssAddress;       /**< From #CO_LSSslave_init */
+    CO_LSS_state_t          lssState;         /**< #CO_LSS_state_t */
+    CO_LSS_address_t        lssSelect;        /**< Received LSS Address by select */
+
+    CO_LSS_address_t        lssFastscan;      /**< Received LSS Address by fastscan */
+    uint8_t                 fastscanPos;      /**< Current state of fastscan */
+
+    uint16_t               *pendingBitRate;   /**< Bit rate value that is temporarily configured */
+    uint8_t                *pendingNodeID;    /**< Node ID that is temporarily configured */
+    uint8_t                 activeNodeID;     /**< Node ID used at the CAN interface */
+
+    volatile void          *sendResponse;     /**< Variable indicates, if LSS response has to be sent by mainline processing function */
+    CO_LSS_cs_t             service;          /**< Service, which will have to be processed by mainline processing function */
+    uint8_t                 CANdata[8];       /**< Received CAN data, which will be processed by mainline processing function */
+
+#if ((CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+    void                  (*pFunctSignalPre)(void *object); /**< From CO_LSSslave_initCallbackPre() or NULL */
+    void                   *functSignalObjectPre;/**< Pointer to object */
+#endif
+    bool_t                (*pFunctLSScheckBitRate)(void *object, uint16_t bitRate); /**< From CO_LSSslave_initCheckBitRateCallback() or NULL */
+    void                   *functLSScheckBitRateObject; /** Pointer to object */
+    void                  (*pFunctLSSactivateBitRate)(void *object, uint16_t delay); /**< From CO_LSSslave_initActivateBitRateCallback() or NULL. Delay is in ms */
+    void                   *functLSSactivateBitRateObject; /** Pointer to object */
+    bool_t                (*pFunctLSScfgStore)(void *object, uint8_t id, uint16_t bitRate); /**< From CO_LSSslave_initCfgStoreCallback() or NULL */
+    void                   *functLSScfgStoreObject; /** Pointer to object */
+
+    CO_CANmodule_t         *CANdevTx;         /**< From #CO_LSSslave_init() */
+    CO_CANtx_t             *TXbuff;           /**< CAN transmit buffer */
+}CO_LSSslave_t;
+
+/**
+ * Initialize LSS object.
+ *
+ * Function must be called in the communication reset section.
+ *
+ * pendingBitRate and pendingNodeID must be pointers to external variables. Both
+ * variables must be initialized on program startup (after #CO_NMT_RESET_NODE)
+ * from non-volatile memory, dip switches or similar. They must not change
+ * during #CO_NMT_RESET_COMMUNICATION. Both variables can be changed by
+ * CO_LSSslave_process(), depending on commands from the LSS master.
+ *
+ * If pendingNodeID is valid (1 <= pendingNodeID <= 0x7F), then this becomes
+ * valid active nodeId just after exit of this function. In that case all other
+ * CANopen objects may be initialized and processed in run time.
+ *
+ * If pendingNodeID is not valid (pendingNodeID == 0xFF), then only LSS slave is
+ * initialized and processed in run time. In that state pendingNodeID can be
+ * configured and after successful configuration reset communication with all
+ * CANopen object is activated automatically.
+ *
+ * @remark The LSS address needs to be unique on the network. For this, the 128
+ * bit wide identity object (1018h) is used. Therefore, this object has to be
+ * fully initialized before passing it to this function (vendorID, product
+ * code, revisionNo, serialNo are set to 0 by default). Otherwise, if
+ * non-configured devices are present on CANopen network, LSS configuration may
+ * behave unpredictable.
+ *
+ * @param LSSslave This object will be initialized.
+ * @param lssAddress LSS address
+ * @param [in,out] pendingBitRate Pending bit rate of the CAN interface
+ * @param [in,out] pendingNodeID Pending node ID or 0xFF - invalid
+ * @param CANdevRx CAN device for LSS slave reception.
+ * @param CANdevRxIdx Index of receive buffer in the above CAN device.
+ * @param CANidLssMaster COB ID for reception.
+ * @param CANdevTx CAN device for LSS slave transmission.
+ * @param CANdevTxIdx Index of transmit buffer in the above CAN device.
+ * @param CANidLssSlave COB ID for transmission.
+ * @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ */
+CO_ReturnError_t CO_LSSslave_init(
+        CO_LSSslave_t          *LSSslave,
+        CO_LSS_address_t       *lssAddress,
+        uint16_t               *pendingBitRate,
+        uint8_t                *pendingNodeID,
+        CO_CANmodule_t         *CANdevRx,
+        uint16_t                CANdevRxIdx,
+        uint32_t                CANidLssMaster,
+        CO_CANmodule_t         *CANdevTx,
+        uint16_t                CANdevTxIdx,
+        uint32_t                CANidLssSlave);
+
+/**
+ * Process LSS communication
+ *
+ * Object is partially pre-processed after LSS message received. Further
+ * processing is inside this function.
+ *
+ * In case that Node-Id is unconfigured, then this function may request CANopen
+ * communication reset. This happens, when valid node-id is configured by LSS
+ * master.
+ *
+ * @param LSSslave This object.
+ * @return True, if #CO_NMT_RESET_COMMUNICATION is requested
+ */
+bool_t CO_LSSslave_process(CO_LSSslave_t *LSSslave);
+
+/**
+ * Get current LSS state
+ *
+ * @param LSSslave This object.
+ * @return #CO_LSS_state_t
+ */
+static inline CO_LSS_state_t CO_LSSslave_getState(CO_LSSslave_t *LSSslave) {
+    return (LSSslave == NULL) ? CO_LSS_STATE_WAITING : LSSslave->lssState;
+}
+
+
+#if ((CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+/**
+ * Initialize LSSslaveRx callback function.
+ *
+ * Function initializes optional callback function, which should immediately
+ * start further LSS processing. Callback is called after LSS message is
+ * received from the CAN bus. It should signal the RTOS to resume corresponding
+ * task.
+ *
+ * @param LSSslave This object.
+ * @param object Pointer to object, which will be passed to pFunctSignal(). Can be NULL
+ * @param pFunctSignalPre Pointer to the callback function. Not called if NULL.
+ */
+void CO_LSSslave_initCallbackPre(
+        CO_LSSslave_t          *LSSslave,
+        void                   *object,
+        void                  (*pFunctSignalPre)(void *object));
+#endif
+
+
+/**
+ * Initialize verify bit rate callback
+ *
+ * Function initializes callback function, which is called when "config bit
+ * timing parameters" is used. The callback function needs to check if the new bit
+ * rate is supported by the CANopen device. Callback returns "true" if supported.
+ * When no callback is set the LSS slave will no-ack the request, indicating to
+ * the master that bit rate change is not supported.
+ *
+ * @param LSSslave This object.
+ * @param object Pointer to object, which will be passed to pFunctLSScheckBitRate(). Can be NULL
+ * @param pFunctLSScheckBitRate Pointer to the callback function. Not called if NULL.
+ */
+void CO_LSSslave_initCheckBitRateCallback(
+        CO_LSSslave_t          *LSSslave,
+        void                   *object,
+        bool_t                (*pFunctLSScheckBitRate)(void *object, uint16_t bitRate));
+
+/**
+ * Initialize activate bit rate callback
+ *
+ * Function initializes callback function, which is called when "activate bit
+ * timing parameters" is used. The callback function gives the user an event to
+ * allow setting a timer or do calculations based on the exact time the request
+ * arrived.
+ * According to DSP 305 6.4.4, the delay has to be applied once before and once after
+ * switching bit rates. During this time, a device mustn't send any messages.
+ *
+ * @param LSSslave This object.
+ * @param object Pointer to object, which will be passed to pFunctLSSactivateBitRate(). Can be NULL
+ * @param pFunctLSSactivateBitRate Pointer to the callback function. Not called if NULL.
+ */
+void CO_LSSslave_initActivateBitRateCallback(
+        CO_LSSslave_t          *LSSslave,
+        void                   *object,
+        void                  (*pFunctLSSactivateBitRate)(void *object, uint16_t delay));
+
+/**
+ * Store configuration callback
+ *
+ * Function initializes callback function, which is called when "store configuration" is used.
+ * The callback function gives the user an event to store the corresponding node id and bit rate
+ * to NVM. Those values have to be supplied to the init function as "persistent values"
+ * after reset. If callback returns "true", success is send to the LSS master. When no
+ * callback is set the LSS slave will no-ack the request, indicating to the master
+ * that storing is not supported.
+ *
+ * @param LSSslave This object.
+ * @param object Pointer to object, which will be passed to pFunctLSScfgStore(). Can be NULL
+ * @param pFunctLSScfgStore Pointer to the callback function. Not called if NULL.
+ */
+void CO_LSSslave_initCfgStoreCallback(
+        CO_LSSslave_t          *LSSslave,
+        void                   *object,
+        bool_t                (*pFunctLSScfgStore)(void *object, uint8_t id, uint16_t bitRate));
+
+/** @} */ /*@defgroup CO_LSSslave*/
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+#endif /* (CO_CONFIG_LSS) & CO_CONFIG_LSS_SLAVE */
+
+#endif /*CO_LSSslave_H*/

controller_yy_app/middleware/CANopenNode/309/CMakeLists.txt

@@ -0,0 +1,5 @@
+# Copyright (c) 2021-2024 HPMicro
+# SPDX-License-Identifier: BSD-3-Clause
+
+sdk_inc(.)
+sdk_src(CO_gateway_ascii.c)

controller_yy_app/middleware/CANopenNode/309/CO_gateway_ascii.c

@@ -0,0 +1,2073 @@
+/*
+ * CANopen access from other networks - ASCII mapping (CiA 309-3 DS v3.0.0)
+ *
+ * @file        CO_gateway_ascii.c
+ * @ingroup     CO_CANopen_309_3
+ * @author      Janez Paternoster
+ * @author      Martin Wagner
+ * @copyright   2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "309/CO_gateway_ascii.h"
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
+
+#include <stdlib.h>
+#include <string.h>
+#include <ctype.h>
+#include <inttypes.h>
+#include <stdio.h>
+
+/* verify configuration */
+#if !((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ENABLE)
+ #error CO_CONFIG_FIFO_ENABLE must be enabled.
+#endif
+#if !((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_COMMANDS)
+ #error CO_CONFIG_FIFO_ASCII_COMMANDS must be enabled.
+#endif
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO
+ #if !((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_DATATYPES)
+  #error CO_CONFIG_FIFO_ASCII_DATATYPES must be enabled.
+ #endif
+#endif
+
+/******************************************************************************/
+CO_ReturnError_t CO_GTWA_init(CO_GTWA_t* gtwa,
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO) || defined CO_DOXYGEN
+                              CO_SDOclient_t* SDO_C,
+                              uint16_t SDOclientTimeoutTime_ms,
+                              bool_t SDOclientBlockTransfer,
+#endif
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_NMT) || defined CO_DOXYGEN
+                              CO_NMT_t *NMT,
+#endif
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS) || defined CO_DOXYGEN
+                              CO_LSSmaster_t *LSSmaster,
+#endif
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS) || defined CO_DOXYGEN
+                              CO_LEDs_t *LEDs,
+#endif
+                              uint8_t dummy)
+{
+    (void)dummy;
+    /* verify arguments */
+    if (gtwa == NULL
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO
+        || SDO_C == NULL || SDOclientTimeoutTime_ms == 0
+#endif
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_NMT
+        || NMT == NULL
+#endif
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS
+        || LSSmaster == NULL
+#endif
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS
+        || LEDs == NULL
+#endif
+    ) {
+        return CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+
+    /* clear the object */
+    memset(gtwa, 0, sizeof(CO_GTWA_t));
+
+    /* initialize variables */
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO
+    gtwa->SDO_C = SDO_C;
+    gtwa->SDOtimeoutTime = SDOclientTimeoutTime_ms;
+    gtwa->SDOblockTransferEnable = SDOclientBlockTransfer;
+#endif
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_NMT
+    gtwa->NMT = NMT;
+#endif
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS
+    gtwa->LSSmaster = LSSmaster;
+#endif
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS
+    gtwa->LEDs = LEDs;
+#endif
+    gtwa->net_default = -1;
+    gtwa->node_default = -1;
+    gtwa->state = CO_GTWA_ST_IDLE;
+    gtwa->respHold = false;
+
+    CO_fifo_init(&gtwa->commFifo,
+                 &gtwa->commBuf[0],
+                 CO_CONFIG_GTWA_COMM_BUF_SIZE + 1);
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG
+    CO_fifo_init(&gtwa->logFifo,
+                 &gtwa->logBuf[0],
+                 CO_CONFIG_GTWA_LOG_BUF_SIZE + 1);
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG) */
+
+    return CO_ERROR_NO;
+}
+
+
+/******************************************************************************/
+void CO_GTWA_initRead(CO_GTWA_t* gtwa,
+                      size_t (*readCallback)(void *object,
+                                             const char *buf,
+                                             size_t count,
+                                             uint8_t *connectionOK),
+                      void *readCallbackObject)
+{
+    if (gtwa != NULL) {
+        gtwa->readCallback = readCallback;
+        gtwa->readCallbackObject = readCallbackObject;
+    }
+}
+
+
+/******************************************************************************/
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG
+void CO_GTWA_log_print(CO_GTWA_t* gtwa, const char *message) {
+    if (gtwa != NULL && message != NULL) {
+        const char *c;
+
+        for (c = &message[0]; *c != 0; c++) {
+            CO_fifo_putc_ov(&gtwa->logFifo, *c);
+        }
+    }
+}
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG */
+
+
+/*******************************************************************************
+ * HELPER FUNCTIONS
+ ******************************************************************************/
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_HELP
+/* help strings ("\n" is used between string, "\r\n" closes the response.) */
+static const char CO_GTWA_helpString[] =
+"\nCommand strings start with '\"[\"<sequence>\"]\"' followed by:\n" \
+"[[<net>] <node>] r[ead] <index> <subindex> [<datatype>]        # SDO upload.\n" \
+"[[<net>] <node>] w[rite] <index> <subindex> <datatype> <value> # SDO download.\n" \
+"\n" \
+"[[<net>] <node>] start                   # NMT Start node.\n" \
+"[[<net>] <node>] stop                    # NMT Stop node.\n" \
+"[[<net>] <node>] preop[erational]        # NMT Set node to pre-operational.\n" \
+"[[<net>] <node>] reset node              # NMT Reset node.\n" \
+"[[<net>] <node>] reset comm[unication]   # NMT Reset communication.\n" \
+"\n" \
+"[<net>] set network <value>              # Set default net.\n" \
+"[<net>] set node <value>                 # Set default node.\n" \
+"[<net>] set sdo_timeout <value>          # Configure SDO client time-out in ms.\n" \
+"[<net>] set sdo_block <0|1>              # Enable/disable SDO block transfer.\n" \
+"\n" \
+"help [datatype|lss]                      # Print this or datatype or lss help.\n" \
+"led                                      # Print status LEDs of this device.\n" \
+"log                                      # Print message log.\n" \
+"\n" \
+"Response:\n" \
+"\"[\"<sequence>\"]\" OK | <value> |\n" \
+"                 ERROR:<SDO-abort-code> | ERROR:<internal-error-code>\n" \
+"\n" \
+"* Every command must be terminated with <CR><LF> ('\\r\\n'). characters. Same\n" \
+"  is response. String is not null terminated, <CR> is optional in command.\n" \
+"* Comments started with '#' are ignored. They may be on the beginning of the\n" \
+"  line or after the command string.\n" \
+"* 'sdo_timeout' is in milliseconds, 500 by default. Block transfer is\n" \
+"  disabled by default.\n" \
+"* If '<net>' or '<node>' is not specified within commands, then value defined\n" \
+"  by 'set network' or 'set node' command is used.\r\n";
+
+static const char CO_GTWA_helpStringDatatypes[] =
+"\nDatatypes:\n" \
+"b                  # Boolean.\n" \
+"i8, i16, i32, i64  # Signed integers.\n" \
+"u8, u16, u32, u64  # Unsigned integers.\n" \
+"x8, x16, x32, x64  # Unsigned integers, displayed as hexadecimal, non-standard.\n" \
+"r32, r64           # Real numbers.\n" \
+"vs                 # Visible string (between double quotes if multi-word).\n" \
+"os, us             # Octet, unicode string, (mime-base64 (RFC2045) based, line).\n" \
+"d                  # domain (mime-base64 (RFC2045) based, one line).\n" \
+"hex                # Hexagonal data, optionally space separated, non-standard.\r\n";
+
+static const char CO_GTWA_helpStringLss[] =
+"\nLSS commands:\n" \
+"lss_switch_glob <0|1>                  # Switch state global command.\n" \
+"lss_switch_sel <vendorID> <product code> \\\n" \
+"               <revisionNo> <serialNo> #Switch state selective.\n" \
+"lss_set_node <node>                    # Configure node-ID.\n" \
+"lss_conf_bitrate <table_selector=0> \\\n" \
+"                 <table_index>         # Configure bit-rate.\n" \
+"lss_activate_bitrate <switch_delay_ms> # Activate new bit-rate.\n" \
+"lss_store                              # LSS store configuration.\n" \
+"lss_inquire_addr [<LSSSUB=0..3>]       # Inquire LSS address.\n" \
+"lss_get_node                           # Inquire node-ID.\n" \
+"_lss_fastscan [<timeout_ms>]           # Identify fastscan, non-standard.\n" \
+"lss_allnodes [<timeout_ms> [<nodeStart=1..127> <store=0|1>\\\n" \
+"                [<scanType0> <vendorId> <scanType1> <productCode>\\\n" \
+"                 <scanType2> <revisionNo> <scanType3> <serialNo>]]]\n" \
+"                                       # Node-ID configuration of all nodes.\n" \
+"\n" \
+"* All LSS commands start with '\"[\"<sequence>\"]\" [<net>]'.\n" \
+"* <table_index>: 0=1000 kbit/s, 1=800 kbit/s, 2=500 kbit/s, 3=250 kbit/s,\n" \
+"                 4=125 kbit/s, 6=50 kbit/s, 7=20 kbit/s, 8=10 kbit/s, 9=auto\n" \
+"* <scanType>: 0=fastscan, 1=ignore, 2=match value in next parameter\r\n";
+#endif
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS
+#define CO_GTWA_LED_PRINTOUTS_SIZE 5
+static const char *CO_GTWA_LED_PRINTOUTS[CO_GTWA_LED_PRINTOUTS_SIZE] = {
+    " CANopen status LEDs: R  G         \r",
+    " CANopen status LEDs: R  G*        \r",
+    " CANopen status LEDs: R* G         \r",
+    " CANopen status LEDs: R* G*        \r",
+    "                                   \r"
+};
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS */
+
+
+/* Get uint32 number from token, verify limits and set *err if necessary */
+static inline uint32_t getU32(char *token, uint32_t min,
+                              uint32_t max, bool_t *err)
+{
+    char *sRet;
+    uint32_t num = strtoul(token, &sRet, 0);
+
+    if (sRet != strchr(token, '\0') || num < min || num > max) {
+        *err = true;
+    }
+
+    return num;
+}
+
+/* Verify net and node, return true on error */
+static bool_t checkNetNode(CO_GTWA_t *gtwa,
+                           int32_t net, int16_t node, uint8_t NodeMin,
+                           CO_GTWA_respErrorCode_t *errCode)
+{
+    bool_t e = false;
+    CO_GTWA_respErrorCode_t eCode;
+
+    if (node == -1) {
+        eCode = CO_GTWA_respErrorNoDefaultNodeSet;
+        e = true;
+    }
+    else if (node < NodeMin || node > 127) {
+        eCode = CO_GTWA_respErrorUnsupportedNode;
+        e = true;
+    }
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_MULTI_NET
+    else if (net == -1) {
+        eCode = CO_GTWA_respErrorNoDefaultNetSet;
+        e = true;
+    }
+    /* not implemented */
+    else if (net < CO_CONFIG_GTW_NET_MIN || net > CO_CONFIG_GTW_NET_MAX) {
+        eCode = CO_GTWA_respErrorUnsupportedNet;
+        e = true;
+    }
+#endif
+    else {
+        gtwa->net = (uint16_t)net;
+        gtwa->node = (uint8_t)node;
+    }
+    if (e) {
+        *errCode = eCode;
+    }
+    return e;
+}
+
+/* Verify net, return true on error */
+static bool_t checkNet(CO_GTWA_t *gtwa, int32_t net,
+                       CO_GTWA_respErrorCode_t *errCode)
+{
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_MULTI_NET
+    bool_t e = false;
+    CO_GTWA_respErrorCode_t eCode;
+
+    if (net == -1) {
+        eCode = CO_GTWA_respErrorNoDefaultNetSet;
+        e = true;
+    }
+    /* not implemented */
+    else if (net < CO_CONFIG_GTW_NET_MIN || net > CO_CONFIG_GTW_NET_MAX) {
+        eCode = CO_GTWA_respErrorUnsupportedNet;
+        e = true;
+    }
+    else {
+        gtwa->net = (uint16_t)net;
+    }
+    if (e) {
+        *errCode = eCode;
+    }
+    return e;
+#else
+    (void)errCode;    /* unused */
+    #define CO_CONFIG_GTW_NET_MIN 0
+    #define CO_CONFIG_GTW_NET_MAX 0xFFFF
+    gtwa->net = (uint16_t)net;
+    return false;
+#endif
+}
+
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO
+/* data types for SDO read or write */
+static const CO_GTWA_dataType_t dataTypes[] = {
+    {"hex", 0, CO_fifo_readHex2a, CO_fifo_cpyTok2Hex},  /* hex, non-standard */
+    {"b",   1, CO_fifo_readU82a,  CO_fifo_cpyTok2U8},   /* BOOLEAN */
+    {"i8",  1, CO_fifo_readI82a,  CO_fifo_cpyTok2I8},   /* INTEGER8 */
+    {"i16", 2, CO_fifo_readI162a, CO_fifo_cpyTok2I16},  /* INTEGER16 */
+    {"i32", 4, CO_fifo_readI322a, CO_fifo_cpyTok2I32},  /* INTEGER32 */
+    {"i64", 8, CO_fifo_readI642a, CO_fifo_cpyTok2I64},  /* INTEGER64 */
+    {"u8",  1, CO_fifo_readU82a,  CO_fifo_cpyTok2U8},   /* UNSIGNED8 */
+    {"u16", 2, CO_fifo_readU162a, CO_fifo_cpyTok2U16},  /* UNSIGNED16 */
+    {"u32", 4, CO_fifo_readU322a, CO_fifo_cpyTok2U32},  /* UNSIGNED32 */
+    {"u64", 8, CO_fifo_readU642a, CO_fifo_cpyTok2U64},  /* UNSIGNED64 */
+    {"x8",  1, CO_fifo_readX82a,  CO_fifo_cpyTok2U8},   /* UNSIGNED8 */
+    {"x16", 2, CO_fifo_readX162a, CO_fifo_cpyTok2U16},  /* UNSIGNED16 */
+    {"x32", 4, CO_fifo_readX322a, CO_fifo_cpyTok2U32},  /* UNSIGNED32 */
+    {"x64", 8, CO_fifo_readX642a, CO_fifo_cpyTok2U64},  /* UNSIGNED64 */
+    {"r32", 4, CO_fifo_readR322a, CO_fifo_cpyTok2R32},  /* REAL32 */
+    {"r64", 8, CO_fifo_readR642a, CO_fifo_cpyTok2R64},  /* REAL64 */
+    {"vs",  0, CO_fifo_readVs2a,  CO_fifo_cpyTok2Vs},   /* VISIBLE_STRING */
+    {"os",  0, CO_fifo_readB642a, CO_fifo_cpyTok2B64},  /* OCTET_STRING base64*/
+    {"us",  0, CO_fifo_readB642a, CO_fifo_cpyTok2B64},/* UNICODE_STRING base64*/
+    {"d",   0, CO_fifo_readB642a, CO_fifo_cpyTok2B64}   /* DOMAIN - base64 */
+};
+
+
+/* get data type from token */
+static const CO_GTWA_dataType_t *CO_GTWA_getDataType(char *token, bool_t *err) {
+    if (token != NULL && *err == false) {
+        int i;
+        int len = sizeof(dataTypes) / sizeof(CO_GTWA_dataType_t);
+
+        for (i = 0; i < len; i++) {
+            const CO_GTWA_dataType_t *dt = &dataTypes[i];
+            if (strcmp(token, dt->syntax) == 0) {
+                return dt;
+            }
+        }
+    }
+
+    *err = true;
+    return NULL;
+}
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
+
+
+/* transfer response buffer and verify if all bytes was read. Return true on
+ * success, or false, if communication is broken. */
+static bool_t respBufTransfer(CO_GTWA_t *gtwa) {
+    uint8_t connectionOK = 1;
+
+    if (gtwa->readCallback == NULL) {
+        /* no callback registered, just purge the response */
+        gtwa->respBufOffset = 0;
+        gtwa->respBufCount = 0;
+        gtwa->respHold = false;
+    }
+    else {
+        /* transfer response to the application */
+        size_t countRead =
+        gtwa->readCallback(gtwa->readCallbackObject,
+                           (const char *)&gtwa->respBuf[gtwa->respBufOffset],
+                           gtwa->respBufCount,
+                           &connectionOK);
+
+        if (countRead < gtwa->respBufCount) {
+            gtwa->respBufOffset += countRead;
+            gtwa->respBufCount -= countRead;
+            gtwa->respHold = true;
+        }
+        else {
+            gtwa->respBufOffset = 0;
+            gtwa->respBufCount = 0;
+            gtwa->respHold = false;
+        }
+    }
+    return connectionOK != 0;
+}
+
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_ERROR_DESC
+#ifndef CO_CONFIG_GTW_ASCII_ERROR_DESC_STRINGS
+#define CO_CONFIG_GTW_ASCII_ERROR_DESC_STRINGS
+typedef struct {
+    const uint32_t code;
+    const char* desc;
+} errorDescs_t;
+
+static const errorDescs_t errorDescs[] = {
+    {100, "Request not supported."},
+    {101, "Syntax error."},
+    {102, "Request not processed due to internal state."},
+    {103, "Time-out."},
+    {104, "No default net set."},
+    {105, "No default node set."},
+    {106, "Unsupported net."},
+    {107, "Unsupported node."},
+    {200, "Lost guarding message."},
+    {201, "Lost connection."},
+    {202, "Heartbeat started."},
+    {203, "Heartbeat lost."},
+    {204, "Wrong NMT state."},
+    {205, "Boot-up."},
+    {300, "Error passive."},
+    {301, "Bus off."},
+    {303, "CAN buffer overflow."},
+    {304, "CAN init."},
+    {305, "CAN active (at init or start-up)."},
+    {400, "PDO already used."},
+    {401, "PDO length exceeded."},
+    {501, "LSS implementation- / manufacturer-specific error."},
+    {502, "LSS node-ID not supported."},
+    {503, "LSS bit-rate not supported."},
+    {504, "LSS parameter storing failed."},
+    {505, "LSS command failed because of media error."},
+    {600, "Running out of memory."}
+};
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO
+static const errorDescs_t errorDescsSDO[] = {
+    {0x00000000, "No abort."},
+    {0x05030000, "Toggle bit not altered."},
+    {0x05040000, "SDO protocol timed out."},
+    {0x05040001, "Command specifier not valid or unknown."},
+    {0x05040002, "Invalid block size in block mode."},
+    {0x05040003, "Invalid sequence number in block mode."},
+    {0x05040004, "CRC error (block mode only)."},
+    {0x05040005, "Out of memory."},
+    {0x06010000, "Unsupported access to an object."},
+    {0x06010001, "Attempt to read a write only object."},
+    {0x06010002, "Attempt to write a read only object."},
+    {0x06020000, "Object does not exist."},
+    {0x06040041, "Object cannot be mapped to the PDO."},
+    {0x06040042, "Number and length of object to be mapped exceeds PDO length."},
+    {0x06040043, "General parameter incompatibility reasons."},
+    {0x06040047, "General internal incompatibility in device."},
+    {0x06060000, "Access failed due to hardware error."},
+    {0x06070010, "Data type does not match, length of service parameter does not match."},
+    {0x06070012, "Data type does not match, length of service parameter too high."},
+    {0x06070013, "Data type does not match, length of service parameter too short."},
+    {0x06090011, "Sub index does not exist."},
+    {0x06090030, "Invalid value for parameter (download only)."},
+    {0x06090031, "Value range of parameter written too high."},
+    {0x06090032, "Value range of parameter written too low."},
+    {0x06090036, "Maximum value is less than minimum value."},
+    {0x060A0023, "Resource not available: SDO connection."},
+    {0x08000000, "General error."},
+    {0x08000020, "Data cannot be transferred or stored to application."},
+    {0x08000021, "Data cannot be transferred or stored to application because of local control."},
+    {0x08000022, "Data cannot be transferred or stored to application because of present device state."},
+    {0x08000023, "Object dictionary not present or dynamic generation fails."},
+    {0x08000024, "No data available."}
+};
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
+#endif /* CO_CONFIG_GTW_ASCII_ERROR_DESC_STRINGS */
+
+static void responseWithError(CO_GTWA_t *gtwa,
+                              CO_GTWA_respErrorCode_t respErrorCode)
+{
+    int i;
+    int len = sizeof(errorDescs) / sizeof(errorDescs_t);
+    const char *desc = "-";
+
+    for (i = 0; i < len; i++) {
+        const errorDescs_t *ed = &errorDescs[i];
+        if (ed->code == respErrorCode) {
+            desc = ed->desc;
+        }
+    }
+
+    gtwa->respBufCount = snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
+                                  "[%"PRId32"] ERROR:%d #%s\r\n",
+                                  gtwa->sequence, respErrorCode, desc);
+    respBufTransfer(gtwa);
+}
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO
+static void responseWithErrorSDO(CO_GTWA_t *gtwa,
+                                 CO_SDO_abortCode_t abortCode,
+                                 bool_t postponed)
+{
+    int i;
+    int len = sizeof(errorDescsSDO) / sizeof(errorDescs_t);
+    const char *desc = "-";
+
+    for (i = 0; i < len; i++) {
+        const errorDescs_t *ed = &errorDescsSDO[i];
+        if (ed->code == abortCode) {
+            desc = ed->desc;
+        }
+    }
+
+    if (!postponed) {
+        gtwa->respBufCount = snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
+                                      "[%"PRId32"] ERROR:0x%08X #%s\r\n",
+                                      gtwa->sequence, abortCode, desc);
+    }
+    else {
+        gtwa->respBufCount = snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
+                                      "\n...ERROR:0x%08X #%s\r\n",
+                                      abortCode, desc);
+    }
+
+    respBufTransfer(gtwa);
+}
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
+
+#else /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_ERROR_DESC */
+static inline void responseWithError(CO_GTWA_t *gtwa,
+                                     CO_GTWA_respErrorCode_t respErrorCode)
+{
+    gtwa->respBufCount = snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
+                                  "[%"PRId32"] ERROR:%d\r\n",
+                                  gtwa->sequence, respErrorCode);
+    respBufTransfer(gtwa);
+}
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO
+static inline void responseWithErrorSDO(CO_GTWA_t *gtwa,
+                                        CO_SDO_abortCode_t abortCode,
+                                        bool_t postponed)
+{
+    if (!postponed) {
+        gtwa->respBufCount = snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
+                                      "[%"PRId32"] ERROR:0x%08X\r\n",
+                                      gtwa->sequence, abortCode);
+    }
+    else {
+        gtwa->respBufCount = snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
+                                      "\n...ERROR:0x%08X\r\n",
+                                      abortCode);
+    }
+
+    respBufTransfer(gtwa);
+}
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_ERROR_DESC */
+
+
+static inline void responseWithOK(CO_GTWA_t *gtwa) {
+    gtwa->respBufCount = snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
+                                  "[%"PRId32"] OK\r\n",
+                                  gtwa->sequence);
+    respBufTransfer(gtwa);
+}
+
+
+static inline void responseWithEmpty(CO_GTWA_t *gtwa) {
+    gtwa->respBufCount = snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
+                                  "\r\n");
+    respBufTransfer(gtwa);
+}
+
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS
+static void responseLSS(CO_GTWA_t *gtwa, CO_LSSmaster_return_t lss_ret) {
+    if (lss_ret == CO_LSSmaster_OK) {
+        responseWithOK(gtwa);
+    }
+    else {
+        CO_GTWA_respErrorCode_t respErrorCode;
+
+        if (lss_ret==CO_LSSmaster_TIMEOUT || lss_ret==CO_LSSmaster_SCAN_NOACK) {
+            respErrorCode = CO_GTWA_respErrorTimeOut;
+        }
+        else if (lss_ret == CO_LSSmaster_OK_MANUFACTURER) {
+            respErrorCode = CO_GTWA_respErrorLSSmanufacturer;
+        }
+        else {
+            respErrorCode = CO_GTWA_respErrorInternalState;
+        }
+        responseWithError(gtwa, respErrorCode);
+    }
+}
+#endif
+
+
+static inline void convertToLower(char *token, size_t maxCount) {
+    size_t i;
+    char *c = &token[0];
+
+    for (i = 0; i < maxCount; i++) {
+        if (*c == 0) {
+            break;
+        } else {
+            *c = tolower((int)*c);
+        }
+        c++;
+    }
+}
+
+
+/*******************************************************************************
+ * PROCESS FUNCTION
+ ******************************************************************************/
+void CO_GTWA_process(CO_GTWA_t *gtwa,
+                     bool_t enable,
+                     uint32_t timeDifference_us,
+                     uint32_t *timerNext_us)
+{
+    (void)timerNext_us; /* may be unused */
+
+    bool_t err = false; /* syntax or other error, true or false, I/O variable */
+    char closed; /* indication of command delimiter, I/O variable */
+    CO_GTWA_respErrorCode_t respErrorCode = CO_GTWA_respErrorNone;
+
+    if (gtwa == NULL) {
+        return;
+    }
+
+    if (!enable) {
+        gtwa->state = CO_GTWA_ST_IDLE;
+        CO_fifo_reset(&gtwa->commFifo);
+        return;
+    }
+
+    /* If there is some more output data for application, read them first.
+     * Hold on this state, if necessary. */
+    if (gtwa->respHold) {
+        timeDifference_us += gtwa->timeDifference_us_cumulative;
+
+        respBufTransfer(gtwa);
+        if (gtwa->respHold) {
+            gtwa->timeDifference_us_cumulative = timeDifference_us;
+            return;
+        }
+        else {
+            gtwa->timeDifference_us_cumulative = 0;
+        }
+    }
+
+    /***************************************************************************
+    * COMMAND PARSER
+    ***************************************************************************/
+    /* if idle, search for new command, skip comments or empty lines */
+    while (gtwa->state == CO_GTWA_ST_IDLE
+           && CO_fifo_CommSearch(&gtwa->commFifo, false)
+    ) {
+        char tok[20];
+        size_t n;
+        uint32_t ui[3];
+        int i;
+        int32_t net = gtwa->net_default;
+        int16_t node = gtwa->node_default;
+
+
+        /* parse mandatory token '"["<sequence>"]"' */
+        closed = -1;
+        n = CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
+        /* Break if error in token or token was found, but closed with
+         * command delimiter. */
+        if (err || (n > 0 && closed != 0)) {
+            err = true;
+            break;
+        }
+        /* If empty line or just comment, continue with next command */
+        else if (n == 0 && closed != 0) {
+            responseWithEmpty(gtwa);
+            continue;
+        }
+        if (tok[0] != '[' || tok[strlen(tok)-1] != ']') {
+            err = true;
+            break;
+        }
+        tok[strlen(tok)-1] = '\0';
+        gtwa->sequence = getU32(tok + 1, 0, 0xFFFFFFFF, &err);
+        if (err) break;
+
+
+        /* parse optional tokens '[[<net>] <node>]', both numerical. Then
+         * follows mandatory token <command>, which is not numerical. */
+        for (i = 0; i < 3; i++) {
+            closed = -1;
+            n = CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok),
+                                  &closed, &err);
+            if (err || n == 0) {
+                /* empty token, break on error */
+                err = true;
+                break;
+            } else if (isdigit((int)tok[0]) == 0) {
+                /* <command> found */
+                break;
+            } else if (closed != 0) {
+                /* numerical value must not be closed */
+                err = true;
+                break;
+            }
+
+            ui[i] = getU32(tok, 0, 0xFFFFFFFF, &err);
+            if (err) break;
+        }
+        if (err) break;
+
+        switch(i) {
+        case 0: /* only <command> (pointed by token) */
+            break;
+        case 1: /* <node> and <command> tokens */
+            if (ui[0] > 127) {
+                err = true;
+                respErrorCode = CO_GTWA_respErrorUnsupportedNode;
+            }
+            else {
+                node = (int16_t) ui[0];
+            }
+            break;
+        case 2: /* <net>, <node> and <command> tokens */
+            if (ui[0] > 0xFFFF) {
+                err = true;
+                respErrorCode = CO_GTWA_respErrorUnsupportedNet;
+            }
+            else if (ui[1] > 127) {
+                err = true;
+                respErrorCode = CO_GTWA_respErrorUnsupportedNode;
+            }
+            else {
+                net = (int32_t) ui[0];
+                node = (int16_t) ui[1];
+            }
+            break;
+        case 3: /* <command> token contains digit */
+            err = true;
+            break;
+        }
+        if (err) break;
+
+        /* command is case insensitive */
+        convertToLower(tok, sizeof(tok));
+
+        /* set command - multiple sub commands */
+        if (strcmp(tok, "set") == 0) {
+            if (closed != 0) {
+                err = true;
+                break;
+            }
+
+            /* command 2 */
+            closed = -1;
+            CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
+            if (err) break;
+
+            convertToLower(tok, sizeof(tok));
+            /* 'set network <value>' */
+            if (strcmp(tok, "network") == 0) {
+                uint16_t value;
+
+                if (closed != 0) {
+                    err = true;
+                    break;
+                }
+
+                /* value */
+                closed = 1;
+                CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok),
+                                  &closed, &err);
+                value = (uint16_t)getU32(tok, CO_CONFIG_GTW_NET_MIN,
+                                         CO_CONFIG_GTW_NET_MAX, &err);
+                if (err) break;
+
+                gtwa->net_default = value;
+                responseWithOK(gtwa);
+            }
+            /* 'set node <value>' */
+            else if (strcmp(tok, "node") == 0) {
+                bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
+                uint8_t value;
+
+                if (closed != 0 || NodeErr) {
+                    err = true;
+                    break;
+                }
+
+                /* value */
+                closed = 1;
+                CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok),
+                                  &closed, &err);
+                value = (uint8_t)getU32(tok, 1, 127, &err);
+                if (err) break;
+
+                gtwa->node_default = value;
+                responseWithOK(gtwa);
+            }
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO
+            /* 'set sdo_timeout <value_ms>' */
+            else if (strcmp(tok, "sdo_timeout") == 0) {
+                bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
+                uint16_t value;
+
+                if (closed != 0 || NodeErr) {
+                    err = true;
+                    break;
+                }
+
+                /* value */
+                closed = 1;
+                CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok),
+                                  &closed, &err);
+                value = (uint16_t)getU32(tok, 1, 0xFFFF, &err);
+                if (err) break;
+
+                gtwa->SDOtimeoutTime = value;
+                responseWithOK(gtwa);
+            }
+            /* 'set sdo_timeout <0|1>' */
+            else if (strcmp(tok, "sdo_block") == 0) {
+                bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
+                uint16_t value;
+
+                if (closed != 0 || NodeErr) {
+                    err = true;
+                    break;
+                }
+
+                /* value */
+                closed = 1;
+                CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok),
+                                  &closed, &err);
+                value = (uint16_t)getU32(tok, 0, 1, &err);
+                if (err) break;
+
+                gtwa->SDOblockTransferEnable = value==1 ? true : false;
+                responseWithOK(gtwa);
+            }
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
+            else {
+                respErrorCode = CO_GTWA_respErrorReqNotSupported;
+                err = true;
+                break;
+            }
+        }
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO
+        /* Upload SDO command - 'r[ead] <index> <subindex> <datatype>' */
+        else if (strcmp(tok, "r") == 0 || strcmp(tok, "read") == 0) {
+            uint16_t idx;
+            uint8_t subidx;
+            CO_SDO_return_t SDO_ret;
+            bool_t NodeErr = checkNetNode(gtwa, net, node, 1, &respErrorCode);
+
+            if (closed != 0 || NodeErr) {
+                err = true;
+                break;
+            }
+
+            /* index */
+            closed = 0;
+            CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
+            idx = (uint16_t)getU32(tok, 0, 0xFFFF, &err);
+            if (err) break;
+
+            /* subindex */
+            closed = -1;
+            n = CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok),
+                                  &closed, &err);
+            subidx = (uint8_t)getU32(tok, 0, 0xFF, &err);
+            if (err || n == 0) {
+                err = true;
+                break;
+            }
+
+            /* optional data type */
+            if (closed == 0) {
+                closed = 1;
+                CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok),
+                                  &closed, &err);
+                convertToLower(tok, sizeof(tok));
+                gtwa->SDOdataType = CO_GTWA_getDataType(tok, &err);
+                if (err) break;
+            }
+            else {
+                gtwa->SDOdataType = &dataTypes[0]; /* use generic data type */
+            }
+
+            /* setup client */
+            SDO_ret = CO_SDOclient_setup(gtwa->SDO_C,
+                                         CO_CAN_ID_SDO_CLI + gtwa->node,
+                                         CO_CAN_ID_SDO_SRV + gtwa->node,
+                                         gtwa->node);
+            if (SDO_ret != CO_SDO_RT_ok_communicationEnd) {
+                respErrorCode = CO_GTWA_respErrorInternalState;
+                err = true;
+                break;
+            }
+
+            /* initiate upload */
+            SDO_ret = CO_SDOclientUploadInitiate(gtwa->SDO_C, idx, subidx,
+                                                 gtwa->SDOtimeoutTime,
+                                                 gtwa->SDOblockTransferEnable);
+            if (SDO_ret != CO_SDO_RT_ok_communicationEnd) {
+                respErrorCode = CO_GTWA_respErrorInternalState;
+                err = true;
+                break;
+            }
+
+            /* indicate that gateway response didn't start yet */
+            gtwa->SDOdataCopyStatus = false;
+            /* continue with state machine */
+            timeDifference_us = 0;
+            gtwa->state = CO_GTWA_ST_READ;
+        }
+
+        /* Download SDO comm. - w[rite] <index> <subindex> <datatype> <value> */
+        else if (strcmp(tok, "w") == 0 || strcmp(tok, "write") == 0) {
+            uint16_t idx;
+            uint8_t subidx;
+            CO_fifo_st status;
+            CO_SDO_return_t SDO_ret;
+            size_t size;
+            bool_t NodeErr = checkNetNode(gtwa, net, node, 1, &respErrorCode);
+
+            if (closed != 0 || NodeErr) {
+                err = true;
+                break;
+            }
+
+            /* index */
+            closed = 0;
+            CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
+            idx = (uint16_t)getU32(tok, 0, 0xFFFF, &err);
+            if (err) break;
+
+            /* subindex */
+            closed = 0;
+            n = CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok),
+                                  &closed, &err);
+            subidx = (uint8_t)getU32(tok, 0, 0xFF, &err);
+            if (err) break;
+
+            /* data type */
+            closed = 0;
+            CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok),
+                              &closed, &err);
+            convertToLower(tok, sizeof(tok));
+            gtwa->SDOdataType = CO_GTWA_getDataType(tok, &err);
+            if (err) break;
+
+            /* setup client */
+            SDO_ret = CO_SDOclient_setup(gtwa->SDO_C,
+                                         CO_CAN_ID_SDO_CLI + gtwa->node,
+                                         CO_CAN_ID_SDO_SRV + gtwa->node,
+                                         gtwa->node);
+            if (SDO_ret != CO_SDO_RT_ok_communicationEnd) {
+                respErrorCode = CO_GTWA_respErrorInternalState;
+                err = true;
+                break;
+            }
+
+            /* initiate download */
+            SDO_ret =
+            CO_SDOclientDownloadInitiate(gtwa->SDO_C, idx, subidx,
+                                         gtwa->SDOdataType->length,
+                                         gtwa->SDOtimeoutTime,
+                                         gtwa->SDOblockTransferEnable);
+            if (SDO_ret != CO_SDO_RT_ok_communicationEnd) {
+                respErrorCode = CO_GTWA_respErrorInternalState;
+                err = true;
+                break;
+            }
+
+            /* copy data from comm to the SDO buffer, according to data type */
+            size = gtwa->SDOdataType->dataTypeScan(&gtwa->SDO_C->bufFifo,
+                                                   &gtwa->commFifo,
+                                                   &status);
+            /* set to true, if command delimiter was found */
+            closed = ((status & CO_fifo_st_closed) == 0) ? 0 : 1;
+            /* set to true, if data are copied only partially */
+            gtwa->SDOdataCopyStatus = (status & CO_fifo_st_partial) != 0;
+
+            /* is syntax error in command or size is zero or not the last token
+             * in command */
+            if ((status & CO_fifo_st_errMask) != 0 || size == 0
+                || (gtwa->SDOdataCopyStatus == false && closed != 1)
+            ) {
+                err = true;
+                break;
+            }
+
+            /* if data size was not known before and is known now, update SDO */
+            if (gtwa->SDOdataType->length == 0 && !gtwa->SDOdataCopyStatus) {
+                CO_SDOclientDownloadInitiateSize(gtwa->SDO_C, size);
+            }
+
+            /* continue with state machine */
+            gtwa->stateTimeoutTmr = 0;
+            timeDifference_us = 0;
+            gtwa->state = CO_GTWA_ST_WRITE;
+        }
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_NMT
+        /* NMT start node - 'start' */
+        else if (strcmp(tok, "start") == 0) {
+            CO_ReturnError_t ret;
+            bool_t NodeErr = checkNetNode(gtwa, net, node, 0, &respErrorCode);
+            CO_NMT_command_t command2 = CO_NMT_ENTER_OPERATIONAL;
+
+            if (closed != 1 || NodeErr) {
+                err = true;
+                break;
+            }
+            ret = CO_NMT_sendCommand(gtwa->NMT, command2, gtwa->node);
+
+            if (ret == CO_ERROR_NO) {
+                responseWithOK(gtwa);
+            }
+            else {
+                respErrorCode = CO_GTWA_respErrorInternalState;
+                err = true;
+                break;
+            }
+        }
+
+        /* NMT stop node - 'stop' */
+        else if (strcmp(tok, "stop") == 0) {
+            CO_ReturnError_t ret;
+            bool_t NodeErr = checkNetNode(gtwa, net, node, 0, &respErrorCode);
+            CO_NMT_command_t command2 = CO_NMT_ENTER_STOPPED;
+
+            if (closed != 1 || NodeErr) {
+                err = true;
+                break;
+            }
+            ret = CO_NMT_sendCommand(gtwa->NMT, command2, gtwa->node);
+
+            if (ret == CO_ERROR_NO) {
+                responseWithOK(gtwa);
+            }
+            else {
+                respErrorCode = CO_GTWA_respErrorInternalState;
+                err = true;
+                break;
+            }
+        }
+
+        /* NMT Set node to pre-operational - 'preop[erational]' */
+        else if (strcmp(tok, "preop") == 0 ||
+                 strcmp(tok, "preoperational") == 0
+        ) {
+            CO_ReturnError_t ret;
+            bool_t NodeErr = checkNetNode(gtwa, net, node, 0, &respErrorCode);
+            CO_NMT_command_t command2 = CO_NMT_ENTER_PRE_OPERATIONAL;
+
+            if (closed != 1 || NodeErr) {
+                err = true;
+                break;
+            }
+            ret = CO_NMT_sendCommand(gtwa->NMT, command2, gtwa->node);
+
+            if (ret == CO_ERROR_NO) {
+                responseWithOK(gtwa);
+            }
+            else {
+                respErrorCode = CO_GTWA_respErrorInternalState;
+                err = true;
+                break;
+            }
+        }
+
+        /* NMT reset (node or communication) - 'reset <node|comm[unication]>'*/
+        else if (strcmp(tok, "reset") == 0) {
+            CO_ReturnError_t ret;
+            bool_t NodeErr = checkNetNode(gtwa, net, node, 0, &respErrorCode);
+            CO_NMT_command_t command2;
+
+            if (closed != 0 || NodeErr) {
+                err = true;
+                break;
+            }
+
+            /* command 2 */
+            closed = 1;
+            CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
+            if (err) break;
+
+            convertToLower(tok, sizeof(tok));
+            if (strcmp(tok, "node") == 0) {
+                command2 = CO_NMT_RESET_NODE;
+            } else if (strcmp(tok, "comm") == 0 ||
+                       strcmp(tok, "communication") == 0
+            ) {
+                command2 = CO_NMT_RESET_COMMUNICATION;
+            } else {
+                err = true;
+                break;
+            }
+
+            ret = CO_NMT_sendCommand(gtwa->NMT, command2, gtwa->node);
+
+            if (ret == CO_ERROR_NO) {
+                responseWithOK(gtwa);
+            }
+            else {
+                respErrorCode = CO_GTWA_respErrorInternalState;
+                err = true;
+                break;
+            }
+        }
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_NMT */
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS
+        /* Switch state global command - 'lss_switch_glob <0|1>' */
+        else if (strcmp(tok, "lss_switch_glob") == 0) {
+            bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
+            uint8_t select;
+
+            if (closed != 0 || NodeErr) {
+                err = true;
+                break;
+            }
+
+            /* get value */
+            closed = 1;
+            CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
+            select = (uint8_t)getU32(tok, 0, 1, &err);
+            if (err) break;
+
+            if (select == 0) {
+                /* send non-confirmed message */
+                CO_LSSmaster_return_t ret;
+                ret = CO_LSSmaster_switchStateDeselect(gtwa->LSSmaster);
+                if (ret == CO_LSSmaster_OK) {
+                    responseWithOK(gtwa);
+                }
+                else {
+                    respErrorCode = CO_GTWA_respErrorInternalState;
+                    err = true;
+                    break;
+                }
+            }
+            else {
+                /* continue with state machine */
+                gtwa->state = CO_GTWA_ST_LSS_SWITCH_GLOB;
+            }
+        }
+        /* Switch state selective command -
+         * 'lss_switch_sel <vendorID> <product code> <revisionNo> <serialNo>' */
+        else if (strcmp(tok, "lss_switch_sel") == 0) {
+            bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
+            CO_LSS_address_t *addr = &gtwa->lssAddress;
+
+            if (closed != 0 || NodeErr) {
+                err = true;
+                break;
+            }
+
+            /* get values */
+            closed = 0;
+            CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
+            addr->identity.vendorID = getU32(tok, 0, 0xFFFFFFFF, &err);
+            if (err) break;
+
+            CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
+            addr->identity.productCode = getU32(tok, 0, 0xFFFFFFFF, &err);
+            if (err) break;
+
+            CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
+            addr->identity.revisionNumber = getU32(tok, 0, 0xFFFFFFFF, &err);
+            if (err) break;
+
+            closed = 1;
+            CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
+            addr->identity.serialNumber = getU32(tok, 0, 0xFFFFFFFF, &err);
+            if (err) break;
+
+            /* continue with state machine */
+            gtwa->state = CO_GTWA_ST_LSS_SWITCH_SEL;
+        }
+        /* LSS configure node-ID command - 'lss_set_node <node>' */
+        else if (strcmp(tok, "lss_set_node") == 0) {
+            bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
+
+            if (closed != 0 || NodeErr) {
+                err = true;
+                break;
+            }
+
+            /* get value */
+            closed = 1;
+            CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
+            gtwa->lssNID = (uint8_t)getU32(tok, 0, 0xFF, &err);
+            if (gtwa->lssNID > 0x7F && gtwa->lssNID < 0xFF) err = true;
+            if (err) break;
+
+            /* continue with state machine */
+            gtwa->state = CO_GTWA_ST_LSS_SET_NODE;
+        }
+        /* LSS configure bit-rate command -
+         * 'lss_conf_bitrate <table_selector=0> <table_index>'
+         * table_index: 0=1000 kbit/s, 1=800 kbit/s, 2=500 kbit/s, 3=250 kbit/s,
+         *   4=125 kbit/s, 6=50 kbit/s, 7=20 kbit/s, 8=10 kbit/s, 9=auto */
+        else if (strcmp(tok, "lss_conf_bitrate") == 0) {
+            bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
+            uint8_t tableIndex;
+            int maxIndex = (sizeof(CO_LSS_bitTimingTableLookup) /
+                            sizeof(CO_LSS_bitTimingTableLookup[0])) - 1;
+
+            if (closed != 0 || NodeErr) {
+                err = true;
+                break;
+            }
+
+            /* First parameter is table selector. We only support the CiA
+             * bit timing table from CiA301 ("0") */
+            closed = 0;
+            CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
+            (void)getU32(tok, 0, 0, &err);
+
+            /* get value */
+            closed = 1;
+            CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
+            tableIndex = (uint8_t)getU32(tok, 0, maxIndex, &err);
+            if (tableIndex == 5) err = true;
+            if (err) break;
+            gtwa->lssBitrate = CO_LSS_bitTimingTableLookup[tableIndex];
+
+            /* continue with state machine */
+            gtwa->state = CO_GTWA_ST_LSS_CONF_BITRATE;
+        }
+        /* LSS activate new bit-rate command -
+         * 'lss_activate_bitrate <switch_delay_ms>' */
+        else if (strcmp(tok, "lss_activate_bitrate") == 0) {
+            bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
+            uint16_t switchDelay;
+            CO_LSSmaster_return_t ret;
+
+            if (closed != 0 || NodeErr) {
+                err = true;
+                break;
+            }
+
+            /* get value */
+            closed = 1;
+            CO_fifo_readToken(&gtwa->commFifo, tok, sizeof(tok), &closed, &err);
+            switchDelay = (uint16_t)getU32(tok, 0, 0xFFFF, &err);
+            if (err) break;
+
+            /* send non-confirmed message */
+            ret = CO_LSSmaster_ActivateBit(gtwa->LSSmaster, switchDelay);
+            if (ret == CO_LSSmaster_OK) {
+                responseWithOK(gtwa);
+            }
+            else {
+                respErrorCode = CO_GTWA_respErrorInternalState;
+                err = true;
+                break;
+            }
+        }
+        /* LSS store configuration command - 'lss_store' */
+        else if (strcmp(tok, "lss_store") == 0) {
+            bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
+
+            if (closed != 1 || NodeErr) {
+                err = true;
+                break;
+            }
+
+            /* continue with state machine */
+            gtwa->state = CO_GTWA_ST_LSS_STORE;
+        }
+        /* Inquire LSS address command - 'lss_inquire_addr [<LSSSUB=0..3>]' */
+        else if (strcmp(tok, "lss_inquire_addr") == 0) {
+            bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
+
+            if (NodeErr) {
+                err = true;
+                break;
+            }
+
+            if (closed == 0) {
+                uint8_t lsssub;
+                /* get value */
+                closed = 1;
+                CO_fifo_readToken(&gtwa->commFifo,tok,sizeof(tok),&closed,&err);
+                lsssub = (uint8_t)getU32(tok, 0, 3, &err);
+                if (err) break;
+                switch (lsssub) {
+                    case 0: gtwa->lssInquireCs = CO_LSS_INQUIRE_VENDOR; break;
+                    case 1: gtwa->lssInquireCs = CO_LSS_INQUIRE_PRODUCT; break;
+                    case 2: gtwa->lssInquireCs = CO_LSS_INQUIRE_REV; break;
+                    default: gtwa->lssInquireCs = CO_LSS_INQUIRE_SERIAL; break;
+                }
+
+                /* continue with state machine */
+                gtwa->state = CO_GTWA_ST_LSS_INQUIRE;
+            }
+            else {
+                /* continue with state machine */
+                gtwa->state = CO_GTWA_ST_LSS_INQUIRE_ADDR_ALL;
+            }
+        }
+        /* LSS inquire node-ID command - 'lss_get_node'*/
+        else if (strcmp(tok, "lss_get_node") == 0) {
+            bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
+
+            if (closed != 1 || NodeErr) {
+                err = true;
+                break;
+            }
+
+            /* continue with state machine */
+            gtwa->lssInquireCs = CO_LSS_INQUIRE_NODE_ID;
+            gtwa->state = CO_GTWA_ST_LSS_INQUIRE;
+        }
+        /* LSS identify fastscan. This is a manufacturer specific command as
+         * the one in DSP309 is quite useless - '_lss_fastscan [<timeout_ms>]'*/
+        else if (strcmp(tok, "_lss_fastscan") == 0) {
+            bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
+            uint16_t timeout_ms = 0;
+
+            if (NodeErr) {
+                err = true;
+                break;
+            }
+
+            if (closed == 0) {
+                /* get value */
+                closed = 1;
+                CO_fifo_readToken(&gtwa->commFifo,tok,sizeof(tok),&closed,&err);
+                timeout_ms = (uint16_t)getU32(tok, 0, 0xFFFF, &err);
+                if (err) break;
+            }
+
+            /* If timeout not specified, use 100ms. Should work in most cases */
+            if (timeout_ms == 0) {
+                timeout_ms = 100;
+            }
+            CO_LSSmaster_changeTimeout(gtwa->LSSmaster, timeout_ms);
+
+            /* prepare lssFastscan, all zero */
+            memset(&gtwa->lssFastscan, 0, sizeof(gtwa->lssFastscan));
+
+            /* continue with state machine */
+            gtwa->state = CO_GTWA_ST__LSS_FASTSCAN;
+        }
+        /* LSS complete node-ID configuration command - 'lss_allnodes
+         * [<timeout_ms> [<nodeStart=1..127> <store=0|1>
+         * <scanType0=0..2> <vendorId> <scanType1=0..2> <productCode>
+         * <scanType2=0..2> <revisionNo> <scanType3=0..2> <serialNo>]]' */
+        else if (strcmp(tok, "lss_allnodes") == 0) {
+            /* Request node enumeration by LSS identify fastscan.
+             * This initiates node enumeration by the means of LSS fastscan
+             * mechanism. When this function is finished:
+             * - All nodes that match the given criteria are assigned a node ID
+             *   beginning with nodeId. If 127 is reached, the process
+             *   is stopped, no matter if there are nodes remaining or not.
+             * - No IDs are assigned because:
+             *   - the given criteria do not match any node,
+             *   - all nodes are already configured.
+             * This function needs that no node is selected when starting the
+             * scan process. */
+            bool_t NodeErr = checkNet(gtwa, net, &respErrorCode);
+            uint16_t timeout_ms = 0;
+
+            if (NodeErr) {
+                err = true;
+                break;
+            }
+
+            if (closed == 0) {
+                /* get optional token timeout (non standard) */
+                closed = -1;
+                CO_fifo_readToken(&gtwa->commFifo,tok,sizeof(tok),&closed,&err);
+                timeout_ms = (uint16_t)getU32(tok, 0, 0xFFFF, &err);
+                if (err) break;
+
+            }
+            /* If timeout not specified, use 100ms. Should work in most cases */
+            gtwa->lssTimeout_ms = timeout_ms == 0 ? 100 : timeout_ms;
+            CO_LSSmaster_changeTimeout(gtwa->LSSmaster, gtwa->lssTimeout_ms);
+            gtwa->lssNodeCount = 0;
+            gtwa->lssSubState = 0;
+
+            if (closed == 1) {
+                /* No other arguments, as by CiA specification for this command.
+                 * Do full scan. */
+                /* use start node ID 2. Should work in most cases */
+                gtwa->lssNID = 2;
+                /* store node ID in node's NVM */
+                gtwa->lssStore = true;
+                /* prepare lssFastscan, all zero */
+                memset(&gtwa->lssFastscan, 0, sizeof(gtwa->lssFastscan));
+            }
+            if (closed == 0) {
+                /* more arguments follow */
+                CO_fifo_readToken(&gtwa->commFifo,tok,sizeof(tok),&closed,&err);
+                gtwa->lssNID = getU32(tok, 1, 127, &err);
+                if (err) break;
+
+                closed = -1;
+                CO_fifo_readToken(&gtwa->commFifo,tok,sizeof(tok),&closed,&err);
+                gtwa->lssStore = (bool_t)getU32(tok, 0, 1, &err);
+                if (err) break;
+
+                if (closed == 1) {
+                    /* No other arguments, prepare lssFastscan, all zero */
+                    memset(&gtwa->lssFastscan, 0, sizeof(gtwa->lssFastscan));
+                }
+            }
+            if (closed == 0) {
+                /* more arguments follow */
+                CO_LSSmaster_fastscan_t *fs = &gtwa->lssFastscan;
+
+                CO_fifo_readToken(&gtwa->commFifo,tok,sizeof(tok),&closed,&err);
+                fs->scan[CO_LSS_FASTSCAN_VENDOR_ID] = getU32(tok, 0, 2, &err);
+                if (err) break;
+
+                CO_fifo_readToken(&gtwa->commFifo,tok,sizeof(tok),&closed,&err);
+                fs->match.identity.vendorID = getU32(tok, 0, 0xFFFFFFFF, &err);
+                if (err) break;
+
+                CO_fifo_readToken(&gtwa->commFifo,tok,sizeof(tok),&closed,&err);
+                fs->scan[CO_LSS_FASTSCAN_PRODUCT] = getU32(tok, 0, 2, &err);
+                if (err) break;
+
+                CO_fifo_readToken(&gtwa->commFifo,tok,sizeof(tok),&closed,&err);
+                fs->match.identity.productCode = getU32(tok,0,0xFFFFFFFF, &err);
+                if (err) break;
+
+                CO_fifo_readToken(&gtwa->commFifo,tok,sizeof(tok),&closed,&err);
+                fs->scan[CO_LSS_FASTSCAN_REV] = getU32(tok, 0, 2, &err);
+                if (err) break;
+
+                CO_fifo_readToken(&gtwa->commFifo,tok,sizeof(tok),&closed,&err);
+                fs->match.identity.revisionNumber=getU32(tok,0,0xFFFFFFFF,&err);
+                if (err) break;
+
+                CO_fifo_readToken(&gtwa->commFifo,tok,sizeof(tok),&closed,&err);
+                fs->scan[CO_LSS_FASTSCAN_SERIAL] = getU32(tok, 0, 2, &err);
+                if (err) break;
+
+                closed = 1;
+                CO_fifo_readToken(&gtwa->commFifo,tok,sizeof(tok),&closed,&err);
+                fs->match.identity.serialNumber = getU32(tok,0,0xFFFFFFFF,&err);
+                if (err) break;
+            }
+
+            /* continue with state machine */
+            gtwa->state = CO_GTWA_ST_LSS_ALLNODES;
+        }
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS */
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG
+        /* Print message log */
+        else if (strcmp(tok, "log") == 0) {
+            if (closed == 0) {
+                err = true;
+                break;
+            }
+            gtwa->state = CO_GTWA_ST_LOG;
+        }
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG */
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_HELP
+        /* Print help */
+        else if (strcmp(tok, "help") == 0) {
+            if (closed == 1) {
+                gtwa->helpString = CO_GTWA_helpString;
+            }
+            else {
+                /* get second token */
+                closed = 1;
+                CO_fifo_readToken(&gtwa->commFifo,tok,sizeof(tok),&closed,&err);
+                if (err) break;
+
+                convertToLower(tok, sizeof(tok));
+                if (strcmp(tok, "datatype") == 0) {
+                    gtwa->helpString = CO_GTWA_helpStringDatatypes;
+                }
+                else if (strcmp(tok, "lss") == 0) {
+                    gtwa->helpString = CO_GTWA_helpStringLss;
+                }
+                else {
+                    err = true;
+                    break;
+                }
+            }
+            /* continue with state machine */
+            gtwa->helpStringOffset = 0;
+            gtwa->state = CO_GTWA_ST_HELP;
+        }
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_HELP */
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS
+        /* Print status led diodes */
+        else if (strcmp(tok, "led") == 0) {
+            if (closed == 0) {
+                err = true;
+                break;
+            }
+            gtwa->ledStringPreviousIndex = 0xFF;
+            gtwa->state = CO_GTWA_ST_LED;
+        }
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS */
+
+        /* Unrecognized command */
+        else {
+            respErrorCode = CO_GTWA_respErrorReqNotSupported;
+            err = true;
+            break;
+        }
+    } /* while CO_GTWA_ST_IDLE && CO_fifo_CommSearch */
+
+
+
+    /***************************************************************************
+    * STATE MACHINE
+    ***************************************************************************/
+    /* If error, generate error response */
+    if (err) {
+        if (respErrorCode == CO_GTWA_respErrorNone) {
+            respErrorCode = CO_GTWA_respErrorSyntax;
+        }
+        responseWithError(gtwa, respErrorCode);
+
+        /* delete command, if it was only partially read */
+        if (closed == 0) {
+            CO_fifo_CommSearch(&gtwa->commFifo, true);
+        }
+        gtwa->state = CO_GTWA_ST_IDLE;
+    }
+
+    else switch (gtwa->state) {
+    case CO_GTWA_ST_IDLE: {
+        return; /* skip timerNext_us calculation */
+    }
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO
+    /* SDO upload state */
+    case CO_GTWA_ST_READ: {
+        CO_SDO_abortCode_t abortCode;
+        size_t sizeTransferred;
+        CO_SDO_return_t ret;
+
+        ret = CO_SDOclientUpload(gtwa->SDO_C,
+                                 timeDifference_us,
+                                 false,
+                                 &abortCode,
+                                 NULL,
+                                 &sizeTransferred,
+                                 timerNext_us);
+
+        if (ret < 0) {
+            responseWithErrorSDO(gtwa, abortCode, gtwa->SDOdataCopyStatus);
+            gtwa->state = CO_GTWA_ST_IDLE;
+        }
+        /* Response data must be read, partially or whole */
+        else if (ret == CO_SDO_RT_uploadDataBufferFull
+                 || ret == CO_SDO_RT_ok_communicationEnd
+        ) {
+            size_t fifoRemain;
+
+            /* write response head first */
+            if (!gtwa->SDOdataCopyStatus) {
+                gtwa->respBufCount = snprintf(gtwa->respBuf,
+                                              CO_GTWA_RESP_BUF_SIZE - 2,
+                                              "[%"PRId32"] ",
+                                              gtwa->sequence);
+                gtwa->SDOdataCopyStatus = true;
+            }
+
+            /* Empty SDO fifo buffer in multiple cycles. Repeat until
+             * application runs out of space (respHold) or fifo empty. */
+            do {
+                /* read SDO fifo (partially) and print specific data type as
+                 * ascii into intermediate respBuf */
+                gtwa->respBufCount += gtwa->SDOdataType->dataTypePrint(
+                    &gtwa->SDO_C->bufFifo,
+                    &gtwa->respBuf[gtwa->respBufCount],
+                    CO_GTWA_RESP_BUF_SIZE - 2 - gtwa->respBufCount,
+                    ret == CO_SDO_RT_ok_communicationEnd);
+                fifoRemain = CO_fifo_getOccupied(&gtwa->SDO_C->bufFifo);
+
+                /* end of communication, print newline and enter idle state */
+                if (ret == CO_SDO_RT_ok_communicationEnd && fifoRemain == 0) {
+                    gtwa->respBufCount +=
+                        sprintf(&gtwa->respBuf[gtwa->respBufCount], "\r\n");
+                    gtwa->state = CO_GTWA_ST_IDLE;
+                }
+
+                /* transfer response to the application */
+                if (respBufTransfer(gtwa) == false) {
+                    /* broken communication, send SDO abort and force finish. */
+                    abortCode = CO_SDO_AB_DATA_TRANSF;
+                    CO_SDOclientUpload(gtwa->SDO_C,
+                                       0,
+                                       true,
+                                       &abortCode,
+                                       NULL,
+                                       NULL,
+                                       NULL);
+                    gtwa->state = CO_GTWA_ST_IDLE;
+                    break;
+                }
+            } while (gtwa->respHold == false && fifoRemain > 0);
+        }
+        break;
+    }
+
+    /* SDO download state */
+    case CO_GTWA_ST_WRITE:
+    case CO_GTWA_ST_WRITE_ABORTED: {
+        CO_SDO_abortCode_t abortCode;
+        size_t sizeTransferred;
+        bool_t abort = false;
+        bool_t hold = false;
+        CO_SDO_return_t ret;
+
+        /* copy data to the SDO buffer if previous dataTypeScan was partial */
+        if (gtwa->SDOdataCopyStatus) {
+            CO_fifo_st status;
+            gtwa->SDOdataType->dataTypeScan(&gtwa->SDO_C->bufFifo,
+                                            &gtwa->commFifo,
+                                            &status);
+            /* set to true, if command delimiter was found */
+            closed = ((status & CO_fifo_st_closed) == 0) ? 0 : 1;
+            /* set to true, if data are copied only partially */
+            gtwa->SDOdataCopyStatus = (status & CO_fifo_st_partial) != 0;
+
+            /* is syntax error in command or not the last token in command */
+            if ((status & CO_fifo_st_errMask) != 0
+                || (gtwa->SDOdataCopyStatus == false && closed != 1)
+            ) {
+                abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
+                abort = true; /* abort SDO communication */
+                /* clear the rest of the command, if necessary */
+                if (closed != 1)
+                    CO_fifo_CommSearch(&gtwa->commFifo, true);
+            }
+            if (gtwa->state == CO_GTWA_ST_WRITE_ABORTED) {
+                /* Stay in this state, until all data transferred via commFifo
+                 * will be purged. */
+                if (!CO_fifo_purge(&gtwa->SDO_C->bufFifo) || closed == 1) {
+                    gtwa->state = CO_GTWA_ST_IDLE;
+                }
+                break;
+            }
+        }
+        /* If not all data were transferred, make sure, there is enough data in
+         * SDO buffer, to continue communication. Otherwise wait and check for
+         * timeout */
+        if (gtwa->SDOdataCopyStatus
+            && CO_fifo_getOccupied(&gtwa->SDO_C->bufFifo) <
+               (CO_CONFIG_GTW_BLOCK_DL_LOOP * 7)
+        ) {
+            if (gtwa->stateTimeoutTmr > CO_GTWA_STATE_TIMEOUT_TIME_US) {
+                abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
+                abort = true;
+            }
+            else {
+                gtwa->stateTimeoutTmr += timeDifference_us;
+                hold = true;
+            }
+        }
+        if (!hold || abort) {
+            /* if OS has CANtx queue, speedup block transfer */
+            int loop = 0;
+            do {
+                ret = CO_SDOclientDownload(gtwa->SDO_C,
+                                           timeDifference_us,
+                                           abort,
+                                           gtwa->SDOdataCopyStatus,
+                                           &abortCode,
+                                           &sizeTransferred,
+                                           timerNext_us);
+                if (++loop >= CO_CONFIG_GTW_BLOCK_DL_LOOP) {
+                    break;
+                }
+            } while (ret == CO_SDO_RT_blockDownldInProgress);
+
+            /* send response in case of error or finish */
+            if (ret < 0) {
+                responseWithErrorSDO(gtwa, abortCode, false);
+                /* purge remaining data if necessary */
+                gtwa->state = gtwa->SDOdataCopyStatus
+                              ? CO_GTWA_ST_WRITE_ABORTED
+                              : CO_GTWA_ST_IDLE;
+            }
+            else if (ret == CO_SDO_RT_ok_communicationEnd) {
+                responseWithOK(gtwa);
+                gtwa->state = CO_GTWA_ST_IDLE;
+            }
+        }
+        break;
+    }
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS
+    case CO_GTWA_ST_LSS_SWITCH_GLOB: {
+        CO_LSSmaster_return_t ret;
+        ret = CO_LSSmaster_switchStateSelect(gtwa->LSSmaster,
+                                             timeDifference_us,
+                                             NULL);
+        if (ret != CO_LSSmaster_WAIT_SLAVE) {
+            responseLSS(gtwa, ret);
+            gtwa->state = CO_GTWA_ST_IDLE;
+        }
+        break;
+    }
+    case CO_GTWA_ST_LSS_SWITCH_SEL: {
+        CO_LSSmaster_return_t ret;
+        ret = CO_LSSmaster_switchStateSelect(gtwa->LSSmaster,
+                                             timeDifference_us,
+                                             &gtwa->lssAddress);
+        if (ret != CO_LSSmaster_WAIT_SLAVE) {
+            responseLSS(gtwa, ret);
+            gtwa->state = CO_GTWA_ST_IDLE;
+        }
+        break;
+    }
+    case CO_GTWA_ST_LSS_SET_NODE: {
+        CO_LSSmaster_return_t ret;
+        ret = CO_LSSmaster_configureNodeId(gtwa->LSSmaster,
+                                           timeDifference_us,
+                                           gtwa->lssNID);
+        if (ret != CO_LSSmaster_WAIT_SLAVE) {
+            if (ret == CO_LSSmaster_OK_ILLEGAL_ARGUMENT) {
+                respErrorCode = CO_GTWA_respErrorLSSnodeIdNotSupported;
+                responseWithError(gtwa, respErrorCode);
+            }
+            else {
+                responseLSS(gtwa, ret);
+            }
+            gtwa->state = CO_GTWA_ST_IDLE;
+        }
+        break;
+    }
+    case CO_GTWA_ST_LSS_CONF_BITRATE: {
+        CO_LSSmaster_return_t ret;
+        ret = CO_LSSmaster_configureBitTiming(gtwa->LSSmaster,
+                                              timeDifference_us,
+                                              gtwa->lssBitrate);
+        if (ret != CO_LSSmaster_WAIT_SLAVE) {
+            if (ret == CO_LSSmaster_OK_ILLEGAL_ARGUMENT) {
+                respErrorCode = CO_GTWA_respErrorLSSbitRateNotSupported;
+                responseWithError(gtwa, respErrorCode);
+            }
+            else {
+                responseLSS(gtwa, ret);
+            }
+            gtwa->state = CO_GTWA_ST_IDLE;
+        }
+        break;
+    }
+    case CO_GTWA_ST_LSS_STORE: {
+        CO_LSSmaster_return_t ret;
+
+        ret = CO_LSSmaster_configureStore(gtwa->LSSmaster, timeDifference_us);
+        if (ret != CO_LSSmaster_WAIT_SLAVE) {
+            if (ret == CO_LSSmaster_OK_ILLEGAL_ARGUMENT) {
+                respErrorCode = CO_GTWA_respErrorLSSparameterStoringFailed;
+                responseWithError(gtwa, respErrorCode);
+            }
+            else {
+                responseLSS(gtwa, ret);
+            }
+            gtwa->state = CO_GTWA_ST_IDLE;
+        }
+        break;
+    }
+    case CO_GTWA_ST_LSS_INQUIRE: {
+        CO_LSSmaster_return_t ret;
+        uint32_t value;
+
+        ret = CO_LSSmaster_Inquire(gtwa->LSSmaster, timeDifference_us,
+                                   gtwa->lssInquireCs, &value);
+        if (ret != CO_LSSmaster_WAIT_SLAVE) {
+            if (ret == CO_LSSmaster_OK) {
+                if (gtwa->lssInquireCs == CO_LSS_INQUIRE_NODE_ID) {
+                    gtwa->respBufCount =
+                        snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
+                                 "[%"PRId32"] 0x%02"PRIX32"\r\n",
+                                 gtwa->sequence, value & 0xFF);
+                } else {
+                    gtwa->respBufCount =
+                        snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
+                                 "[%"PRId32"] 0x%08"PRIX32"\r\n",
+                                 gtwa->sequence, value);
+                }
+                respBufTransfer(gtwa);
+            }
+            else {
+                responseLSS(gtwa, ret);
+            }
+            gtwa->state = CO_GTWA_ST_IDLE;
+        }
+        break;
+    }
+    case CO_GTWA_ST_LSS_INQUIRE_ADDR_ALL: {
+        CO_LSSmaster_return_t ret;
+
+        ret = CO_LSSmaster_InquireLssAddress(gtwa->LSSmaster, timeDifference_us,
+                                             &gtwa->lssAddress);
+        if (ret != CO_LSSmaster_WAIT_SLAVE) {
+            if (ret == CO_LSSmaster_OK) {
+                gtwa->respBufCount =
+                    snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
+                             "[%"PRId32"] 0x%08"PRIX32" 0x%08"PRIX32 \
+                             " 0x%08"PRIX32" 0x%08"PRIX32"\r\n",
+                             gtwa->sequence,
+                             gtwa->lssAddress.identity.vendorID,
+                             gtwa->lssAddress.identity.productCode,
+                             gtwa->lssAddress.identity.revisionNumber,
+                             gtwa->lssAddress.identity.serialNumber);
+                respBufTransfer(gtwa);
+            }
+            else {
+                responseLSS(gtwa, ret);
+            }
+            gtwa->state = CO_GTWA_ST_IDLE;
+        }
+        break;
+    }
+    case CO_GTWA_ST__LSS_FASTSCAN: {
+        CO_LSSmaster_return_t ret;
+
+        ret = CO_LSSmaster_IdentifyFastscan(gtwa->LSSmaster, timeDifference_us,
+                                            &gtwa->lssFastscan);
+        if (ret != CO_LSSmaster_WAIT_SLAVE) {
+            if (ret == CO_LSSmaster_OK || ret == CO_LSSmaster_SCAN_FINISHED) {
+                gtwa->respBufCount =
+                    snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
+                             "[%"PRId32"] 0x%08"PRIX32" 0x%08"PRIX32 \
+                             " 0x%08"PRIX32" 0x%08"PRIX32"\r\n",
+                             gtwa->sequence,
+                             gtwa->lssFastscan.found.identity.vendorID,
+                             gtwa->lssFastscan.found.identity.productCode,
+                             gtwa->lssFastscan.found.identity.revisionNumber,
+                             gtwa->lssFastscan.found.identity.serialNumber);
+                respBufTransfer(gtwa);
+            }
+            else {
+                responseLSS(gtwa, ret);
+            }
+            CO_LSSmaster_changeTimeout(gtwa->LSSmaster,
+                                       CO_LSSmaster_DEFAULT_TIMEOUT);
+            gtwa->state = CO_GTWA_ST_IDLE;
+        }
+        break;
+    }
+    case CO_GTWA_ST_LSS_ALLNODES: {
+        CO_LSSmaster_return_t ret;
+        if (gtwa->lssSubState == 0) { /* _lss_fastscan */
+            ret = CO_LSSmaster_IdentifyFastscan(gtwa->LSSmaster,
+                                                timeDifference_us,
+                                                &gtwa->lssFastscan);
+            if (ret != CO_LSSmaster_WAIT_SLAVE) {
+                CO_LSSmaster_changeTimeout(gtwa->LSSmaster,
+                                           CO_LSSmaster_DEFAULT_TIMEOUT);
+
+                if (ret == CO_LSSmaster_OK || ret == CO_LSSmaster_SCAN_NOACK) {
+                    /* no (more) nodes found, send report sum and finish */
+                    gtwa->respBufCount =
+                        snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
+                                 "# Found %d nodes, search finished.\n" \
+                                 "[%"PRId32"] OK\r\n",
+                                 gtwa->lssNodeCount,
+                                 gtwa->sequence);
+                    respBufTransfer(gtwa);
+                    gtwa->state = CO_GTWA_ST_IDLE;
+                }
+                else if (ret == CO_LSSmaster_SCAN_FINISHED) {
+                    /* next sub-step */
+                    gtwa->lssSubState++;
+                }
+                else {
+                    /* error occurred */
+                    responseLSS(gtwa, ret);
+                    gtwa->state = CO_GTWA_ST_IDLE;
+                }
+            }
+        }
+        if (gtwa->lssSubState == 1) { /* lss_set_node */
+            ret = CO_LSSmaster_configureNodeId(gtwa->LSSmaster,
+                                               timeDifference_us,
+                                               gtwa->lssNID);
+            if (ret != CO_LSSmaster_WAIT_SLAVE) {
+                if (ret == CO_LSSmaster_OK) {
+                    /* next sub-step */
+                    gtwa->lssSubState += gtwa->lssStore ? 1 : 2;
+                }
+                else {
+                    /* error occurred */
+                    if (ret == CO_LSSmaster_OK_ILLEGAL_ARGUMENT) {
+                        respErrorCode = CO_GTWA_respErrorLSSnodeIdNotSupported;
+                        responseWithError(gtwa, respErrorCode);
+                    }
+                    else {
+                        responseLSS(gtwa, ret);
+                    }
+                    gtwa->state = CO_GTWA_ST_IDLE;
+                }
+            }
+        }
+        if (gtwa->lssSubState == 2) { /* lss_store */
+            ret = CO_LSSmaster_configureStore(gtwa->LSSmaster,
+                                              timeDifference_us);
+            if (ret != CO_LSSmaster_WAIT_SLAVE) {
+                if (ret == CO_LSSmaster_OK) {
+                    /* next sub-step */
+                    gtwa->lssSubState++;
+                }
+                else {
+                    /* error occurred */
+                    if (ret == CO_LSSmaster_OK_ILLEGAL_ARGUMENT) {
+                        respErrorCode =
+                            CO_GTWA_respErrorLSSparameterStoringFailed;
+                        responseWithError(gtwa, respErrorCode);
+                    }
+                    else {
+                        responseLSS(gtwa, ret);
+                    }
+                    gtwa->state = CO_GTWA_ST_IDLE;
+                }
+            }
+        }
+        if (gtwa->lssSubState >= 3) { /* lss_switch_glob 0 */
+            /* send non-confirmed message */
+            ret = CO_LSSmaster_switchStateDeselect(gtwa->LSSmaster);
+            if (ret != CO_LSSmaster_OK) {
+                /* error occurred */
+                responseLSS(gtwa, ret);
+                gtwa->state = CO_GTWA_ST_IDLE;
+            }
+            else {
+                /* cycle finished successfully, send report */
+                uint8_t lssNidAssigned = gtwa->lssNID;
+                const char msg2Fmt[] = "# Not all nodes scanned!\n" \
+                                       "[%"PRId32"] OK\r\n";
+                char msg2[sizeof(msg2Fmt)+10] = {0};
+
+                /* increment variables, check end-of-nodeId */
+                gtwa->lssNodeCount++;
+                if (gtwa->lssNID < 127) {
+                    /* repeat cycle with next node-id */
+                    gtwa->lssNID++;
+                    CO_LSSmaster_changeTimeout(gtwa->LSSmaster,
+                                               gtwa->lssTimeout_ms);
+                    gtwa->lssSubState = 0;
+                }
+                else {
+                    /* If we can't assign more node IDs, quit scanning */
+                    sprintf(msg2, msg2Fmt, gtwa->sequence);
+                    gtwa->state = CO_GTWA_ST_IDLE;
+                }
+
+                /* send report */
+                gtwa->respBufCount =
+                    snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
+                             "# Node-ID %d assigned to: 0x%08"PRIX32" 0x%08" \
+                             PRIX32" 0x%08"PRIX32" 0x%08"PRIX32"\n%s",
+                             lssNidAssigned,
+                             gtwa->lssFastscan.found.identity.vendorID,
+                             gtwa->lssFastscan.found.identity.productCode,
+                             gtwa->lssFastscan.found.identity.revisionNumber,
+                             gtwa->lssFastscan.found.identity.serialNumber,
+                             msg2);
+                respBufTransfer(gtwa);
+            }
+        }
+        break;
+    } /* CO_GTWA_ST_LSS_ALLNODES */
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS */
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG
+    /* print message log */
+    case CO_GTWA_ST_LOG: {
+        do {
+            gtwa->respBufCount = CO_fifo_read(&gtwa->logFifo, gtwa->respBuf,
+                                              CO_GTWA_RESP_BUF_SIZE, NULL);
+            respBufTransfer(gtwa);
+
+            if (CO_fifo_getOccupied(&gtwa->logFifo) == 0) {
+                gtwa->state = CO_GTWA_ST_IDLE;
+                break;
+            }
+        } while (gtwa->respHold == false);
+        break;
+    }
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG */
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_HELP
+    /* Print help string (in multiple segments if necessary) */
+    case CO_GTWA_ST_HELP: {
+        size_t lenBuf = CO_GTWA_RESP_BUF_SIZE;
+        size_t lenHelp = strlen(gtwa->helpString);
+
+        do {
+            size_t lenHelpRemain = lenHelp - gtwa->helpStringOffset;
+            size_t lenCopied = lenBuf < lenHelpRemain ? lenBuf : lenHelpRemain;
+
+            memcpy(gtwa->respBuf,
+                   &gtwa->helpString[gtwa->helpStringOffset],
+                   lenCopied);
+
+            gtwa->respBufCount = lenCopied;
+            gtwa->helpStringOffset += lenCopied;
+            respBufTransfer(gtwa);
+
+            if (gtwa->helpStringOffset == lenHelp) {
+                gtwa->state = CO_GTWA_ST_IDLE;
+                break;
+            }
+        } while (gtwa->respHold == false);
+        break;
+    }
+#endif
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS
+    /* print CANopen status LED diodes */
+    case CO_GTWA_ST_LED: {
+        uint8_t i;
+
+        if (CO_fifo_CommSearch(&gtwa->commFifo, false)) {
+            gtwa->state = CO_GTWA_ST_IDLE;
+            i = 4;
+        }
+        else {
+            i = CO_LED_RED(gtwa->LEDs, CO_LED_CANopen) * 2 +
+                CO_LED_GREEN(gtwa->LEDs, CO_LED_CANopen);
+        }
+        if (i > (CO_GTWA_LED_PRINTOUTS_SIZE - 1))
+            i = CO_GTWA_LED_PRINTOUTS_SIZE - 1;
+
+        if (i != gtwa->ledStringPreviousIndex) {
+            gtwa->respBufCount = snprintf(gtwa->respBuf, CO_GTWA_RESP_BUF_SIZE,
+                                          "%s", CO_GTWA_LED_PRINTOUTS[i]);
+            respBufTransfer(gtwa);
+            gtwa->ledStringPreviousIndex = i;
+        }
+        break;
+    }
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS */
+
+    /* illegal state */
+    default: {
+        respErrorCode = CO_GTWA_respErrorInternalState;
+        responseWithError(gtwa, respErrorCode);
+        gtwa->state = CO_GTWA_ST_IDLE;
+        break;
+    }
+    } /* switch (gtwa->state) */
+
+    /* execute next CANopen processing immediately, if idle and more commands
+     * available */
+    if (timerNext_us != NULL && gtwa->state == CO_GTWA_ST_IDLE
+        && CO_fifo_CommSearch(&gtwa->commFifo, false)
+    ) {
+        *timerNext_us = 0;
+    }
+}
+
+#endif  /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */

controller_yy_app/middleware/CANopenNode/309/CO_gateway_ascii.h

@@ -0,0 +1,538 @@
+/**
+ * CANopen access from other networks - ASCII mapping (CiA 309-3 DS v3.0.0)
+ *
+ * @file        CO_gateway_ascii.h
+ * @ingroup     CO_CANopen_309_3
+ * @author      Janez Paternoster
+ * @author      Martin Wagner
+ * @copyright   2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CO_GATEWAY_ASCII_H
+#define CO_GATEWAY_ASCII_H
+
+#include "301/CO_driver.h"
+#include "301/CO_fifo.h"
+#include "301/CO_SDOclient.h"
+#include "301/CO_NMT_Heartbeat.h"
+#include "305/CO_LSSmaster.h"
+#include "303/CO_LEDs.h"
+
+/* default configuration, see CO_config.h */
+#ifndef CO_CONFIG_GTW
+#define CO_CONFIG_GTW (0)
+#endif
+
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII) || defined CO_DOXYGEN
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_CANopen_309_3 Gateway ASCII mapping
+ * @ingroup CO_CANopen_309
+ * @{
+ *
+ * CANopen access from other networks - ASCII mapping (CiA 309-3 DSP v3.0.0)
+ *
+ * This module enables ascii command interface (CAN gateway), which can be used
+ * for master interaction with CANopen network. Some sort of string input/output
+ * stream can be used, for example serial port + terminal on microcontroller or
+ * stdio in OS or sockets, etc.
+ *
+ * For example, one wants to read 'Heartbeat producer time' parameter (0x1017,0)
+ * on remote node (with id=4). Parameter is 16-bit integer. He can can enter
+ * command string: `[1] 4 read 0x1017 0 i16`. CANopenNode will use SDO client,
+ * send request to remote node via CAN, wait for response via CAN and prints
+ * `[1] OK` to output stream on success.
+ *
+ * This module is usually initialized and processed in CANopen.c file.
+ * Application should register own callback function for reading the output
+ * stream. Application writes new commands with CO_GTWA_write().
+ */
+
+/**
+ * @defgroup CO_CANopen_309_3_Syntax Command syntax
+ * @{
+ *
+ * @code{.unparsed}
+Command strings start with '"["<sequence>"]"' followed by:
+[[<net>] <node>] r[ead] <index> <subindex> [<datatype>]        # SDO upload.
+[[<net>] <node>] w[rite] <index> <subindex> <datatype> <value> # SDO download.
+
+[[<net>] <node>] start                   # NMT Start node.
+[[<net>] <node>] stop                    # NMT Stop node.
+[[<net>] <node>] preop[erational]        # NMT Set node to pre-operational.
+[[<net>] <node>] reset node              # NMT Reset node.
+[[<net>] <node>] reset comm[unication]   # NMT Reset communication.
+
+[<net>] set network <value>              # Set default net.
+[<net>] set node <value>                 # Set default node.
+[<net>] set sdo_timeout <value>          # Configure SDO time-out.
+[<net>] set sdo_block <value>            # Enable/disable SDO block transfer.
+
+help [datatype|lss]                      # Print this or datatype or lss help.
+led                                      # Print status LED diodes.
+log                                      # Print message log.
+
+Response:
+"["<sequence>"]" OK | <value> |
+                 ERROR:<SDO-abort-code> | ERROR:<internal-error-code>
+
+* Every command must be terminated with <CR><LF> ('\\r\\n'). characters. Same
+  is response. String is not null terminated, <CR> is optional in command.
+* Comments started with '#' are ignored. They may be on the beginning of the
+  line or after the command string.
+* 'sdo_timeout' is in milliseconds, 500 by default. Block transfer is
+  disabled by default.
+* If '<net>' or '<node>' is not specified within commands, then value defined
+  by 'set network' or 'set node' command is used.
+
+Datatypes:
+b                  # Boolean.
+i8, i16, i32, i64  # Signed integers.
+u8, u16, u32, u64  # Unsigned integers.
+x8, x16, x32, x64  # Unsigned integers, displayed as hexadecimal, non-standard.
+r32, r64           # Real numbers.
+t, td              # Time of day, time difference.
+vs                 # Visible string (between double quotes if multi-word).
+os, us             # Octet, unicode string, (mime-base64 (RFC2045) based, line).
+d                  # domain (mime-base64 (RFC2045) based, one line).
+hex                # Hexagonal data, optionally space separated, non-standard.
+
+LSS commands:
+lss_switch_glob <0|1>                  # Switch state global command.
+lss_switch_sel <vendorID> <product code> \\
+               <revisionNo> <serialNo> #Switch state selective.
+lss_set_node <node>                    # Configure node-ID.
+lss_conf_bitrate <table_selector=0> \\
+                 <table_index>         # Configure bit-rate.
+lss_activate_bitrate <switch_delay_ms> # Activate new bit-rate.
+lss_store                              # LSS store configuration.
+lss_inquire_addr [<LSSSUB=0..3>]       # Inquire LSS address.
+lss_get_node                           # Inquire node-ID.
+_lss_fastscan [<timeout_ms>]           # Identify fastscan, non-standard.
+lss_allnodes [<timeout_ms> [<nodeStart=1..127> <store=0|1>\\
+                [<scanType0> <vendorId> <scanType1> <productCode>\\
+                 <scanType2> <revisionNo> <scanType3> <serialNo>]]]
+                                       # Node-ID configuration of all nodes.
+
+* All LSS commands start with '\"[\"<sequence>\"]\" [<net>]'.
+* <table_index>: 0=1000 kbit/s, 1=800 kbit/s, 2=500 kbit/s, 3=250 kbit/s,
+                 4=125 kbit/s, 6=50 kbit/s, 7=20 kbit/s, 8=10 kbit/s, 9=auto
+* <scanType>: 0=fastscan, 1=ignore, 2=match value in next parameter
+ * @endcode
+ *
+ * This help text is the same as variable contents in CO_GTWA_helpString.
+ * @}
+ */
+
+
+/** Size of response string buffer. This is intermediate buffer. If there is
+ * larger amount of data to transfer, then multiple transfers will occur. */
+#ifndef CO_GTWA_RESP_BUF_SIZE
+#define CO_GTWA_RESP_BUF_SIZE 200
+#endif
+
+
+/** Timeout time in microseconds for some internal states. */
+#ifndef CO_GTWA_STATE_TIMEOUT_TIME_US
+#define CO_GTWA_STATE_TIMEOUT_TIME_US 1200000
+#endif
+
+
+/**
+ * Response error codes as specified by CiA 309-3. Values less or equal to 0
+ * are used for control for some functions and are not part of the standard.
+ */
+typedef enum {
+    /** 0 - No error or idle */
+    CO_GTWA_respErrorNone                        = 0,
+    /** 100 - Request not supported */
+    CO_GTWA_respErrorReqNotSupported             = 100,
+    /** 101 - Syntax error */
+    CO_GTWA_respErrorSyntax                      = 101,
+    /** 102 - Request not processed due to internal state */
+    CO_GTWA_respErrorInternalState               = 102,
+    /** 103 - Time-out (where applicable) */
+    CO_GTWA_respErrorTimeOut                     = 103,
+    /** 104 - No default net set */
+    CO_GTWA_respErrorNoDefaultNetSet             = 104,
+    /** 105 - No default node set */
+    CO_GTWA_respErrorNoDefaultNodeSet            = 105,
+    /** 106 - Unsupported net */
+    CO_GTWA_respErrorUnsupportedNet              = 106,
+    /** 107 - Unsupported node */
+    CO_GTWA_respErrorUnsupportedNode             = 107,
+    /** 200 - Lost guarding message */
+    CO_GTWA_respErrorLostGuardingMessage         = 200,
+    /** 201 - Lost connection */
+    CO_GTWA_respErrorLostConnection              = 201,
+    /** 202 - Heartbeat started */
+    CO_GTWA_respErrorHeartbeatStarted            = 202,
+    /** 203 - Heartbeat lost */
+    CO_GTWA_respErrorHeartbeatLost               = 203,
+    /** 204 - Wrong NMT state */
+    CO_GTWA_respErrorWrongNMTstate               = 204,
+    /** 205 - Boot-up */
+    CO_GTWA_respErrorBootUp                      = 205,
+    /** 300 - Error passive */
+    CO_GTWA_respErrorErrorPassive                = 300,
+    /** 301 - Bus off */
+    CO_GTWA_respErrorBusOff                      = 301,
+    /** 303 - CAN buffer overflow */
+    CO_GTWA_respErrorCANbufferOverflow           = 303,
+    /** 304 - CAN init */
+    CO_GTWA_respErrorCANinit                     = 304,
+    /** 305 - CAN active (at init or start-up) */
+    CO_GTWA_respErrorCANactive                   = 305,
+    /** 400 - PDO already used */
+    CO_GTWA_respErrorPDOalreadyUsed              = 400,
+    /** 401 - PDO length exceeded */
+    CO_GTWA_respErrorPDOlengthExceeded           = 401,
+    /** 501 - LSS implementation- / manufacturer-specific error */
+    CO_GTWA_respErrorLSSmanufacturer             = 501,
+    /** 502 - LSS node-ID not supported */
+    CO_GTWA_respErrorLSSnodeIdNotSupported       = 502,
+    /** 503 - LSS bit-rate not supported */
+    CO_GTWA_respErrorLSSbitRateNotSupported      = 503,
+    /** 504 - LSS parameter storing failed */
+    CO_GTWA_respErrorLSSparameterStoringFailed   = 504,
+    /** 505 - LSS command failed because of media error */
+    CO_GTWA_respErrorLSSmediaError               = 505,
+    /** 600 - Running out of memory */
+    CO_GTWA_respErrorRunningOutOfMemory          = 600
+} CO_GTWA_respErrorCode_t;
+
+
+/**
+ * Internal states of the Gateway-ascii state machine.
+ */
+typedef enum {
+    /** Gateway is idle, no command is processing. This state is starting point
+     * for new commands, which are parsed here. */
+    CO_GTWA_ST_IDLE = 0x00U,
+    /** SDO 'read' (upload) */
+    CO_GTWA_ST_READ = 0x10U,
+    /** SDO 'write' (download) */
+    CO_GTWA_ST_WRITE = 0x11U,
+    /** SDO 'write' (download) - aborted, purging remaining data */
+    CO_GTWA_ST_WRITE_ABORTED = 0x12U,
+    /** LSS 'lss_switch_glob' */
+    CO_GTWA_ST_LSS_SWITCH_GLOB = 0x20U,
+    /** LSS 'lss_switch_sel' */
+    CO_GTWA_ST_LSS_SWITCH_SEL = 0x21U,
+    /** LSS 'lss_set_node' */
+    CO_GTWA_ST_LSS_SET_NODE = 0x22U,
+    /** LSS 'lss_conf_bitrate' */
+    CO_GTWA_ST_LSS_CONF_BITRATE = 0x23U,
+    /** LSS 'lss_store' */
+    CO_GTWA_ST_LSS_STORE = 0x24U,
+    /** LSS 'lss_inquire_addr' or 'lss_get_node' */
+    CO_GTWA_ST_LSS_INQUIRE = 0x25U,
+    /** LSS 'lss_inquire_addr', all parameters */
+    CO_GTWA_ST_LSS_INQUIRE_ADDR_ALL = 0x26U,
+    /** LSS '_lss_fastscan' */
+    CO_GTWA_ST__LSS_FASTSCAN = 0x30U,
+    /** LSS 'lss_allnodes' */
+    CO_GTWA_ST_LSS_ALLNODES = 0x31U,
+    /** print message 'log' */
+    CO_GTWA_ST_LOG = 0x80U,
+    /** print 'help' text */
+    CO_GTWA_ST_HELP = 0x81U,
+    /** print 'status' of the node */
+    CO_GTWA_ST_LED = 0x82U
+} CO_GTWA_state_t;
+
+
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO) || defined CO_DOXYGEN
+/*
+ * CANopen Gateway-ascii data types structure
+ */
+typedef struct {
+    /** Data type syntax, as defined in CiA309-3 */
+    char* syntax;
+    /** Data type length in bytes, 0 if size is not known */
+    size_t length;
+    /** Function, which reads data of specific data type from fifo buffer and
+     * writes them as corresponding ascii string. It is a pointer to
+     * #CO_fifo_readU82a function or similar and is used with SDO upload. For
+     * description of parameters see #CO_fifo_readU82a */
+    size_t (*dataTypePrint)(CO_fifo_t *fifo,
+                            char *buf,
+                            size_t count,
+                            bool_t end);
+    /** Function, which reads ascii-data of specific data type from fifo buffer
+     * and copies them to another fifo buffer as binary data. It is a pointer to
+     * #CO_fifo_cpyTok2U8 function or similar and is used with SDO download. For
+     * description of parameters see #CO_fifo_cpyTok2U8 */
+    size_t (*dataTypeScan)(CO_fifo_t *dest,
+                           CO_fifo_t *src,
+                           CO_fifo_st *status);
+} CO_GTWA_dataType_t;
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
+
+
+/**
+ * CANopen Gateway-ascii object
+ */
+typedef struct {
+    /** Pointer to external function for reading response from Gateway-ascii
+     * object. Pointer is initialized in CO_GTWA_initRead().
+     *
+     * @param object Void pointer to custom object
+     * @param buf Buffer from which data can be read
+     * @param count Count of bytes available inside buffer
+     * @param [out] connectionOK different than 0 indicates connection is OK.
+     *
+     * @return Count of bytes actually transferred.
+     */
+    size_t (*readCallback)(void *object,
+                           const char *buf,
+                           size_t count,
+                           uint8_t *connectionOK);
+    /** Pointer to object, which will be used inside readCallback, from
+     * CO_GTWA_init() */
+    void *readCallbackObject;
+    /** Sequence number of the command */
+    uint32_t sequence;
+    /** Default CANopen Net number is undefined (-1) at startup */
+    int32_t net_default;
+    /** Default CANopen Node ID number is undefined (-1) at startup */
+    int16_t node_default;
+    /** Current CANopen Net number */
+    uint16_t net;
+    /** Current CANopen Node ID */
+    uint8_t node;
+    /** CO_fifo_t object for command (not pointer) */
+    CO_fifo_t commFifo;
+    /** Command buffer of usable size @ref CO_CONFIG_GTWA_COMM_BUF_SIZE */
+    char commBuf[CO_CONFIG_GTWA_COMM_BUF_SIZE + 1];
+    /** Response buffer of usable size @ref CO_GTWA_RESP_BUF_SIZE */
+    char respBuf[CO_GTWA_RESP_BUF_SIZE];
+    /** Actual size of data in respBuf */
+    size_t respBufCount;
+    /** If only part of data has been successfully written into external
+     * application (with readCallback()), then Gateway-ascii object will stay
+     * in current state. This situation is indicated with respHold variable and
+     * respBufOffset indicates offset to untransferred data inside respBuf. */
+    size_t respBufOffset;
+    /** See respBufOffset above */
+    bool_t respHold;
+    /** Sum of time difference from CO_GTWA_process() in case of respHold */
+    uint32_t timeDifference_us_cumulative;
+    /** Current state of the gateway object */
+    CO_GTWA_state_t state;
+    /** Timeout timer for the current state */
+    uint32_t stateTimeoutTmr;
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO) || defined CO_DOXYGEN
+    /** SDO client object from CO_GTWA_init() */
+    CO_SDOclient_t *SDO_C;
+    /** Timeout time for SDO transfer in milliseconds, if no response */
+    uint16_t SDOtimeoutTime;
+    /** SDO block transfer enabled? */
+    bool_t SDOblockTransferEnable;
+    /** Indicate status of data copy from / to SDO buffer. If reading, true
+     * indicates, that response has started. If writing, true indicates, that
+     * SDO buffer contains only part of data and more data will follow. */
+    bool_t SDOdataCopyStatus;
+    /** Data type of variable in current SDO communication */
+    const CO_GTWA_dataType_t *SDOdataType;
+#endif
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_NMT) || defined CO_DOXYGEN
+    /** NMT object from CO_GTWA_init() */
+    CO_NMT_t *NMT;
+#endif
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS) || defined CO_DOXYGEN
+    /** LSSmaster object from CO_GTWA_init() */
+    CO_LSSmaster_t *LSSmaster;
+    /** 128 bit number, uniquely identifying each node */
+    CO_LSS_address_t lssAddress;
+    /** LSS Node-ID parameter */
+    uint8_t lssNID;
+    /** LSS bitrate parameter */
+    uint16_t lssBitrate;
+    /** LSS inquire parameter */
+    CO_LSS_cs_t lssInquireCs;
+    /** LSS fastscan parameter */
+    CO_LSSmaster_fastscan_t lssFastscan;
+    /** LSS allnodes sub state parameter */
+    uint8_t lssSubState;
+    /** LSS allnodes node count parameter */
+    uint8_t lssNodeCount;
+    /** LSS allnodes store parameter */
+    bool_t lssStore;
+    /** LSS allnodes timeout parameter */
+    uint16_t lssTimeout_ms;
+#endif
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG) || defined CO_DOXYGEN
+    /** Message log buffer of usable size @ref CO_CONFIG_GTWA_LOG_BUF_SIZE */
+    char logBuf[CO_CONFIG_GTWA_LOG_BUF_SIZE + 1];
+    /** CO_fifo_t object for message log (not pointer) */
+    CO_fifo_t logFifo;
+#endif
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_HELP) || defined CO_DOXYGEN
+    /** Offset, when printing help text */
+    const char *helpString;
+    size_t helpStringOffset;
+#endif
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS) || defined CO_DOXYGEN
+    /** CO_LEDs_t object for CANopen status LEDs imitation from CO_GTWA_init()*/
+    CO_LEDs_t *LEDs;
+    uint8_t ledStringPreviousIndex;
+#endif
+} CO_GTWA_t;
+
+
+/**
+ * Initialize Gateway-ascii object
+ *
+ * @param gtwa This object will be initialized
+ * @param SDO_C SDO client object
+ * @param SDOclientTimeoutTime_ms Default timeout in milliseconds, 500 typically
+ * @param SDOclientBlockTransfer If true, block transfer will be set by default
+ * @param NMT NMT object
+ * @param LSSmaster LSS master object
+ * @param LEDs LEDs object
+ * @param dummy dummy argument, set to 0
+ *
+ * @return #CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT
+ */
+CO_ReturnError_t CO_GTWA_init(CO_GTWA_t* gtwa,
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO) || defined CO_DOXYGEN
+                              CO_SDOclient_t* SDO_C,
+                              uint16_t SDOclientTimeoutTime_ms,
+                              bool_t SDOclientBlockTransfer,
+#endif
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_NMT) || defined CO_DOXYGEN
+                              CO_NMT_t *NMT,
+#endif
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS) || defined CO_DOXYGEN
+                              CO_LSSmaster_t *LSSmaster,
+#endif
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS) || defined CO_DOXYGEN
+                              CO_LEDs_t *LEDs,
+#endif
+                              uint8_t dummy);
+
+
+/**
+ * Initialize read callback in Gateway-ascii object
+ *
+ * Callback will be used for transfer data to output stream of the application.
+ * It will be called from CO_GTWA_process() zero or multiple times, depending on
+ * the data available. If readCallback is uninitialized or NULL, then output
+ * data will be purged.
+ *
+ * @param gtwa This object will be initialized
+ * @param readCallback Pointer to external function for reading response from
+ * Gateway-ascii object. See #CO_GTWA_t for parameters.
+ * @param readCallbackObject Pointer to object, which will be used inside
+ * readCallback
+ */
+void CO_GTWA_initRead(CO_GTWA_t* gtwa,
+                      size_t (*readCallback)(void *object,
+                                             const char *buf,
+                                             size_t count,
+                                             uint8_t *connectionOK),
+                      void *readCallbackObject);
+
+
+/**
+ * Get free write buffer space
+ *
+ * @param gtwa This object
+ *
+ * @return number of available bytes
+ */
+static inline size_t CO_GTWA_write_getSpace(CO_GTWA_t* gtwa) {
+    return CO_fifo_getSpace(&gtwa->commFifo);
+}
+
+
+/**
+ * Write command into CO_GTWA_t object.
+ *
+ * This function copies ascii command from buf into internal fifo buffer.
+ * Command must be closed with '\n' character. Function returns number of bytes
+ * successfully copied. If there is not enough space in destination, not all
+ * bytes will be copied and data can be refilled later (in case of large SDO
+ * download).
+ *
+ * @param gtwa This object
+ * @param buf Buffer which will be copied
+ * @param count Number of bytes in buf
+ *
+ * @return number of bytes actually written.
+ */
+static inline size_t CO_GTWA_write(CO_GTWA_t* gtwa,
+                                   const char *buf,
+                                   size_t count)
+{
+    return CO_fifo_write(&gtwa->commFifo, buf, count, NULL);
+}
+
+
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG) || defined CO_DOXYGEN
+/**
+ * Print message log string into fifo buffer
+ *
+ * This function enables recording of system log messages including CANopen
+ * events. Function can be called by application for recording any message.
+ * Message is copied to internal fifo buffer. In case fifo is full, old messages
+ * will be owerwritten. Message log fifo can be read with non-standard command
+ * "log". After log is read, it is emptied.
+ *
+ * @param gtwa This object
+ * @param message Null terminated string
+ */
+void CO_GTWA_log_print(CO_GTWA_t* gtwa, const char *message);
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG */
+
+
+/**
+ * Process Gateway-ascii object
+ *
+ * This is non-blocking function and must be called cyclically
+ *
+ * @param gtwa This object will be initialized.
+ * @param enable If true, gateway operates normally. If false, gateway is
+ * completely disabled and no command interaction is possible. Can be connected
+ * to hardware switch, for example.
+ * @param timeDifference_us Time difference from previous function call in
+ * [microseconds].
+ * @param [out] timerNext_us info to OS - see CO_process().
+ *
+ * @return CO_ReturnError_t: CO_ERROR_NO on success or CO_ERROR_ILLEGAL_ARGUMENT
+ */
+void CO_GTWA_process(CO_GTWA_t *gtwa,
+                     bool_t enable,
+                     uint32_t timeDifference_us,
+                     uint32_t *timerNext_us);
+
+/** @} */ /* CO_CANopen_309_3 */
+
+#ifdef __cplusplus
+}
+#endif /*__cplusplus*/
+
+#endif  /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
+
+#endif /* CO_GATEWAY_ASCII_H */

controller_yy_app/middleware/CANopenNode/CANopen.c

@@ -0,0 +1,1156 @@
+/*
+ * Main CANopen stack file. It combines Object dictionary (CO_OD) and all other
+ * CANopen source files. Configuration information are read from CO_OD.h file.
+ *
+ * @file        CANopen.c
+ * @ingroup     CO_CANopen
+ * @author      Janez Paternoster
+ * @copyright   2010 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+
+#include "CANopen.h"
+
+#include <stdlib.h>
+
+
+/* Global variables ***********************************************************/
+/* #define CO_USE_GLOBALS */    /* If defined, global variables will be used
+                                   instead of dynamically allocated. */
+extern const CO_OD_entry_t CO_OD[CO_OD_NoOfElements];   /* Object Dictionary */
+static CO_t COO;                    /* Pointers to CANopen objects */
+CO_t *CO = NULL;                    /* Pointer to COO */
+
+static CO_CANrx_t          *CO_CANmodule_rxArray0;
+static CO_CANtx_t          *CO_CANmodule_txArray0;
+static CO_OD_extension_t   *CO_SDO_ODExtensions;
+static CO_HBconsNode_t     *CO_HBcons_monitoredNodes;
+
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII) && !defined CO_GTWA_ENABLE
+#define CO_GTWA_ENABLE      true
+#endif
+#if CO_NO_TRACE > 0
+static uint32_t            *CO_traceTimeBuffers[CO_NO_TRACE];
+static int32_t             *CO_traceValueBuffers[CO_NO_TRACE];
+static uint32_t             CO_traceBufferSize[CO_NO_TRACE];
+#endif
+#ifndef CO_STATUS_FIRMWARE_DOWNLOAD_IN_PROGRESS
+#define CO_STATUS_FIRMWARE_DOWNLOAD_IN_PROGRESS 0
+#endif
+
+
+/* Verify number of CANopenNode objects from CO_OD.h **************************/
+#if    CO_NO_SYNC                                 >  1     \
+    || CO_NO_EMERGENCY                            != 1     \
+    || (CO_NO_SDO_SERVER < 1 || CO_NO_SDO_SERVER > 128)    \
+    || CO_NO_TIME                                 >  1     \
+    || CO_NO_SDO_CLIENT                           > 128    \
+    || CO_NO_GFC                                  >  1     \
+    || CO_NO_SRDO                                 > 64     \
+    || (CO_NO_RPDO < 1 || CO_NO_RPDO > 0x200)              \
+    || (CO_NO_TPDO < 1 || CO_NO_TPDO > 0x200)              \
+    || ODL_consumerHeartbeatTime_arrayLength      == 0     \
+    || ODL_errorStatusBits_stringLength           < 10     \
+    || CO_NO_LSS_SLAVE                            >  1     \
+    || CO_NO_LSS_MASTER                           >  1
+#error Features from CO_OD.h file are not corectly configured for this project!
+#endif
+
+/* Indexes of CO_CANrx_t objects in CO_CANmodule_t and total number of them. **/
+#define CO_RXCAN_NMT      0
+#define CO_RXCAN_SYNC    (CO_RXCAN_NMT      + CO_NO_NMT)
+#define CO_RXCAN_EMERG   (CO_RXCAN_SYNC     + CO_NO_SYNC)
+#define CO_RXCAN_TIME    (CO_RXCAN_EMERG    + CO_NO_EM_CONS)
+#define CO_RXCAN_GFC     (CO_RXCAN_TIME     + CO_NO_TIME)
+#define CO_RXCAN_SRDO    (CO_RXCAN_GFC      + CO_NO_GFC)
+#define CO_RXCAN_RPDO    (CO_RXCAN_SRDO     + CO_NO_SRDO*2)
+#define CO_RXCAN_SDO_SRV (CO_RXCAN_RPDO     + CO_NO_RPDO)
+#define CO_RXCAN_SDO_CLI (CO_RXCAN_SDO_SRV  + CO_NO_SDO_SERVER)
+#define CO_RXCAN_CONS_HB (CO_RXCAN_SDO_CLI  + CO_NO_SDO_CLIENT)
+#define CO_RXCAN_LSS_SLV (CO_RXCAN_CONS_HB  + CO_NO_HB_CONS)
+#define CO_RXCAN_LSS_MST (CO_RXCAN_LSS_SLV  + CO_NO_LSS_SLAVE)
+#define CO_RXCAN_NO_MSGS (CO_NO_NMT         + \
+                          CO_NO_SYNC        + \
+                          CO_NO_EM_CONS     + \
+                          CO_NO_TIME        + \
+                          CO_NO_GFC         + \
+                          CO_NO_SRDO*2      + \
+                          CO_NO_RPDO        + \
+                          CO_NO_SDO_SERVER  + \
+                          CO_NO_SDO_CLIENT  + \
+                          CO_NO_HB_CONS     + \
+                          CO_NO_LSS_SLAVE   + \
+                          CO_NO_LSS_MASTER)
+
+/* Indexes of CO_CANtx_t objects in CO_CANmodule_t and total number of them. **/
+#define CO_TXCAN_NMT      0
+#define CO_TXCAN_SYNC    (CO_TXCAN_NMT      + CO_NO_NMT_MST)
+#define CO_TXCAN_EMERG   (CO_TXCAN_SYNC     + CO_NO_SYNC)
+#define CO_TXCAN_TIME    (CO_TXCAN_EMERG    + CO_NO_EMERGENCY)
+#define CO_TXCAN_GFC     (CO_TXCAN_TIME     + CO_NO_TIME)
+#define CO_TXCAN_SRDO    (CO_TXCAN_GFC      + CO_NO_GFC)
+#define CO_TXCAN_TPDO    (CO_TXCAN_SRDO     + CO_NO_SRDO*2)
+#define CO_TXCAN_SDO_SRV (CO_TXCAN_TPDO     + CO_NO_TPDO)
+#define CO_TXCAN_SDO_CLI (CO_TXCAN_SDO_SRV  + CO_NO_SDO_SERVER)
+#define CO_TXCAN_HB      (CO_TXCAN_SDO_CLI  + CO_NO_SDO_CLIENT)
+#define CO_TXCAN_LSS_SLV (CO_TXCAN_HB       + CO_NO_HB_PROD)
+#define CO_TXCAN_LSS_MST (CO_TXCAN_LSS_SLV  + CO_NO_LSS_SLAVE)
+#define CO_TXCAN_NO_MSGS (CO_NO_NMT_MST     + \
+                          CO_NO_SYNC        + \
+                          CO_NO_EMERGENCY   + \
+                          CO_NO_TIME        + \
+                          CO_NO_GFC         + \
+                          CO_NO_SRDO*2      + \
+                          CO_NO_TPDO        + \
+                          CO_NO_SDO_SERVER  + \
+                          CO_NO_SDO_CLIENT  + \
+                          CO_NO_HB_PROD     + \
+                          CO_NO_LSS_SLAVE   + \
+                          CO_NO_LSS_MASTER)
+
+
+/* Create objects from heap ***************************************************/
+#ifndef CO_USE_GLOBALS
+CO_ReturnError_t CO_new(uint32_t *heapMemoryUsed) {
+    int16_t i;
+    uint16_t errCnt = 0;
+    uint32_t CO_memoryUsed = 0;
+
+    /* If CANopen was initialized before, return. */
+    if (CO != NULL) {
+        return CO_ERROR_NO;
+    }
+
+    /* globals */
+    CO = &COO;
+
+    /* CANmodule */
+    CO->CANmodule[0] = (CO_CANmodule_t *)calloc(1, sizeof(CO_CANmodule_t));
+    if (CO->CANmodule[0] == NULL) errCnt++;
+    CO_CANmodule_rxArray0 =
+        (CO_CANrx_t *)calloc(CO_RXCAN_NO_MSGS, sizeof(CO_CANrx_t));
+    if (CO_CANmodule_rxArray0 == NULL) errCnt++;
+    CO_CANmodule_txArray0 =
+        (CO_CANtx_t *)calloc(CO_TXCAN_NO_MSGS, sizeof(CO_CANtx_t));
+    if (CO_CANmodule_txArray0 == NULL) errCnt++;
+    CO_memoryUsed += sizeof(CO_CANmodule_t) +
+                     sizeof(CO_CANrx_t) * CO_RXCAN_NO_MSGS +
+                     sizeof(CO_CANtx_t) * CO_TXCAN_NO_MSGS;
+
+    /* SDOserver */
+    for (i = 0; i < CO_NO_SDO_SERVER; i++) {
+        CO->SDO[i] = (CO_SDO_t *)calloc(1, sizeof(CO_SDO_t));
+        if (CO->SDO[i] == NULL) errCnt++;
+    }
+    CO_SDO_ODExtensions = (CO_OD_extension_t *)calloc(
+        CO_OD_NoOfElements, sizeof(CO_OD_extension_t));
+    if (CO_SDO_ODExtensions == NULL) errCnt++;
+    CO_memoryUsed += sizeof(CO_SDO_t) * CO_NO_SDO_SERVER +
+                     sizeof(CO_OD_extension_t) * CO_OD_NoOfElements;
+
+    /* Emergency */
+    CO->em = (CO_EM_t *)calloc(1, sizeof(CO_EM_t));
+    if (CO->em == NULL) errCnt++;
+    CO->emPr = (CO_EMpr_t *)calloc(1, sizeof(CO_EMpr_t));
+    if (CO->emPr == NULL) errCnt++;
+    CO_memoryUsed += sizeof(CO_EM_t) + sizeof(CO_EMpr_t);
+
+    /* NMT_Heartbeat */
+    CO->NMT = (CO_NMT_t *)calloc(1, sizeof(CO_NMT_t));
+    if (CO->NMT == NULL) errCnt++;
+    CO_memoryUsed += sizeof(CO_NMT_t);
+
+#if CO_NO_SYNC == 1
+    /* SYNC */
+    CO->SYNC = (CO_SYNC_t *)calloc(1, sizeof(CO_SYNC_t));
+    if (CO->SYNC == NULL) errCnt++;
+    CO_memoryUsed += sizeof(CO_SYNC_t);
+#endif
+
+#if CO_NO_TIME == 1
+    /* TIME */
+    CO->TIME = (CO_TIME_t *)calloc(1, sizeof(CO_TIME_t));
+    if (CO->TIME == NULL) errCnt++;
+    CO_memoryUsed += sizeof(CO_TIME_t);
+#endif
+
+#if CO_NO_GFC == 1
+    CO->GFC = (CO_GFC_t *)calloc(1, sizeof(CO_GFC_t));
+    if (CO->GFC == NULL) errCnt++;
+    CO_memoryUsed += sizeof(CO_GFC_t);
+#endif
+
+#if CO_NO_SRDO != 0
+    /* SRDO */
+    CO->SRDOGuard = (CO_SRDOGuard_t *)calloc(1, sizeof(CO_SRDOGuard_t));
+    if (CO->SRDOGuard == NULL) errCnt++;
+    for (i = 0; i < CO_NO_SRDO; i++) {
+        CO->SRDO[i] = (CO_SRDO_t *)calloc(1, sizeof(CO_SRDO_t));
+        if (CO->SRDO[i] == NULL) errCnt++;
+    }
+    CO_memoryUsed += sizeof(CO_SRDO_t) * CO_NO_SRDO + sizeof(CO_SRDOGuard_t);
+#endif
+    /* RPDO */
+    for (i = 0; i < CO_NO_RPDO; i++) {
+        CO->RPDO[i] = (CO_RPDO_t *)calloc(1, sizeof(CO_RPDO_t));
+        if (CO->RPDO[i] == NULL) errCnt++;
+    }
+    CO_memoryUsed += sizeof(CO_RPDO_t) * CO_NO_RPDO;
+
+    /* TPDO */
+    for (i = 0; i < CO_NO_TPDO; i++) {
+        CO->TPDO[i] = (CO_TPDO_t *)calloc(1, sizeof(CO_TPDO_t));
+        if (CO->TPDO[i] == NULL) errCnt++;
+    }
+    CO_memoryUsed += sizeof(CO_TPDO_t) * CO_NO_TPDO;
+
+    /* Heartbeat consumer */
+    CO->HBcons = (CO_HBconsumer_t *)calloc(1, sizeof(CO_HBconsumer_t));
+    if (CO->HBcons == NULL) errCnt++;
+    CO_HBcons_monitoredNodes =
+        (CO_HBconsNode_t *)calloc(CO_NO_HB_CONS, sizeof(CO_HBconsNode_t));
+    if (CO_HBcons_monitoredNodes == NULL) errCnt++;
+    CO_memoryUsed += sizeof(CO_HBconsumer_t) +
+                     sizeof(CO_HBconsNode_t) * CO_NO_HB_CONS;
+
+#if CO_NO_SDO_CLIENT != 0
+    /* SDOclient */
+    for (i = 0; i < CO_NO_SDO_CLIENT; i++) {
+        CO->SDOclient[i] =
+            (CO_SDOclient_t *)calloc(1, sizeof(CO_SDOclient_t));
+        if (CO->SDOclient[i] == NULL) errCnt++;
+    }
+    CO_memoryUsed += sizeof(CO_SDOclient_t) * CO_NO_SDO_CLIENT;
+#endif
+
+#if (CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE
+    /* LEDs */
+    CO->LEDs = (CO_LEDs_t *)calloc(1, sizeof(CO_LEDs_t));
+    if (CO->LEDs == NULL) errCnt++;
+    CO_memoryUsed += sizeof(CO_LEDs_t);
+#endif
+
+#if CO_NO_LSS_SLAVE == 1
+    /* LSSslave */
+    CO->LSSslave = (CO_LSSslave_t *)calloc(1, sizeof(CO_LSSslave_t));
+    if (CO->LSSslave == NULL) errCnt++;
+    CO_memoryUsed += sizeof(CO_LSSslave_t);
+#endif
+
+#if CO_NO_LSS_MASTER == 1
+    /* LSSmaster */
+    CO->LSSmaster = (CO_LSSmaster_t *)calloc(1, sizeof(CO_LSSmaster_t));
+    if (CO->LSSmaster == NULL) errCnt++;
+    CO_memoryUsed += sizeof(CO_LSSmaster_t);
+#endif
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
+    /* Gateway-ascii */
+    CO->gtwa = (CO_GTWA_t *)calloc(1, sizeof(CO_GTWA_t));
+    if (CO->gtwa == NULL) errCnt++;
+    CO_memoryUsed += sizeof(CO_GTWA_t);
+#endif
+
+#if CO_NO_TRACE > 0
+    /* Trace */
+    for (i = 0; i < CO_NO_TRACE; i++) {
+        CO->trace[i] = (CO_trace_t *)calloc(1, sizeof(CO_trace_t));
+        if (CO->trace[i] == NULL) errCnt++;
+    }
+    CO_memoryUsed += sizeof(CO_trace_t) * CO_NO_TRACE;
+    for (i = 0; i < CO_NO_TRACE; i++) {
+        CO_traceTimeBuffers[i] =
+            (uint32_t *)calloc(OD_traceConfig[i].size, sizeof(uint32_t));
+        CO_traceValueBuffers[i] =
+            (int32_t *)calloc(OD_traceConfig[i].size, sizeof(int32_t));
+        if (CO_traceTimeBuffers[i] != NULL &&
+            CO_traceValueBuffers[i] != NULL) {
+            CO_traceBufferSize[i] = OD_traceConfig[i].size;
+        } else {
+            CO_traceBufferSize[i] = 0;
+        }
+        CO_memoryUsed += CO_traceBufferSize[i] * sizeof(uint32_t) * 2;
+    }
+#endif
+
+    if (heapMemoryUsed != NULL) {
+        *heapMemoryUsed = CO_memoryUsed;
+    }
+
+    return (errCnt == 0) ? CO_ERROR_NO : CO_ERROR_OUT_OF_MEMORY;
+}
+
+
+/******************************************************************************/
+void CO_delete(void *CANptr) {
+    int16_t i;
+
+    CO_CANsetConfigurationMode(CANptr);
+
+    /* If CANopen isn't initialized, return. */
+    if (CO == NULL) {
+        return;
+    }
+
+    CO_CANmodule_disable(CO->CANmodule[0]);
+
+#if CO_NO_TRACE > 0
+    /* Trace */
+    for (i = 0; i < CO_NO_TRACE; i++) {
+        free(CO->trace[i]);
+        free(CO_traceTimeBuffers[i]);
+        free(CO_traceValueBuffers[i]);
+    }
+#endif
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
+    /* Gateway-ascii */
+    free(CO->gtwa);
+#endif
+
+#if CO_NO_LSS_MASTER == 1
+    /* LSSmaster */
+    free(CO->LSSmaster);
+#endif
+
+#if CO_NO_LSS_SLAVE == 1
+    /* LSSslave */
+    free(CO->LSSslave);
+#endif
+
+#if CO_NO_SDO_CLIENT != 0
+    /* SDOclient */
+    for (i = 0; i < CO_NO_SDO_CLIENT; i++) {
+        free(CO->SDOclient[i]);
+    }
+#endif
+
+    /* Heartbeat consumer */
+    free(CO_HBcons_monitoredNodes);
+    free(CO->HBcons);
+
+#if CO_NO_GFC == 1
+    /* GFC */
+    free(CO->GFC);
+#endif
+
+#if CO_NO_SRDO != 0
+    /* SRDO */
+    for (i = 0; i < CO_NO_SRDO; i++) {
+        free(CO->SRDO[i]);
+    }
+    free(CO->SRDOGuard);
+#endif
+
+    /* TPDO */
+    for (i = 0; i < CO_NO_TPDO; i++) {
+        free(CO->TPDO[i]);
+    }
+
+    /* RPDO */
+    for (i = 0; i < CO_NO_RPDO; i++) {
+        free(CO->RPDO[i]);
+    }
+
+#if CO_NO_TIME == 1
+    /* TIME */
+    free(CO->TIME);
+#endif
+
+#if CO_NO_SYNC == 1
+    /* SYNC */
+    free(CO->SYNC);
+#endif
+
+    /* NMT_Heartbeat */
+    free(CO->NMT);
+
+    /* Emergency */
+    free(CO->emPr);
+    free(CO->em);
+
+    /* SDOserver */
+    free(CO_SDO_ODExtensions);
+    for (i = 0; i < CO_NO_SDO_SERVER; i++) {
+        free(CO->SDO[i]);
+    }
+
+    /* CANmodule */
+    free(CO_CANmodule_txArray0);
+    free(CO_CANmodule_rxArray0);
+    free(CO->CANmodule[0]);
+
+    /* globals */
+    CO = NULL;
+}
+#endif /* #ifndef CO_USE_GLOBALS */
+
+
+/* Alternatively create objects as globals ************************************/
+#ifdef CO_USE_GLOBALS
+    static CO_CANmodule_t       COO_CANmodule;
+    static CO_CANrx_t           COO_CANmodule_rxArray0[CO_RXCAN_NO_MSGS];
+    static CO_CANtx_t           COO_CANmodule_txArray0[CO_TXCAN_NO_MSGS];
+    static CO_SDO_t             COO_SDO[CO_NO_SDO_SERVER];
+    static CO_OD_extension_t    COO_SDO_ODExtensions[CO_OD_NoOfElements];
+    static CO_EM_t              COO_EM;
+    static CO_EMpr_t            COO_EMpr;
+    static CO_NMT_t             COO_NMT;
+#if CO_NO_SYNC == 1
+    static CO_SYNC_t            COO_SYNC;
+#endif
+#if CO_NO_TIME == 1
+    static CO_TIME_t            COO_TIME;
+#endif
+#if CO_NO_GFC == 1
+    static CO_GFC_t             COO_GFC;
+#endif
+#if CO_NO_SRDO != 0
+    static CO_SRDOGuard_t       COO_SRDOGuard;
+    static CO_SRDO_t            COO_SRDO[CO_NO_SRDO];
+#endif
+    static CO_RPDO_t            COO_RPDO[CO_NO_RPDO];
+    static CO_TPDO_t            COO_TPDO[CO_NO_TPDO];
+    static CO_HBconsumer_t      COO_HBcons;
+    static CO_HBconsNode_t      COO_HBcons_monitoredNodes[CO_NO_HB_CONS];
+#if CO_NO_SDO_CLIENT != 0
+    static CO_SDOclient_t       COO_SDOclient[CO_NO_SDO_CLIENT];
+#endif
+#if (CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE
+    static CO_LEDs_t            COO_LEDs;
+#endif
+#if CO_NO_LSS_SLAVE == 1
+    static CO_LSSslave_t        COO_LSSslave;
+#endif
+#if CO_NO_LSS_MASTER == 1
+    static CO_LSSmaster_t       COO_LSSmaster;
+#endif
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
+    static CO_GTWA_t            COO_gtwa;
+#endif
+#if CO_NO_TRACE > 0
+  #ifndef CO_TRACE_BUFFER_SIZE_FIXED
+    #define CO_TRACE_BUFFER_SIZE_FIXED 100
+  #endif
+    static CO_trace_t           COO_trace[CO_NO_TRACE];
+    static uint32_t             COO_traceTimeBuffers[CO_NO_TRACE][CO_TRACE_BUFFER_SIZE_FIXED];
+    static int32_t              COO_traceValueBuffers[CO_NO_TRACE][CO_TRACE_BUFFER_SIZE_FIXED];
+#endif
+
+CO_ReturnError_t CO_new(uint32_t *heapMemoryUsed) {
+    int16_t i;
+
+    (void)heapMemoryUsed;
+
+    /* If CANopen was initialized before, return. */
+    if (CO != NULL) {
+        return CO_ERROR_NO;
+    }
+
+    /* globals */
+    CO = &COO;
+
+    /* CANmodule */
+    CO->CANmodule[0] = &COO_CANmodule;
+    CO_CANmodule_rxArray0 = &COO_CANmodule_rxArray0[0];
+    CO_CANmodule_txArray0 = &COO_CANmodule_txArray0[0];
+
+    /* SDOserver */
+    for (i = 0; i < CO_NO_SDO_SERVER; i++) {
+        CO->SDO[i] = &COO_SDO[i];
+    }
+    CO_SDO_ODExtensions = &COO_SDO_ODExtensions[0];
+
+    /* Emergency */
+    CO->em = &COO_EM;
+    CO->emPr = &COO_EMpr;
+
+    /* NMT_Heartbeat */
+    CO->NMT = &COO_NMT;
+
+#if CO_NO_SYNC == 1
+    /* SYNC */
+    CO->SYNC = &COO_SYNC;
+#endif
+
+#if CO_NO_TIME == 1
+    /* TIME */
+    CO->TIME = &COO_TIME;
+#endif
+
+#if CO_NO_GFC == 1
+    /* GFC */
+    CO->GFC = &COO_GFC;
+#endif
+
+#if CO_NO_SRDO != 0
+    /* SRDO */
+    CO->SRDOGuard = &COO_SRDOGuard;
+    for (i = 0; i < CO_NO_SRDO; i++) {
+        CO->SRDO[i] = &COO_SRDO[i];
+    }
+#endif
+
+    /* RPDO */
+    for (i = 0; i < CO_NO_RPDO; i++) {
+        CO->RPDO[i] = &COO_RPDO[i];
+    }
+
+    /* TPDO */
+    for (i = 0; i < CO_NO_TPDO; i++) {
+        CO->TPDO[i] = &COO_TPDO[i];
+    }
+
+    /* Heartbeat consumer */
+    CO->HBcons = &COO_HBcons;
+    CO_HBcons_monitoredNodes = &COO_HBcons_monitoredNodes[0];
+
+#if CO_NO_SDO_CLIENT != 0
+    /* SDOclient */
+    for (i = 0; i < CO_NO_SDO_CLIENT; i++) {
+        CO->SDOclient[i] = &COO_SDOclient[i];
+    }
+#endif
+
+#if (CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE
+    /* LEDs */
+    CO->LEDs = &COO_LEDs;
+#endif
+
+#if CO_NO_LSS_SLAVE == 1
+    /* LSSslave */
+    CO->LSSslave = &COO_LSSslave;
+#endif
+
+#if CO_NO_LSS_MASTER == 1
+    /* LSSmaster */
+    CO->LSSmaster = &COO_LSSmaster;
+#endif
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
+    /* Gateway-ascii */
+    CO->gtwa = &COO_gtwa;
+#endif
+
+#if CO_NO_TRACE > 0
+    /* Trace */
+    for (i = 0; i < CO_NO_TRACE; i++) {
+        CO->trace[i] = &COO_trace[i];
+        CO_traceTimeBuffers[i] = &COO_traceTimeBuffers[i][0];
+        CO_traceValueBuffers[i] = &COO_traceValueBuffers[i][0];
+        CO_traceBufferSize[i] = CO_TRACE_BUFFER_SIZE_FIXED;
+    }
+#endif
+
+    return CO_ERROR_NO;
+}
+
+
+/******************************************************************************/
+void CO_delete(void *CANptr) {
+    CO_CANsetConfigurationMode(CANptr);
+
+    /* globals */
+    CO = NULL;
+}
+#endif /* #ifdef CO_USE_GLOBALS */
+
+
+/******************************************************************************/
+CO_ReturnError_t CO_CANinit(void *CANptr,
+                            uint32_t bitRate)
+{
+    CO_ReturnError_t err;
+
+    CO->CANmodule[0]->CANnormal = false;
+    CO_CANsetConfigurationMode(CANptr);
+
+    /* CANmodule */
+    err = CO_CANmodule_init(CO->CANmodule[0],
+                            CANptr,
+                            CO_CANmodule_rxArray0,
+                            CO_RXCAN_NO_MSGS,
+                            CO_CANmodule_txArray0,
+                            CO_TXCAN_NO_MSGS,
+                            bitRate);
+
+    return err;
+}
+
+
+/******************************************************************************/
+#if CO_NO_LSS_SLAVE == 1
+CO_ReturnError_t CO_LSSinit(uint8_t *nodeId,
+                            uint16_t *bitRate)
+{
+    CO_LSS_address_t lssAddress;
+    CO_ReturnError_t err;
+
+    /* LSSslave */
+    lssAddress.identity.productCode = OD_identity.productCode;
+    lssAddress.identity.revisionNumber = OD_identity.revisionNumber;
+    lssAddress.identity.serialNumber = OD_identity.serialNumber;
+    lssAddress.identity.vendorID = OD_identity.vendorID;
+
+    err = CO_LSSslave_init(CO->LSSslave,
+                           &lssAddress,
+                           bitRate,
+                           nodeId,
+                           CO->CANmodule[0],
+                           CO_RXCAN_LSS_SLV,
+                           CO_CAN_ID_LSS_MST,
+                           CO->CANmodule[0],
+                           CO_TXCAN_LSS_SLV,
+                           CO_CAN_ID_LSS_SLV);
+
+    return err;
+}
+#endif /* CO_NO_LSS_SLAVE == 1 */
+
+
+/******************************************************************************/
+CO_ReturnError_t CO_CANopenInit(uint8_t nodeId) {
+    int16_t i;
+    CO_ReturnError_t err;
+
+    /* Verify CANopen Node-ID */
+    CO->nodeIdUnconfigured = false;
+#if CO_NO_LSS_SLAVE == 1
+    if (nodeId == CO_LSS_NODE_ID_ASSIGNMENT) {
+        CO->nodeIdUnconfigured = true;
+    }
+    else
+#endif
+    if (nodeId < 1 || nodeId > 127) {
+        return CO_ERROR_ILLEGAL_ARGUMENT;
+    }
+    /* Verify parameters from CO_OD */
+#if CO_NO_SRDO != 0
+    if (sizeof(OD_SRDOCommunicationParameter_t) != sizeof(CO_SRDOCommPar_t) ||
+        sizeof(OD_SRDOMappingParameter_t)       != sizeof(CO_SRDOMapPar_t)) {
+        return CO_ERROR_OD_PARAMETERS;
+    }
+#endif
+    if (sizeof(OD_TPDOCommunicationParameter_t) != sizeof(CO_TPDOCommPar_t) ||
+        sizeof(OD_TPDOMappingParameter_t)       != sizeof(CO_TPDOMapPar_t)  ||
+        sizeof(OD_RPDOCommunicationParameter_t) != sizeof(CO_RPDOCommPar_t) ||
+        sizeof(OD_RPDOMappingParameter_t)       != sizeof(CO_RPDOMapPar_t)) {
+        return CO_ERROR_OD_PARAMETERS;
+    }
+#if CO_NO_SDO_CLIENT != 0
+    if (sizeof(OD_SDOClientParameter_t)         != sizeof(CO_SDOclientPar_t)) {
+        return CO_ERROR_OD_PARAMETERS;
+    }
+#endif
+
+#if (CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE
+    /* LEDs */
+    err = CO_LEDs_init(CO->LEDs);
+
+    if (err) return err;
+#endif
+
+#if CO_NO_LSS_SLAVE == 1
+    if (CO->nodeIdUnconfigured) {
+        return CO_ERROR_NODE_ID_UNCONFIGURED_LSS;
+    }
+#endif
+
+    /* SDOserver */
+    for (i = 0; i < CO_NO_SDO_SERVER; i++) {
+        uint32_t COB_IDClientToServer;
+        uint32_t COB_IDServerToClient;
+        if (i == 0) {
+            /*Default SDO server must be located at first index*/
+            COB_IDClientToServer = CO_CAN_ID_SDO_CLI + nodeId;
+            COB_IDServerToClient = CO_CAN_ID_SDO_SRV + nodeId;
+        }
+        else {
+            COB_IDClientToServer =
+                OD_SDOServerParameter[i].COB_IDClientToServer;
+            COB_IDServerToClient =
+                OD_SDOServerParameter[i].COB_IDServerToClient;
+        }
+
+        err = CO_SDO_init(CO->SDO[i],
+                          COB_IDClientToServer,
+                          COB_IDServerToClient,
+                          OD_H1200_SDO_SERVER_PARAM + i,
+                          i == 0 ? 0 : CO->SDO[0],
+                          &CO_OD[0],
+                          CO_OD_NoOfElements,
+                          CO_SDO_ODExtensions,
+                          nodeId,
+                          1000,
+                          CO->CANmodule[0],
+                          CO_RXCAN_SDO_SRV + i,
+                          CO->CANmodule[0],
+                          CO_TXCAN_SDO_SRV + i);
+
+        if (err) return err;
+    }
+
+
+    /* Emergency */
+    err = CO_EM_init(CO->em,
+                     CO->emPr,
+                     CO->SDO[0],
+                     &OD_errorStatusBits[0],
+                     ODL_errorStatusBits_stringLength,
+                     &OD_errorRegister,
+                     &OD_preDefinedErrorField[0],
+                     ODL_preDefinedErrorField_arrayLength,
+                     CO->CANmodule[0],
+                     CO_RXCAN_EMERG,
+                     CO->CANmodule[0],
+                     CO_TXCAN_EMERG,
+                     (uint16_t)CO_CAN_ID_EMERGENCY + nodeId);
+
+    if (err) return err;
+
+    /* NMT_Heartbeat */
+    err = CO_NMT_init(CO->NMT,
+                      CO->emPr,
+                      nodeId,
+                      500,
+                      CO->CANmodule[0],
+                      CO_RXCAN_NMT,
+                      CO_CAN_ID_NMT_SERVICE,
+                      CO->CANmodule[0],
+                      CO_TXCAN_NMT,
+                      CO_CAN_ID_NMT_SERVICE,
+                      CO->CANmodule[0],
+                      CO_TXCAN_HB,
+                      CO_CAN_ID_HEARTBEAT + nodeId);
+
+    if (err) return err;
+
+#if CO_NO_SYNC == 1
+    /* SYNC */
+    err = CO_SYNC_init(CO->SYNC,
+                       CO->em,
+                       CO->SDO[0],
+                       &CO->NMT->operatingState,
+                       OD_COB_ID_SYNCMessage,
+                       OD_communicationCyclePeriod,
+                       OD_synchronousCounterOverflowValue,
+                       CO->CANmodule[0],
+                       CO_RXCAN_SYNC,
+                       CO->CANmodule[0],
+                       CO_TXCAN_SYNC);
+
+    if (err) return err;
+#endif
+
+#if CO_NO_TIME == 1
+    /* TIME */
+    err = CO_TIME_init(CO->TIME,
+                       CO->em,
+                       CO->SDO[0],
+                       &CO->NMT->operatingState,
+                       OD_COB_ID_TIME,
+                       1000*30,
+                       CO->CANmodule[0],
+                       CO_RXCAN_TIME,
+                       CO->CANmodule[0],
+                       CO_TXCAN_TIME);
+
+    if (err) return err;
+#endif
+
+#if CO_NO_GFC == 1
+    /* GFC */
+    CO_GFC_init(CO->GFC,
+                &OD_globalFailSafeCommandParameter,
+                CO->CANmodule[0],
+                CO_RXCAN_GFC,
+                CO_CAN_ID_GFC,
+                CO->CANmodule[0],
+                CO_TXCAN_GFC,
+                CO_CAN_ID_GFC);
+#endif
+
+#if CO_NO_SRDO != 0
+    /* SRDO */
+    err = CO_SRDOGuard_init(CO->SRDOGuard,
+                      CO->SDO[0],
+                      &CO->NMT->operatingState,
+                      &OD_configurationValid,
+                      OD_H13FE_SRDO_VALID,
+                      OD_H13FF_SRDO_CHECKSUM);
+    if (err) return err;
+
+    for (i = 0; i < CO_NO_SRDO; i++) {
+        CO_CANmodule_t *CANdev = CO->CANmodule[0];
+        uint16_t CANdevRxIdx = CO_RXCAN_SRDO + 2*i;
+        uint16_t CANdevTxIdx = CO_TXCAN_SRDO + 2*i;
+
+        err = CO_SRDO_init(CO->SRDO[i],
+                           CO->SRDOGuard,
+                           CO->em,
+                           CO->SDO[0],
+                           nodeId,
+                           ((i == 0) ? CO_CAN_ID_SRDO_1 : 0),
+                           (CO_SRDOCommPar_t*)&OD_SRDOCommunicationParameter[i],
+                           (CO_SRDOMapPar_t *)&OD_SRDOMappingParameter[i],
+                           &OD_safetyConfigurationChecksum[i],
+                           OD_H1301_SRDO_1_PARAM + i,
+                           OD_H1381_SRDO_1_MAPPING + i,
+                           CANdev,
+                           CANdevRxIdx,
+                           CANdevRxIdx + 1,
+                           CANdev,
+                           CANdevTxIdx,
+                           CANdevTxIdx + 1);
+        if (err) return err;
+    }
+#endif
+
+    /* RPDO */
+    for (i = 0; i < CO_NO_RPDO; i++) {
+        CO_CANmodule_t *CANdevRx = CO->CANmodule[0];
+        uint16_t CANdevRxIdx = CO_RXCAN_RPDO + i;
+
+        err = CO_RPDO_init(CO->RPDO[i],
+                           CO->em,
+                           CO->SDO[0],
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+                           CO->SYNC,
+#endif
+                           &CO->NMT->operatingState,
+                           nodeId,
+                           ((i < 4) ? (CO_CAN_ID_RPDO_1 + i * 0x100) : 0),
+                           0,
+                           (CO_RPDOCommPar_t*)&OD_RPDOCommunicationParameter[i],
+                           (CO_RPDOMapPar_t *)&OD_RPDOMappingParameter[i],
+                           OD_H1400_RXPDO_1_PARAM + i,
+                           OD_H1600_RXPDO_1_MAPPING + i,
+                           CANdevRx,
+                           CANdevRxIdx);
+
+        if (err) return err;
+    }
+
+    /* TPDO */
+    for (i = 0; i < CO_NO_TPDO; i++) {
+        err = CO_TPDO_init(CO->TPDO[i],
+                           CO->em,
+                           CO->SDO[0],
+#if (CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE
+                           CO->SYNC,
+#endif
+                           &CO->NMT->operatingState,
+                           nodeId,
+                           ((i < 4) ? (CO_CAN_ID_TPDO_1 + i * 0x100) : 0),
+                           0,
+                           (CO_TPDOCommPar_t*)&OD_TPDOCommunicationParameter[i],
+                           (CO_TPDOMapPar_t *)&OD_TPDOMappingParameter[i],
+                           OD_H1800_TXPDO_1_PARAM + i,
+                           OD_H1A00_TXPDO_1_MAPPING + i,
+                           CO->CANmodule[0],
+                           CO_TXCAN_TPDO + i);
+
+        if (err) return err;
+    }
+
+    /* Heartbeat consumer */
+    err = CO_HBconsumer_init(CO->HBcons,
+                             CO->em,
+                             CO->SDO[0],
+                             &OD_consumerHeartbeatTime[0],
+                             CO_HBcons_monitoredNodes,
+                             CO_NO_HB_CONS,
+                             CO->CANmodule[0],
+                             CO_RXCAN_CONS_HB);
+
+    if (err) return err;
+
+#if CO_NO_SDO_CLIENT != 0
+    /* SDOclient */
+    for (i = 0; i < CO_NO_SDO_CLIENT; i++) {
+        err = CO_SDOclient_init(CO->SDOclient[i],
+                                (void *)CO->SDO[0],
+                                (CO_SDOclientPar_t *)&OD_SDOClientParameter[i],
+                                CO->CANmodule[0],
+                                CO_RXCAN_SDO_CLI + i,
+                                CO->CANmodule[0],
+                                CO_TXCAN_SDO_CLI + i);
+
+        if (err) return err;
+    }
+#endif
+
+#if CO_NO_LSS_MASTER == 1
+    /* LSSmaster */
+    err = CO_LSSmaster_init(CO->LSSmaster,
+                            CO_LSSmaster_DEFAULT_TIMEOUT,
+                            CO->CANmodule[0],
+                            CO_RXCAN_LSS_MST,
+                            CO_CAN_ID_LSS_SLV,
+                            CO->CANmodule[0],
+                            CO_TXCAN_LSS_MST,
+                            CO_CAN_ID_LSS_MST);
+
+    if (err) return err;
+#endif
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
+    /* Gateway-ascii */
+    err = CO_GTWA_init(CO->gtwa,
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO
+                       CO->SDOclient[0],
+                       500,
+                       false,
+#endif
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_NMT
+                       CO->NMT,
+#endif
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS
+                       CO->LSSmaster,
+#endif
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS
+                       CO->LEDs,
+#endif
+                       0);
+    if (err) return err;
+#endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
+
+#if CO_NO_TRACE > 0
+    /* Trace */
+    for (i = 0; i < CO_NO_TRACE; i++) {
+        CO_trace_init(CO->trace[i],
+                      CO->SDO[0],
+                      OD_traceConfig[i].axisNo,
+                      CO_traceTimeBuffers[i],
+                      CO_traceValueBuffers[i],
+                      CO_traceBufferSize[i],
+                      &OD_traceConfig[i].map,
+                      &OD_traceConfig[i].format,
+                      &OD_traceConfig[i].trigger,
+                      &OD_traceConfig[i].threshold,
+                      &OD_trace[i].value,
+                      &OD_trace[i].min,
+                      &OD_trace[i].max,
+                      &OD_trace[i].triggerTime,
+                      OD_INDEX_TRACE_CONFIG + i,
+                      OD_INDEX_TRACE + i);
+    }
+#endif
+
+    return CO_ERROR_NO;
+}
+
+
+/******************************************************************************/
+CO_NMT_reset_cmd_t CO_process(CO_t *co,
+                              uint32_t timeDifference_us,
+                              uint32_t *timerNext_us)
+{
+    uint8_t i;
+    bool_t NMTisPreOrOperational = false;
+    CO_NMT_reset_cmd_t reset = CO_RESET_NOT;
+
+    CO_CANmodule_process(CO->CANmodule[0]);
+
+#if CO_NO_LSS_SLAVE == 1
+    bool_t resetLSS = CO_LSSslave_process(co->LSSslave);
+#endif
+
+#if (CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE
+    bool_t unc = co->nodeIdUnconfigured;
+    uint16_t CANerrorStatus = CO->CANmodule[0]->CANerrorStatus;
+    CO_LEDs_process(co->LEDs,
+                    timeDifference_us,
+                    unc ? CO_NMT_INITIALIZING : co->NMT->operatingState,
+    #if CO_NO_LSS_SLAVE == 1
+                    CO_LSSslave_getState(co->LSSslave)
+                        == CO_LSS_STATE_CONFIGURATION,
+    #else
+                    false,
+    #endif
+                    (CANerrorStatus & CO_CAN_ERRTX_BUS_OFF) != 0,
+                    (CANerrorStatus & CO_CAN_ERR_WARN_PASSIVE) != 0,
+                    0, /* RPDO event timer timeout */
+                    unc ? false : CO_isError(co->em, CO_EM_SYNC_TIME_OUT),
+                    unc ? false : (CO_isError(co->em, CO_EM_HEARTBEAT_CONSUMER)
+                        || CO_isError(co->em, CO_EM_HB_CONSUMER_REMOTE_RESET)),
+                    OD_errorRegister != 0,
+                    CO_STATUS_FIRMWARE_DOWNLOAD_IN_PROGRESS,
+                    timerNext_us);
+#endif /* (CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE */
+
+#if CO_NO_LSS_SLAVE == 1
+    if (resetLSS) {
+        reset = CO_RESET_COMM;
+    }
+    if (co->nodeIdUnconfigured) {
+        return reset;
+    }
+#endif
+
+    if (co->NMT->operatingState == CO_NMT_PRE_OPERATIONAL ||
+        co->NMT->operatingState == CO_NMT_OPERATIONAL)
+        NMTisPreOrOperational = true;
+
+    /* SDOserver */
+    for (i = 0; i < CO_NO_SDO_SERVER; i++) {
+        CO_SDO_process(co->SDO[i],
+                       NMTisPreOrOperational,
+                       timeDifference_us,
+                       timerNext_us);
+    }
+
+    /* Emergency */
+    CO_EM_process(co->emPr,
+                  NMTisPreOrOperational,
+                  timeDifference_us,
+                  OD_inhibitTimeEMCY,
+                  timerNext_us);
+
+    /* NMT_Heartbeat */
+    reset = CO_NMT_process(co->NMT,
+                           timeDifference_us,
+                           OD_producerHeartbeatTime,
+                           OD_NMTStartup,
+                           OD_errorRegister,
+                           OD_errorBehavior,
+                           timerNext_us);
+
+#if CO_NO_TIME == 1
+    /* TIME */
+    CO_TIME_process(co->TIME,
+                    timeDifference_us);
+#endif
+
+    /* Heartbeat consumer */
+    CO_HBconsumer_process(co->HBcons,
+                          NMTisPreOrOperational,
+                          timeDifference_us,
+                          timerNext_us);
+
+#if (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII
+    /* Gateway-ascii */
+    CO_GTWA_process(co->gtwa,
+                    CO_GTWA_ENABLE,
+                    timeDifference_us,
+                    timerNext_us);
+#endif
+
+    return reset;
+}
+
+
+/******************************************************************************/
+#if CO_NO_SYNC == 1
+bool_t CO_process_SYNC(CO_t *co,
+                       uint32_t timeDifference_us,
+                       uint32_t *timerNext_us)
+{
+    bool_t syncWas = false;
+
+#if CO_NO_LSS_SLAVE == 1
+    if (co->nodeIdUnconfigured) {
+        return syncWas;
+    }
+#endif
+
+    const CO_SYNC_status_t sync_process = CO_SYNC_process(co->SYNC,
+                                   timeDifference_us,
+                                   OD_synchronousWindowLength,
+                                   timerNext_us);
+
+    switch (sync_process) {
+    case CO_SYNC_NONE:
+        break;
+    case CO_SYNC_RECEIVED:
+        syncWas = true;
+        break;
+    case CO_SYNC_OUTSIDE_WINDOW:
+        CO_CANclearPendingSyncPDOs(co->CANmodule[0]);
+        break;
+    }
+
+    return syncWas;
+}
+#endif /* CO_NO_SYNC == 1 */
+
+
+/******************************************************************************/
+void CO_process_RPDO(CO_t *co,
+                     bool_t syncWas)
+{
+    int16_t i;
+
+#if CO_NO_LSS_SLAVE == 1
+    if (co->nodeIdUnconfigured) {
+        return;
+    }
+#endif
+
+    for (i = 0; i < CO_NO_RPDO; i++) {
+        CO_RPDO_process(co->RPDO[i], syncWas);
+    }
+}
+
+
+/******************************************************************************/
+void CO_process_TPDO(CO_t *co,
+                     bool_t syncWas,
+                     uint32_t timeDifference_us,
+                     uint32_t *timerNext_us)
+{
+    int16_t i;
+
+#if CO_NO_LSS_SLAVE == 1
+    if (co->nodeIdUnconfigured) {
+        return;
+    }
+#endif
+
+    /* Verify PDO Change Of State and process PDOs */
+    for (i = 0; i < CO_NO_TPDO; i++) {
+        if (!co->TPDO[i]->sendRequest)
+            co->TPDO[i]->sendRequest = CO_TPDOisCOS(co->TPDO[i]);
+        CO_TPDO_process(co->TPDO[i], syncWas, timeDifference_us, timerNext_us);
+    }
+}
+
+/******************************************************************************/
+#if CO_NO_SRDO != 0
+void CO_process_SRDO(CO_t *co,
+                     uint32_t timeDifference_us,
+                     uint32_t *timerNext_us)
+{
+    int16_t i;
+    uint8_t firstOperational;
+
+#if CO_NO_LSS_SLAVE == 1
+    if (co->nodeIdUnconfigured) {
+        return;
+    }
+#endif
+
+    firstOperational = CO_SRDOGuard_process(co->SRDOGuard);
+
+    /* Verify PDO Change Of State and process PDOs */
+    for (i = 0; i < CO_NO_SRDO; i++) {
+        CO_SRDO_process(co->SRDO[i], firstOperational, timeDifference_us, timerNext_us);
+    }
+}
+#endif

controller_yy_app/middleware/CANopenNode/CANopen.h

@@ -0,0 +1,482 @@
+/**
+ * Main CANopenNode file.
+ *
+ * @file        CANopen.h
+ * @ingroup     CO_CANopen
+ * @author      Janez Paternoster
+ * @author      Uwe Kindler
+ * @copyright   2010 - 2020 Janez Paternoster
+ *
+ * This file is part of CANopenNode, an opensource CANopen Stack.
+ * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+ * For more information on CANopen see <http://www.can-cia.org/>.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+
+#ifndef CANopen_H
+#define CANopen_H
+
+#include "301/CO_driver.h"
+#include "301/CO_SDOserver.h"
+#include "301/CO_SDOclient.h"
+#include "301/CO_Emergency.h"
+#include "301/CO_NMT_Heartbeat.h"
+#include "301/CO_TIME.h"
+#include "301/CO_SYNC.h"
+#include "301/CO_PDO.h"
+#include "301/CO_HBconsumer.h"
+#include "303/CO_LEDs.h"
+#include "304/CO_GFC.h"
+#include "304/CO_SRDO.h"
+#include "305/CO_LSSslave.h"
+#include "305/CO_LSSmaster.h"
+#include "309/CO_gateway_ascii.h"
+#include "extra/CO_trace.h"
+#include "CO_OD.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup CO_CANopen CANopen
+ * @{
+ *
+ * CANopenNode is free and open source implementation of CANopen communication
+ * protocol.
+ *
+ * CANopen is the internationally standardized (EN 50325-4) (CiA DS-301)
+ * CAN-based higher-layer protocol for embedded control system. For more
+ * information on CANopen see http://www.can-cia.org/
+ *
+ * CANopenNode homepage is https://github.com/CANopenNode/CANopenNode
+ *
+ * CANopen.h file combines Object dictionary (CO_OD) and all other CANopen
+ * source files. Configuration information are read from CO_OD.h file.
+ * CO_OD.h/.c files defines CANopen Object Dictionary and are generated by
+ * external tool.
+ * CANopen.h file contains most common configuration of CANopenNode objects
+ * and can also be a template for custom, more complex configurations.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * https://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ * @}
+ */
+
+/**
+ * @defgroup CO_CANopen_301 CANopen_301
+ * @{
+ *
+ * CANopen application layer and communication profile (CiA 301 v4.2.0)
+ *
+ * Definitions of data types, encoding rules, object dictionary objects and
+ * CANopen communication services and protocols.
+ * @}
+ */
+
+/**
+ * @defgroup CO_CANopen_303 CANopen_303
+ * @{
+ *
+ * CANopen recommendation for indicator specification (CiA 303-3 v1.4.0)
+ *
+ * Description of communication related indicators - green and red LED diodes.
+ * @}
+ */
+
+/**
+ * @defgroup CO_CANopen_304 CANopen_304
+ * @{
+ *
+ * CANopen Safety (EN 50325­-5:2010 (formerly CiA 304))
+ *
+ * Standard defines the usage of Safety Related Data Objects (SRDO) and the GFC.
+ * This is an additional protocol (to SDO, PDO) to exchange data.
+ * The meaning of "security" here refers not to security (crypto) but to data consistency.
+ * @}
+ */
+
+/**
+ * @defgroup CO_CANopen_305 CANopen_305
+ * @{
+ *
+ * CANopen layer setting services (LSS) and protocols (CiA 305 DSP v3.0.0)
+ *
+ * Inquire or change three parameters on a CANopen device with LSS slave
+ * capability by a CANopen device with LSS master capability via the CAN
+ * network: the settings of Node-ID of the CANopen device, bit timing
+ * parameters of the physical layer (bit rate) or LSS address compliant to the
+ * identity object (1018h).
+ * @}
+ */
+
+/**
+ * @defgroup CO_CANopen_309 CANopen_309
+ * @{
+ *
+ * CANopen access from other networks (CiA 309)
+ *
+ * Standard defines the services and protocols to interface CANopen networks to
+ * other networks. Standard is organized as follows:
+ * - Part 1: General principles and services
+ * - Part 2: Modbus/TCP mapping
+ * - Part 3: ASCII mapping
+ * - Part 4: Amendment 7 to Fieldbus Integration into PROFINET IO
+ * @}
+ */
+
+/**
+ * @defgroup CO_CANopen_extra CANopen_extra
+ * @{
+ *
+ * Additional non-standard objects related to CANopenNode.
+ * @}
+ */
+
+/**
+ * @addtogroup CO_CANopen
+ * @{
+ */
+
+#ifdef CO_DOXYGEN
+/**
+ * @defgroup CO_NO_OBJ CANopen configuration
+ *
+ * Definitions specify, which and how many CANopenNode communication objects
+ * will be used in current configuration. Usage of some objects is mandatory and
+ * is fixed. Others are defined in CO_OD.h.
+ * @{
+ */
+/* Definitions valid only for documentation. */
+/** Number of NMT objects, fixed to 1 slave(CANrx) */
+#define CO_NO_NMT (1)
+/** Number of NMT master objects, 0 or 1 master(CANtx). It depends on
+ * @ref CO_CONFIG_NMT setting. */
+#define CO_NO_NMT_MST (0 - 1)
+/** Number of SYNC objects, 0 or 1 (consumer(CANrx) + producer(CANtx)) */
+#define CO_NO_SYNC (0 - 1)
+/** Number of Emergency producer objects, fixed to 1 producer(CANtx) */
+#define CO_NO_EMERGENCY (1)
+/** Number of Emergency consumer objects, 0 or 1 consumer(CANrx). It depends on
+ * @ref CO_CONFIG_EM setting. */
+#define CO_NO_EM_CONS (0 - 1)
+/** Number of Time-stamp objects, 0 or 1 (consumer(CANrx) + producer(CANtx)) */
+#define CO_NO_TIME (0 - 1)
+/** Number of GFC objects, 0 or 1 (consumer(CANrx) + producer(CANtx)) */
+#define CO_NO_GFC (0 - 1)
+/** Number of SRDO objects, 0 to 64 (consumer(CANrx) + producer(CANtx)) */
+#define CO_NO_RPDO (0 - 64)
+/** Number of RPDO objects, 1 to 512 consumers (CANrx) */
+#define CO_NO_RPDO (1 - 512)
+/** Number of TPDO objects, 1 to 512 producers (CANtx) */
+#define CO_NO_TPDO (1 - 512)
+/** Number of SDO server objects, from 1 to 128 (CANrx + CANtx) */
+#define CO_NO_SDO_SERVER (1 - 128)
+/** Number of SDO client objects, from 0 to 128 (CANrx + CANtx) */
+#define CO_NO_SDO_CLIENT (0 - 128)
+/** Number of HB producer objects, fixed to 1 producer(CANtx) */
+#define CO_NO_HB_PROD (1)
+/** Number of HB consumer objects, from 0 to 127 consumers(CANrx) */
+#define CO_NO_HB_CONS (0 - 127)
+/** Number of LSS slave objects, 0 or 1 (CANrx + CANtx). It depends on
+ * @ref CO_CONFIG_LSS setting. */
+#define CO_NO_LSS_SLAVE (0 - 1)
+/** Number of LSS master objects, 0 or 1 (CANrx + CANtx). It depends on
+ * @ref CO_CONFIG_LSS setting. */
+#define CO_NO_LSS_MASTER (0 - 1)
+/** Number of Trace objects, 0 to many */
+#define CO_NO_TRACE (0 - )
+/** @} */
+
+#else  /* CO_DOXYGEN */
+
+/* Valid Definitions for program. */
+/* NMT slave count (fixed) */
+#define CO_NO_NMT     1
+
+/* NMT master count depends on stack configuration */
+#if (CO_CONFIG_NMT) & CO_CONFIG_NMT_MASTER
+#define CO_NO_NMT_MST 1
+#else
+#define CO_NO_NMT_MST 0
+#endif
+
+/* Emergency consumer depends on stack configuration */
+#if (CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER
+#define CO_NO_EM_CONS 1
+#else
+#define CO_NO_EM_CONS 0
+#endif
+
+/* Heartbeat producer count (fixed) */
+#define CO_NO_HB_PROD 1
+
+/* Heartbeat consumer count depends on Object Dictionary configuration */
+#ifdef ODL_consumerHeartbeatTime_arrayLength
+    #define CO_NO_HB_CONS ODL_consumerHeartbeatTime_arrayLength
+#else
+    #define CO_NO_HB_CONS 0
+#endif
+
+/* LSS slave count depends on stack configuration */
+#if (CO_CONFIG_LSS) & CO_CONFIG_LSS_SLAVE
+#define CO_NO_LSS_SLAVE 1
+#else
+#define CO_NO_LSS_SLAVE 0
+#endif
+
+/* LSS master count depends on stack configuration */
+#if (CO_CONFIG_LSS) & CO_CONFIG_LSS_MASTER
+#define CO_NO_LSS_MASTER 1
+#else
+#define CO_NO_LSS_MASTER 0
+#endif
+#endif /* CO_DOXYGEN */
+
+
+/**
+ * CANopen object with pointers to all CANopenNode objects.
+ */
+typedef struct {
+    bool_t nodeIdUnconfigured;       /**< True in unconfigured LSS slave */
+    CO_CANmodule_t *CANmodule[1];    /**< CAN module objects */
+    CO_SDO_t *SDO[CO_NO_SDO_SERVER]; /**< SDO object */
+    CO_EM_t *em;                     /**< Emergency report object */
+    CO_EMpr_t *emPr;                 /**< Emergency process object */
+    CO_NMT_t *NMT;                   /**< NMT object */
+#if CO_NO_SYNC == 1 || defined CO_DOXYGEN
+    CO_SYNC_t *SYNC;                 /**< SYNC object */
+#endif
+#if CO_NO_TIME == 1 || defined CO_DOXYGEN
+    CO_TIME_t *TIME;                 /**< TIME object */
+#endif
+    CO_RPDO_t *RPDO[CO_NO_RPDO];     /**< RPDO objects */
+    CO_TPDO_t *TPDO[CO_NO_TPDO];     /**< TPDO objects */
+    CO_HBconsumer_t *HBcons;         /**< Heartbeat consumer object*/
+#if CO_NO_SDO_CLIENT != 0 || defined CO_DOXYGEN
+    CO_SDOclient_t *SDOclient[CO_NO_SDO_CLIENT]; /**< SDO client object */
+#endif
+#if ((CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE) || defined CO_DOXYGEN
+    CO_LEDs_t *LEDs;                 /**< LEDs object */
+#endif
+#if CO_NO_GFC != 0 || defined CO_DOXYGEN
+    CO_GFC_t *GFC;                   /**< GFC objects */
+#endif
+#if CO_NO_SRDO != 0 || defined CO_DOXYGEN
+    CO_SRDOGuard_t *SRDOGuard;       /**< SRDO objects */
+    CO_SRDO_t *SRDO[CO_NO_SRDO];     /**< SRDO objects */
+#endif
+#if CO_NO_LSS_SLAVE == 1 || defined CO_DOXYGEN
+    CO_LSSslave_t *LSSslave;         /**< LSS slave object */
+#endif
+#if CO_NO_LSS_MASTER == 1 || defined CO_DOXYGEN
+    CO_LSSmaster_t *LSSmaster;       /**< LSS master object */
+#endif
+#if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII) || defined CO_DOXYGEN
+    CO_GTWA_t *gtwa;                /**< Gateway-ascii object (CiA309-3) */
+#endif
+#if CO_NO_TRACE > 0 || defined CO_DOXYGEN
+    CO_trace_t *trace[CO_NO_TRACE]; /**< Trace object for recording variables */
+#endif
+} CO_t;
+
+
+/** CANopen object */
+extern CO_t *CO;
+
+
+/**
+ * Allocate and initialize memory for CANopen object
+ *
+ * Function must be called the first time, after program starts.
+ *
+ * @remark
+ * With some microcontrollers it is necessary to specify Heap size within
+ * linker configuration.
+ *
+ * @param [out] heapMemoryUsed Information about heap memory used. Ignored if
+ * NULL.
+ *
+ * @return #CO_ReturnError_t: CO_ERROR_NO, CO_ERROR_ILLEGAL_ARGUMENT,
+ * CO_ERROR_OUT_OF_MEMORY
+ */
+CO_ReturnError_t CO_new(uint32_t *heapMemoryUsed);
+
+
+/**
+ * Delete CANopen object and free memory. Must be called at program exit.
+ *
+ * @param CANptr Pointer to the user-defined CAN base structure, passed to
+ *               CO_CANmodule_init().
+ */
+void CO_delete(void *CANptr);
+
+
+/**
+ * Initialize CAN driver
+ *
+ * Function must be called in the communication reset section.
+ *
+ * @param CANptr Pointer to the user-defined CAN base structure, passed to
+ *               CO_CANmodule_init().
+ * @param bitRate CAN bit rate.
+ * @return #CO_ReturnError_t: CO_ERROR_NO, CO_ERROR_ILLEGAL_ARGUMENT,
+ * CO_ERROR_ILLEGAL_BAUDRATE, CO_ERROR_OUT_OF_MEMORY
+ */
+CO_ReturnError_t CO_CANinit(void *CANptr,
+                            uint32_t bitRate);
+
+
+#if CO_NO_LSS_SLAVE == 1 || defined CO_DOXYGEN
+/**
+ * Initialize CANopen LSS slave
+ *
+ * Function must be called before CO_CANopenInit.
+ *
+ * See #CO_LSSslave_init() for description of parameters.
+ *
+ * @param [in,out] pendingNodeID Pending node ID or 0xFF(unconfigured)
+ * @param [in,out] pendingBitRate Pending bit rate of the CAN interface
+ * @return #CO_ReturnError_t: CO_ERROR_NO, CO_ERROR_ILLEGAL_ARGUMENT
+ */
+CO_ReturnError_t CO_LSSinit(uint8_t *pendingNodeID,
+                            uint16_t *pendingBitRate);
+#endif /* CO_NO_LSS_SLAVE == 1 */
+
+
+/**
+ * Initialize CANopenNode.
+ *
+ * Function must be called in the communication reset section.
+ *
+ * @param nodeId CANopen Node ID (1 ... 127) or 0xFF(unconfigured). In the
+ * CANopen initialization it is the same as pendingBitRate from CO_LSSinit().
+ * If it is unconfigured, then some CANopen objects will not be initialized nor
+ * processed.
+ * @return #CO_ReturnError_t: CO_ERROR_NO, CO_ERROR_ILLEGAL_ARGUMENT
+ */
+CO_ReturnError_t CO_CANopenInit(uint8_t nodeId);
+
+
+/**
+ * Process CANopen objects.
+ *
+ * Function must be called cyclically. It processes all "asynchronous" CANopen
+ * objects.
+ *
+ * @param co CANopen object.
+ * @param timeDifference_us Time difference from previous function call in
+ *                          microseconds.
+ * @param [out] timerNext_us info to OS - maximum delay time after this function
+ *        should be called next time in [microseconds]. Value can be used for OS
+ *        sleep time. Initial value must be set to maximum interval time.
+ *        Output will be equal or lower to initial value. Calculation is based
+ *        on various timers which expire in known time. Parameter should be
+ *        used in combination with callbacks configured with
+ *        CO_***_initCallbackPre() functions. Those callbacks should also
+ *        trigger calling of CO_process() function. Parameter is ignored if
+ *        NULL. See also @ref CO_CONFIG_FLAG_CALLBACK_PRE configuration macro.
+ *
+ * @return #CO_NMT_reset_cmd_t from CO_NMT_process().
+ */
+CO_NMT_reset_cmd_t CO_process(CO_t *co,
+                              uint32_t timeDifference_us,
+                              uint32_t *timerNext_us);
+
+
+#if CO_NO_SYNC == 1 || defined CO_DOXYGEN
+/**
+ * Process CANopen SYNC objects.
+ *
+ * Function must be called cyclically from real time thread with constant
+ * interval (1ms typically). It processes SYNC CANopen objects.
+ *
+ * @param co CANopen object.
+ * @param timeDifference_us Time difference from previous function call in
+ * microseconds.
+ * @param [out] timerNext_us info to OS - see CO_process().
+ *
+ * @return True, if CANopen SYNC message was just received or transmitted.
+ */
+bool_t CO_process_SYNC(CO_t *co,
+                       uint32_t timeDifference_us,
+                       uint32_t *timerNext_us);
+#endif /* CO_NO_SYNC == 1 */
+
+
+/**
+ * Process CANopen RPDO objects.
+ *
+ * Function must be called cyclically from real time thread with constant.
+ * interval (1ms typically). It processes receive PDO CANopen objects.
+ *
+ * @param co CANopen object.
+ * @param syncWas True, if CANopen SYNC message was just received or
+ * transmitted.
+ */
+void CO_process_RPDO(CO_t *co, bool_t syncWas);
+
+
+/**
+ * Process CANopen TPDO objects.
+ *
+ * Function must be called cyclically from real time thread with constant.
+ * interval (1ms typically). It processes transmit PDO CANopen objects.
+ *
+ * @param co CANopen object.
+ * @param syncWas True, if CANopen SYNC message was just received or
+ * transmitted.
+ * @param timeDifference_us Time difference from previous function call in
+ * microseconds.
+ * @param [out] timerNext_us info to OS - see CO_process().
+ */
+void CO_process_TPDO(CO_t *co,
+                     bool_t syncWas,
+                     uint32_t timeDifference_us,
+                     uint32_t *timerNext_us);
+
+#if CO_NO_SRDO != 0 || defined CO_DOXYGEN
+/**
+ * Process CANopen SRDO objects.
+ *
+ * Function must be called cyclically from real time thread with constant.
+ * interval (1ms typically). It processes receive SRDO CANopen objects.
+ *
+ * @param co CANopen object.
+ * @param timeDifference_us Time difference from previous function call in
+ * microseconds.
+ * @param [out] timerNext_us info to OS - see CO_process().
+ */
+void CO_process_SRDO(CO_t *co,
+                     uint32_t timeDifference_us,
+                     uint32_t *timerNext_us);
+#endif /* CO_NO_SRDO != 0 */
+
+/** @} */ /* CO_CANopen */
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* CANopen_H */

controller_yy_app/middleware/CANopenNode/CMakeLists.txt

@@ -0,0 +1,12 @@
+# Copyright (c) 2021-2024 HPMicro
+# SPDX-License-Identifier: BSD-3-Clause
+
+sdk_inc(.)
+sdk_src(CANopen.c)
+add_subdirectory(301)
+add_subdirectory(303)
+add_subdirectory(304)
+add_subdirectory(305)
+add_subdirectory(309)
+add_subdirectory(extra)
+add_subdirectory(port)

controller_yy_app/middleware/CANopenNode/LICENSE

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.