Add initial support of ThingSet via CAN ISO-TP

Author: martinjaeger

main/CMakeLists.txt

@@ -9,6 +9,8 @@ set(app_sources
 	"web_fs.c"
 	"web_server.c"
 	"provisioning.c"
+	"../lib/isotp/isotp.c"
+	"isotp_user.c"
 )
 
 idf_component_register(SRCS ${app_sources} INCLUDE_DIRS ".")

main/can.c

@@ -21,11 +21,28 @@
 #include "driver/can.h"
 #include "driver/gpio.h"
 
+#include "../lib/isotp/isotp.h"
+#include "../lib/isotp/isotp_defines.h"
+
+static const char *TAG = "can";
+
 bool update_bms_received = false;
 bool update_mppt_received = false;
 
 #if CONFIG_THINGSET_CAN
 
+#define ISOTP_BUFSIZE 512
+
+/* Alloc IsoTpLink statically in RAM */
+static IsoTpLink isotp_link;
+
+/* Alloc send and receive buffer statically in RAM */
+static uint8_t isotp_recv_buf[ISOTP_BUFSIZE];
+static uint8_t isotp_send_buf[ISOTP_BUFSIZE];
+
+uint32_t can_addr_client = 0xF1;     // this device
+uint32_t can_addr_server = 0x14;     // select MPPT or BMS
+
 // buffer for JSON string generated from received data objects via CAN
 static char json_buf[500];
 
@@ -160,7 +177,6 @@ static int generate_json_string(char *buf, size_t len, DataObject *objs, size_t
     return pos;
 }
 
