97 lines
3.0 KiB
C
97 lines
3.0 KiB
C
#include <stdio.h>
|
||
#include <stdlib.h>
|
||
#include "freertos/FreeRTOS.h"
|
||
#include "freertos/task.h"
|
||
#include "freertos/queue.h"
|
||
#include "freertos/semphr.h"
|
||
#include "esp_err.h"
|
||
#include "esp_log.h"
|
||
#include "driver/twai.h"
|
||
#include "modbus.h"
|
||
|
||
#define TX_GPIO_NUM 42
|
||
#define RX_GPIO_NUM 41
|
||
#define RX_BUF_LEN (8)
|
||
|
||
static const char *TAG = "twal";
|
||
|
||
static const twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(TX_GPIO_NUM, RX_GPIO_NUM, TWAI_MODE_NORMAL);
|
||
static const twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS();
|
||
static const twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
|
||
|
||
|
||
static void twai_receive_task(void *arg)
|
||
{
|
||
ESP_LOGI(TAG, "twai_receive_task");
|
||
|
||
uint8_t rx_data[RX_BUF_LEN];
|
||
int rx_len = 0;
|
||
twai_message_t message;
|
||
|
||
while (1)
|
||
{
|
||
/* 接受一个modbus帧 */
|
||
if (twai_receive(&message, portMAX_DELAY) == ESP_OK) {
|
||
/* 远程帧据不处理 */
|
||
if (message.rtr) {
|
||
ESP_LOGI(TAG, "remote frame, return");
|
||
}
|
||
else if (message.data_length_code != 4) {
|
||
ESP_LOGI(TAG, "Invalid data length");
|
||
}
|
||
else {
|
||
modbus_data->angle = (message.data[1] << 8) | message.data[0];
|
||
modbus_data->speed = (message.data[3] << 8) | message.data[2];
|
||
ESP_LOGI(TAG, "angle: %d, speed: %d", modbus_data->angle, modbus_data->speed);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
ESP_LOGI(TAG, "Failed to receive message");
|
||
}
|
||
|
||
/* 因为设计的缺陷，这里要手动调用modbus的回调来调方向 */
|
||
ModBusWordWriteHook(MODBUS_ANGLE_ADDR, 1);
|
||
}
|
||
}
|
||
|
||
|
||
static void twai_send_task(void *arg)
|
||
{
|
||
ESP_LOGI(TAG, "twai_send_task");
|
||
|
||
twai_message_t message;
|
||
while (1)
|
||
{
|
||
message.identifier = 0xAAAA;
|
||
message.extd = 1;
|
||
message.data_length_code = 8;
|
||
message.data[0] = modbus_data->encoder_accumul & 0xff;
|
||
message.data[1] = (modbus_data->encoder_accumul >> 8) & 0xff;
|
||
message.data[2] = (modbus_data->encoder_accumul >> 16) & 0xff;
|
||
message.data[3] = (modbus_data->encoder_accumul >> 24) & 0xff;
|
||
message.data[4] = modbus_data->encoder_accumur & 0xff;
|
||
message.data[5] = (modbus_data->encoder_accumur >> 8) & 0xff;
|
||
message.data[6] = (modbus_data->encoder_accumur >> 16) & 0xff;
|
||
message.data[7] = (modbus_data->encoder_accumur >> 24) & 0xff;
|
||
|
||
if (twai_transmit(&message, pdMS_TO_TICKS(1000)) != ESP_OK) {
|
||
ESP_LOGI(TAG, "Failed to queue message for transmission");
|
||
}
|
||
|
||
vTaskDelay(pdMS_TO_TICKS(100));
|
||
}
|
||
}
|
||
|
||
|
||
void twai_init(void)
|
||
{
|
||
ESP_LOGI(TAG, "twai init");
|
||
|
||
ESP_ERROR_CHECK(twai_driver_install(&g_config, &t_config, &f_config));
|
||
ESP_ERROR_CHECK(twai_start());
|
||
|
||
xTaskCreatePinnedToCore(twai_receive_task, "TWAI_rx", 4096, NULL, 9, NULL, tskNO_AFFINITY);
|
||
xTaskCreatePinnedToCore(twai_send_task, "TWAI_tx", 4096, NULL, 8, NULL, tskNO_AFFINITY);
|
||
}
|