405 lines
18 KiB
C
405 lines
18 KiB
C
#include <stdio.h>
|
||
#include <stdlib.h>
|
||
#include <string.h>
|
||
#include "freertos/FreeRTOS.h"
|
||
#include "freertos/task.h"
|
||
#include "freertos/queue.h"
|
||
#include "freertos/semphr.h"
|
||
#include "esp_system.h"
|
||
#include "esp_err.h"
|
||
#include "esp_log.h"
|
||
#include "driver/twai.h"
|
||
|
||
#include "twai_communication.h"
|
||
#include "../lis3dsh/shake_detect.h"
|
||
#include "../twai_ota/twai_ota.h"
|
||
|
||
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();
|
||
bool twai_fft_array_data_send_enable = false;
|
||
|
||
|
||
// twai的数据只有8byte，因此，不在data中用多余的字节来表示数据类别
|
||
// 使用identifier的低2byte分别表示cmd 和subcmd，即三级命令
|
||
static void twai_receive_task(void *arg)
|
||
{
|
||
ESP_LOGI(TAG, "twai_receive_task");
|
||
twai_message_t rx_msg;
|
||
twai_message_t cmd_message = {.extd = 1, .identifier = CMD_RESP_ID, .data_length_code = 8, .data = {0, 0, 0, 0, 0, 0, 0, 0}};
|
||
twai_message_t ota_resp_message = {.extd = 1, .identifier = OTA_RESP_ID, .data_length_code = 8, .data = {0, 0, 0, 0, 0, 0, 0, 0}};
|
||
uint32_t identifier = 0;
|
||
uint8_t cmd = 0;
|
||
uint8_t sub_cmd = 0;
|
||
|
||
while (1)
|
||
{
|
||
memset(rx_msg.data, 0, 8);
|
||
esp_err_t err = twai_receive(&rx_msg, portMAX_DELAY);
|
||
if (err == ESP_OK)
|
||
{
|
||
memset(cmd_message.data, 0, 8);
|
||
memset(ota_resp_message.data, 0, 8);
|
||
identifier = rx_msg.identifier & 0xffff0000;
|
||
cmd = (rx_msg.identifier >> 8) & 0xff;
|
||
sub_cmd = rx_msg.identifier & 0xff;
|
||
ESP_LOGI(TAG, "identifer=0x%x cmd: 0x%02x, sub_cmd: 0x%02x", (unsigned int)identifier, (unsigned int)cmd, (unsigned int)sub_cmd);
|
||
if (identifier == MEASUREMENT_ID)
|
||
{
|
||
switch (cmd)
|
||
{
|
||
case MC_READ_LISDSH_CONFIG:
|
||
// 读lisdsh配置参数
|
||
switch (sub_cmd)
|
||
{
|
||
case MLCS_FSCALE:
|
||
uint8_t fscale = lis3dsh_get_fscale();
|
||
cmd_message.data[0] = fscale;
|
||
cmd_message.data_length_code = 1;
|
||
break;
|
||
case MLCS_ODR:
|
||
uint8_t odr = lis3dsh_get_odr();
|
||
cmd_message.data[0] = odr;
|
||
cmd_message.data_length_code = 1;
|
||
break;
|
||
default:
|
||
cmd_message.data[0] = CMD_ERR;
|
||
cmd_message.data_length_code = 1;
|
||
ESP_LOGI(TAG, "sub_cmd[0x%02x] none", (unsigned int)sub_cmd);
|
||
break;
|
||
}
|
||
break;
|
||
case MC_READ_SHAKE_CONFIG:
|
||
// 读震动检测配置参数
|
||
switch (sub_cmd)
|
||
{
|
||
case MSCS_FEQ_RANGE:
|
||
memcpy(cmd_message.data + 0, &shake_cfg.feq_min_id, 2);
|
||
memcpy(cmd_message.data + 2, &shake_cfg.feq_max_id, 2);
|
||
cmd_message.data_length_code = 4;
|
||
break;
|
||
case MSCS_AMP_TH:
|
||
{
|
||
memcpy(cmd_message.data + 0, &shake_cfg.enter_amplitude_th, 4);
|
||
memcpy(cmd_message.data + 4, &shake_cfg.exit_amplitude_th, 4);
|
||
cmd_message.data_length_code = 8;
|
||
break;
|
||
}
|
||
case MSCS_CHANNAL:
|
||
cmd_message.data[0] = shake_cfg.channel1;
|
||
cmd_message.data[1] = shake_cfg.channel2;
|
||
cmd_message.data[2] = shake_cfg.select;
|
||
cmd_message.data_length_code = 3;
|
||
break;
|
||
default:
|
||
cmd_message.data[0] = CMD_ERR;
|
||
cmd_message.data_length_code = 1;
|
||
ESP_LOGI(TAG, "sub_cmd[0x%02x] none", (unsigned int)sub_cmd);
|
||
break;
|
||
}
|
||
break;
|
||
case MC_READ_MEASURE:
|
||
// 读测量数据
|
||
switch (sub_cmd)
|
||
{
|
||
case MDS_ACCELERATION_CX:
|
||
{
|
||
uint16_t cnt = (buf_r - 1 + N_SAMPLES) % N_SAMPLES;
|
||
memcpy(cmd_message.data + 0, &cnt, 2);
|
||
memcpy(cmd_message.data + 4, &lis3dsh_data[cnt].a[0], 4);
|
||
cmd_message.data_length_code = 8;
|
||
break;
|
||
}
|
||
case MDS_ACCELERATION_YZ:
|
||
{
|
||
uint16_t cnt = (buf_r - 1 + N_SAMPLES) % N_SAMPLES;
|
||
memcpy(cmd_message.data + 0, &lis3dsh_data[cnt].a[1], 4);
|
||
memcpy(cmd_message.data + 4, &lis3dsh_data[cnt].a[2], 4);
|
||
cmd_message.data_length_code = 8;
|
||
break;
|
||
}
|
||
case MDS_FFT:
|
||
{
|
||
uint16_t cnt = fft_data.count;
|
||
memcpy(cmd_message.data + 0, &cnt, 2);
|
||
cmd_message.data[2] = fft_data.is_shake;
|
||
cmd_message.data_length_code = 3;
|
||
break;
|
||
}
|
||
case MDS_ANGLE_CX:
|
||
{
|
||
uint16_t cnt = angle_data.count;
|
||
memcpy(cmd_message.data + 0, &cnt, 2);
|
||
memcpy(cmd_message.data + 4, &angle_data.x, 4);
|
||
cmd_message.data_length_code = 8;
|
||
break;
|
||
}
|
||
case MDS_ANGLE_YZ:
|
||
{
|
||
memcpy(cmd_message.data + 0, &angle_data.y, 4);
|
||
memcpy(cmd_message.data + 4, &angle_data.z, 4);
|
||
cmd_message.data_length_code = 8;
|
||
break;
|
||
}
|
||
default:
|
||
{
|
||
cmd_message.data[0] = CMD_ERR;
|
||
cmd_message.data_length_code = 1;
|
||
ESP_LOGI(TAG, "sub_cmd[0x%02x] none", (unsigned int)sub_cmd);
|
||
break;
|
||
}
|
||
}
|
||
break;
|
||
|
||
case MC_WRITE_LISDSH_CONFIG:
|
||
// 写lis3dsh配置参数
|
||
switch (sub_cmd)
|
||
{
|
||
case MLCS_FSCALE:
|
||
{
|
||
if (rx_msg.data_length_code < 1)
|
||
{
|
||
cmd_message.data[0] = CMD_ERR;
|
||
cmd_message.data_length_code = 1;
|
||
ESP_LOGI(TAG, "cmd data err, no data field.");
|
||
break;
|
||
}
|
||
uint8_t fscale = rx_msg.data[0];
|
||
cmd_message.data[0] = lis3dsh_set_fscale(fscale);
|
||
cmd_message.data_length_code = 1;
|
||
break;
|
||
}
|
||
case MLCS_ODR:
|
||
{
|
||
if (rx_msg.data_length_code < 1)
|
||
{
|
||
cmd_message.data[0] = CMD_ERR;
|
||
cmd_message.data_length_code = 1;
|
||
ESP_LOGI(TAG, "cmd data err, no data field.");
|
||
break;
|
||
}
|
||
uint8_t ord = rx_msg.data[0];
|
||
cmd_message.data[0] = lis3dsh_set_odr(ord);
|
||
cmd_message.data_length_code = 1;
|
||
break;
|
||
}
|
||
default:
|
||
{
|
||
cmd_message.data[0] = CMD_ERR;
|
||
cmd_message.data_length_code = 1;
|
||
ESP_LOGI(TAG, "sub_cmd[0x%02x] none", (unsigned int)sub_cmd);
|
||
break;
|
||
}
|
||
}
|
||
break;
|
||
case MC_WRITE_SHAKE_CONFIG:
|
||
// 写shake配置参数
|
||
switch (sub_cmd)
|
||
{
|
||
case MSCS_FEQ_RANGE:
|
||
{
|
||
if (rx_msg.data_length_code < 4)
|
||
{
|
||
cmd_message.data[0] = CMD_ERR;
|
||
cmd_message.data_length_code = 1;
|
||
ESP_LOGI(TAG, "cmd data err, no data field.");
|
||
break;
|
||
}
|
||
memcpy(&shake_cfg.feq_min_id, rx_msg.data + 0, 2);
|
||
memcpy(&shake_cfg.feq_max_id, rx_msg.data + 2, 2);
|
||
cmd_message.data[0] = CMD_OK;
|
||
cmd_message.data_length_code = 1;
|
||
break;
|
||
}
|
||
case MSCS_AMP_TH:
|
||
{
|
||
if (rx_msg.data_length_code < 8)
|
||
{
|
||
cmd_message.data[0] = CMD_ERR;
|
||
cmd_message.data_length_code = 1;
|
||
ESP_LOGI(TAG, "cmd data err, no data field.");
|
||
break;
|
||
}
|
||
memcpy(&shake_cfg.enter_amplitude_th, rx_msg.data + 0, 4);
|
||
memcpy(&shake_cfg.exit_amplitude_th, rx_msg.data + 4, 4);
|
||
cmd_message.data[0] = CMD_OK;
|
||
cmd_message.data_length_code = 1;
|
||
break;
|
||
}
|
||
case MSCS_CHANNAL:
|
||
{
|
||
if (rx_msg.data_length_code < 3)
|
||
{
|
||
cmd_message.data[0] = CMD_ERR;
|
||
cmd_message.data_length_code = 1;
|
||
ESP_LOGI(TAG, "cmd data err, no data field.");
|
||
break;
|
||
}
|
||
shake_cfg.channel1 = rx_msg.data[0];
|
||
shake_cfg.channel2 = rx_msg.data[1];
|
||
shake_cfg.select = rx_msg.data[2];
|
||
cmd_message.data[0] = CMD_OK;
|
||
cmd_message.data_length_code = 1;
|
||
break;
|
||
}
|
||
default:
|
||
{
|
||
cmd_message.data[0] = CMD_ERR;
|
||
cmd_message.data_length_code = 1;
|
||
ESP_LOGI(TAG, "sub_cmd[0x%02x] none", (unsigned int)sub_cmd);
|
||
break;
|
||
}
|
||
}
|
||
break;
|
||
|
||
case MC_LISDSH_CONFIG_RESTORE_DEFAULT:
|
||
// 恢复lis3dsh默认配置
|
||
cmd_message.data[0] = lis3dsh_config_restore_default();
|
||
cmd_message.data_length_code = 1;
|
||
break;
|
||
case MC_SHAKE_CONFIG_RESTORE_DEFAULT:
|
||
// 恢复shake默认配置
|
||
cmd_message.data[0] = shake_config_restore_default();
|
||
cmd_message.data_length_code = 1;
|
||
break;
|
||
|
||
case MC_LISDSH_CONFIG_SAVE:
|
||
// 保存lis3dsh配置参数
|
||
cmd_message.data[0] = lis3dsh_config_nvs_save();
|
||
cmd_message.data_length_code = 1;
|
||
break;
|
||
case MC_SHAKE_CONFIG_SAVE:
|
||
// 保存shake配置参数
|
||
cmd_message.data[0] = shake_config_nvs_save();
|
||
cmd_message.data_length_code = 1;
|
||
break;
|
||
case MC_OPEN_DATA_SEND:
|
||
twai_fft_array_data_send_enable = rx_msg.data[0];
|
||
cmd_message.data[0] = CMD_OK;
|
||
cmd_message.data_length_code = 1;
|
||
break;
|
||
default:
|
||
cmd_message.data[0] = CMD_ERR;
|
||
cmd_message.data_length_code = 1;
|
||
ESP_LOGI(TAG, "cmd[0x%02d] none", (unsigned int)cmd);
|
||
break;
|
||
}
|
||
// 发送回应数据
|
||
cmd_message.identifier = (cmd_message.identifier & 0xffff0000) | ((uint16_t)cmd << 8) | sub_cmd;
|
||
twai_transmit(&cmd_message, pdMS_TO_TICKS(10));
|
||
}
|
||
else if (identifier == ESP_RESTART_ID)
|
||
{
|
||
// 重启命令
|
||
cmd_message.data[0] = CMD_OK;
|
||
cmd_message.data_length_code = 1;
|
||
cmd_message.identifier = (cmd_message.identifier & 0xffff0000) | ((uint16_t)cmd << 8) | sub_cmd;
|
||
twai_transmit(&cmd_message, pdMS_TO_TICKS(10));
|
||
ESP_LOGI(TAG, "cmd restart, after 3s...");
|
||
vTaskDelay(pdMS_TO_TICKS(1000));
|
||
vTaskDelay(pdMS_TO_TICKS(1000));
|
||
vTaskDelay(pdMS_TO_TICKS(1000));
|
||
esp_restart();
|
||
}
|
||
else if (identifier == OTA_ID)
|
||
{
|
||
// OTA命令
|
||
switch (cmd)
|
||
{
|
||
case OC_ENTER:
|
||
ota_resp_message.data[0] = ota_enter_command();
|
||
ota_resp_message.data_length_code = 1;
|
||
break;
|
||
case OC_DOWNLOAD_DATA_IN:
|
||
ota_resp_message.data[0] = ota_download_data_in(rx_msg.data, rx_msg.data_length_code);
|
||
ota_resp_message.data_length_code = 1;
|
||
break;
|
||
case OC_DOWNLOAD_FINISH:
|
||
ota_resp_message.data[0] = ota_download_finish_command();
|
||
ota_resp_message.data_length_code = 1;
|
||
break;
|
||
case READ_VERSIONS:
|
||
if (sub_cmd <= 0 || sub_cmd > 4)
|
||
{
|
||
ota_resp_message.data[0] = CMD_ERR;
|
||
ota_resp_message.data_length_code = 1;
|
||
ESP_LOGI(TAG, "sub_cmd[0x%02x] none", (unsigned int)sub_cmd);
|
||
break;
|
||
}
|
||
char *version = ota_read_versions();
|
||
memcpy(ota_resp_message.data, version + (sub_cmd - 1) * 8, 8);
|
||
ota_resp_message.data_length_code = 8;
|
||
break;
|
||
default:
|
||
ota_resp_message.data[0] = CMD_ERR;
|
||
ota_resp_message.data_length_code = 1;
|
||
ESP_LOGI(TAG, "cmd[0x%02d] none", (unsigned int)cmd);
|
||
break;
|
||
}
|
||
ota_resp_message.identifier = (ota_resp_message.identifier & 0xffff0000) | ((uint16_t)cmd << 8) | sub_cmd;
|
||
twai_transmit(&ota_resp_message, pdMS_TO_TICKS(10));
|
||
}
|
||
}
|
||
else
|
||
{
|
||
ESP_LOGI(TAG, "Failed to receive message");
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
int twai_send_data(uint32_t identifier, const uint8_t *data, int len, bool enable)
|
||
{
|
||
if (enable == false)
|
||
return 0;
|
||
if (len > TWAI_SEND_DATA_MAX)
|
||
{
|
||
ESP_LOGE(TAG, "data len[%d] too long", len);
|
||
return -1;
|
||
}
|
||
|
||
uint8_t *data_p = data;
|
||
twai_message_t message;
|
||
message.identifier = identifier | ((uint8_t)(len / 8) << 8);
|
||
message.extd = 1;
|
||
|
||
while (len > 8)
|
||
{
|
||
message.identifier += 1; // 包号，从1开始
|
||
message.data_length_code = 8;
|
||
memcpy(message.data, data_p, 8);
|
||
if (twai_transmit(&message, pdMS_TO_TICKS(10)) != ESP_OK) {
|
||
ESP_LOGE(TAG, "twai_transmit failed!");
|
||
return -1;
|
||
}
|
||
len -= 8;
|
||
data_p += 8;
|
||
}
|
||
|
||
if (len > 0)
|
||
{
|
||
message.identifier += 1;
|
||
message.data_length_code = len;
|
||
memcpy(message.data, data_p, len);
|
||
if (twai_transmit(&message, pdMS_TO_TICKS(10)) != ESP_OK) {
|
||
ESP_LOGE(TAG, "twai_transmit failed!");
|
||
return -1;
|
||
}
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
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, 10, NULL, tskNO_AFFINITY);
|
||
}
|