-
 char *get_mppt_json_data()
 {
     generate_json_string(json_buf, sizeof(json_buf),
@@ -184,69 +200,103 @@ void can_setup()
     gpio_set_level(CONFIG_GPIO_CAN_STB, 0);
 #endif
 
-    if (can_driver_install(&g_config, &t_config, &f_config) == ESP_OK) {
-        printf("CAN driver installed\n");
-    }
-    else {
-        printf("Failed to install CAN driver\n");
+    if (can_driver_install(&g_config, &t_config, &f_config) != ESP_OK) {
+        ESP_LOGE(TAG, "Failed to install CAN driver");
         return;
     }
 
-    if (can_start() == ESP_OK) {
-        printf("CAN driver started\n");
-    }
-    else {
-        printf("Failed to start CAN driver\n");
+    if (can_start() != ESP_OK) {
+        ESP_LOGE(TAG, "Failed to start CAN driver");
         return;
     }
+
+    /* Initialize link with the CAN ID we send with */
+    isotp_init_link(&isotp_link, can_addr_server << 8 | can_addr_client | 0x1ada << 16,
+        isotp_send_buf, sizeof(isotp_send_buf), isotp_recv_buf, sizeof(isotp_recv_buf));
 }
 
 void can_receive_task(void *arg)
 {
     can_message_t message;
-    //unsigned int msg_priority;        // currently not used
-    unsigned int node_id;
-    unsigned int data_object_id;
+    unsigned int device_addr;
+    unsigned int data_node_id;
+
+    uint8_t payload[500];
 
     while (1) {
         if (can_receive(&message, pdMS_TO_TICKS(10000)) == ESP_OK) {
 
-            // ThingSet publication message format: https://thingset.github.io/spec/can
-            //msg_priority = message.identifier >> 26;
-            node_id = message.identifier & 0x000000FF;
-            data_object_id = (message.identifier >> 8) & 0x000000FF;
-
-            if (node_id == 0) {
-                for (int i = 0; i < sizeof(data_obj_bms)/sizeof(DataObject); i++) {
-                    if (data_obj_bms[i].id == data_object_id) {
-                        memcpy(data_obj_bms[i].raw_data, message.data, message.data_length_code);
-                        data_obj_bms[i].len = message.data_length_code;
-                    }
+            /* checking for CAN ID used to receive ISO-TP frames */
+            if (message.identifier == (can_addr_client << 8 | can_addr_server | 0x1ada << 16)) {
+                ESP_LOGI(TAG, "ISO TP msg part received");
+                isotp_on_can_message(&isotp_link, message.data, message.data_length_code);
+
+                /* process multiple frame transmissions and timeouts */
+                isotp_poll(&isotp_link);
+
+                /* extract received data */
+                uint16_t out_size;
+                int ret = isotp_receive(&isotp_link, payload, sizeof(payload) - 1, &out_size);
+                if (ret == ISOTP_RET_OK) {
+                    payload[out_size] = '\0';
+                    ESP_LOGI(TAG, "Received %d bytes via ISO-TP: %s", out_size, payload);
+                    /* now handle received message */
                 }
-                update_bms_received = true;
             }
-            else if (node_id == 10) {
-                for (int i = 0; i < sizeof(data_obj_mppt)/sizeof(DataObject); i++) {
-                    if (data_obj_mppt[i].id == data_object_id) {
-                        memcpy(data_obj_mppt[i].raw_data, message.data, message.data_length_code);
-                        data_obj_mppt[i].len = message.data_length_code;
+            else {
+                // ThingSet publication message format: https://thingset.github.io/spec/can
+                device_addr = message.identifier & 0x000000FF;
+                data_node_id = (message.identifier >> 8) & 0x0000FFFF;
+
+                if (device_addr == 0) {
+                    for (int i = 0; i < sizeof(data_obj_bms)/sizeof(DataObject); i++) {
+                        if (data_obj_bms[i].id == data_node_id) {
+                            memcpy(data_obj_bms[i].raw_data, message.data, message.data_length_code);
+                            data_obj_bms[i].len = message.data_length_code;
+                        }
                     }
+                    update_bms_received = true;
+                }
+                else if (device_addr == 10) {
+                    for (int i = 0; i < sizeof(data_obj_mppt)/sizeof(DataObject); i++) {
+                        if (data_obj_mppt[i].id == data_node_id) {
+                            memcpy(data_obj_mppt[i].raw_data, message.data, message.data_length_code);
+                            data_obj_mppt[i].len = message.data_length_code;
+                        }
+                    }
+                    update_mppt_received = true;
                 }
-                update_mppt_received = true;
-            }
 
-            printf("CAN msg node %u, data object 0x%.2x = 0x",
-                node_id, data_object_id);
-            if (!(message.flags & CAN_MSG_FLAG_RTR)) {
-                for (int i = 0; i < message.data_length_code; i++) {
-                    printf("%.2x", message.data[i]);
+                printf("CAN device addr %u, data node 0x%.2x = 0x", device_addr, data_node_id);
+                if (!(message.flags & CAN_MSG_FLAG_RTR)) {
+                    for (int i = 0; i < message.data_length_code; i++) {
+                        printf("%.2x", message.data[i]);
+                    }
                 }
+                printf("\n");
             }
-            printf("\n");
         }
     }
 }
 
+void isotp_task(void *arg)
+{
+    //uint8_t ts_request[] = { 0x01, 0x18, 0x70, 0xA0 };
+    uint8_t ts_request[] = "?output";
+
+    while (1) {
+
+        int ret = isotp_send(&isotp_link, ts_request, strlen((char *)ts_request));
+        if (ISOTP_RET_OK == ret) {
+            printf("ISOTP Send OK\n");
+        } else {
+            printf("ISOTP Send ERROR\n");
+        }
+
+        vTaskDelay(3000 / portTICK_PERIOD_MS);
+    }
+}
+
 #else /* not CONFIG_THINGSET_CAN */
 
 char *get_mppt_json_data()

main/can.h

@@ -34,6 +34,11 @@ void can_setup();
  */
 void can_receive_task(void *arg);
 
+/**
+ * Thread performing regular requests to other devices using ISO-TP
+ */
+void isotp_task(void *arg);
+
 /**
  * Get data from MPPT connected via CAN bus and convert it to JSON
  *

main/isotp_user.c

@@ -0,0 +1,53 @@
+/*
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Copyright (c) 2020 Martin Jäger / Libre Solar
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "esp_system.h"
+#include "esp_err.h"
+#include "esp_log.h"
+
+#include "driver/can.h"
+#include "driver/gpio.h"
+
+#include "../lib/isotp/isotp.h"
+
+/*
+ * required, this must send a single CAN message with the given arbitration
+ * ID (i.e. the CAN message ID) and data. The size will never be more than 8
+ * bytes.
+ */
+int isotp_user_send_can(const uint32_t arbitration_id,
+                        const uint8_t* data, const uint8_t size)
+{
+    can_message_t msg;
+    memcpy(msg.data, data, size);
+    msg.data_length_code = size;
+    msg.identifier = arbitration_id;
+    msg.flags = CAN_MSG_FLAG_EXTD;
+    return can_transmit(&msg, 0);
+}
+
+/*
+ * required, return system tick, unit is millisecond
+ */
+uint32_t isotp_user_get_ms(void)
+{
+    return esp_timer_get_time() / 1000;
+}
+
+/*
+ * optional, provide to receive debugging log messages
+ */
+void isotp_user_debug(const char* message, ...)
+{
+	va_list argp;
+	va_start(argp, message);
+	printf(message, argp);
+	va_end(argp);
+}

main/main.c

@@ -60,6 +60,8 @@ void app_main(void)
     can_setup();
     xTaskCreatePinnedToCore(can_receive_task, "CAN_rx", 4096,
         NULL, RX_TASK_PRIO, NULL, 1);
+    xTaskCreatePinnedToCore(isotp_task, "CAN_isotp", 1024,
+        NULL, RX_TASK_PRIO, NULL, 1);
 #endif
 
 #if CONFIG_THINGSET_SERIAL