#include #include #include #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); }