834 lines
20 KiB
C
834 lines
20 KiB
C
#include "string.h"
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <math.h>
|
|
#include "communication.h"
|
|
#include "ctr_logic.h"
|
|
#include "bldc.h"
|
|
#include "dac.h"
|
|
#include "ProximitySwitches.h"
|
|
#include "ds18b20.h"
|
|
#include "hc595display.h"
|
|
#include "FreeRTOS.h"
|
|
#include "task.h"
|
|
#include "flash.h"
|
|
|
|
modbus_rtu_t modbus_rtu_A;
|
|
modbus_rtu_t modbus_rtu_B;
|
|
modbus_rtu_t modbus_rtu_hmi;
|
|
|
|
uint8_t rtu_tx_buf_A[BUFFER_SIZE];
|
|
uint8_t rtu_rx_buf_A[BUFFER_SIZE];
|
|
uint8_t rtu_tx_buf_B[BUFFER_SIZE];
|
|
uint8_t rtu_rx_buf_B[BUFFER_SIZE];
|
|
uint8_t rtu_tx_buf_hmi[BUFFER_SIZE];
|
|
uint8_t rtu_rx_buf_hmi[BUFFER_SIZE];
|
|
|
|
static uint16_t CRC_Check(uint8_t *CRC_Ptr, uint8_t LEN)
|
|
{
|
|
if (!CRC_Ptr)
|
|
return 0;
|
|
|
|
uint16_t CRC_Value = 0;
|
|
uint8_t i = 0;
|
|
uint8_t j = 0;
|
|
|
|
CRC_Value = 0xffff;
|
|
for (i = 0; i < LEN; i++)
|
|
{
|
|
CRC_Value ^= *(CRC_Ptr + i);
|
|
for (j = 0; j < 8; j++)
|
|
{
|
|
if (CRC_Value & 0x00001)
|
|
CRC_Value = (CRC_Value >> 1) ^ 0xA001;
|
|
else
|
|
CRC_Value = (CRC_Value >> 1);
|
|
}
|
|
}
|
|
CRC_Value = ((CRC_Value >> 8) + (CRC_Value << 8));
|
|
|
|
return CRC_Value;
|
|
}
|
|
|
|
static void fun_set_range(float *input, float max, float min)
|
|
{
|
|
if (*input > max)
|
|
*input = max;
|
|
else if (*input < min)
|
|
*input = min;
|
|
}
|
|
|
|
void fun_ini_modbus(modbus_rtu_t *modbus_rtu)
|
|
{
|
|
if (!modbus_rtu)
|
|
return;
|
|
|
|
BOOL *tx_flag = &modbus_rtu->tx_flag;
|
|
uint16_t *delay_out = &modbus_rtu->delay_out;
|
|
uint8_t *addr = &modbus_rtu->addr;
|
|
uint8_t *cmd = &modbus_rtu->cmd;
|
|
uint16_t *reg_start = &modbus_rtu->reg_start;
|
|
uint16_t *reg_num = &modbus_rtu->reg_num;
|
|
|
|
memset(modbus_rtu, 0, sizeof(modbus_rtu_t));
|
|
*tx_flag = TRUE;
|
|
*delay_out = DELAY_OUT;
|
|
*addr = 0x02;
|
|
*cmd = 0x03;
|
|
*reg_start = 0x0000;
|
|
*reg_num = 0x0012;
|
|
|
|
if (modbus_rtu == &modbus_rtu_A)
|
|
{
|
|
modbus_rtu->huart = huart4;
|
|
modbus_rtu->tx_buf = rtu_tx_buf_A;
|
|
modbus_rtu->rx_buf = rtu_rx_buf_A;
|
|
}
|
|
else if (modbus_rtu == &modbus_rtu_B)
|
|
{
|
|
modbus_rtu->huart = huart2;
|
|
modbus_rtu->tx_buf = rtu_tx_buf_B;
|
|
modbus_rtu->rx_buf = rtu_rx_buf_B;
|
|
}
|
|
else if (modbus_rtu == &modbus_rtu_hmi)
|
|
{
|
|
modbus_rtu->huart = huart3;
|
|
modbus_rtu->tx_buf = rtu_tx_buf_hmi;
|
|
modbus_rtu->rx_buf = rtu_rx_buf_hmi;
|
|
}
|
|
__HAL_UART_ENABLE_IT(&modbus_rtu->huart, UART_IT_RXNE);
|
|
__HAL_UART_ENABLE_IT(&modbus_rtu->huart, UART_IT_IDLE);
|
|
}
|
|
|
|
static void fun_send_modbus(modbus_rtu_t *modbus_rtu)
|
|
{
|
|
if (!modbus_rtu)
|
|
return;
|
|
if (!modbus_rtu->tx_buf)
|
|
return;
|
|
|
|
if (modbus_rtu == &modbus_rtu_hmi)
|
|
return;
|
|
|
|
UART_HandleTypeDef *huart = &modbus_rtu->huart;
|
|
uint8_t *tx_buf = modbus_rtu->tx_buf;
|
|
uint8_t *addr = &modbus_rtu->addr;
|
|
uint8_t *cmd = &modbus_rtu->cmd;
|
|
uint16_t *reg_start = &modbus_rtu->reg_start;
|
|
uint16_t *reg_num = &modbus_rtu->reg_num;
|
|
uint16_t *crc = &modbus_rtu->crc;
|
|
|
|
*tx_buf++ = *addr;
|
|
*tx_buf++ = *cmd;
|
|
*tx_buf++ = *((uint8_t *)reg_start + 1);
|
|
*tx_buf++ = *((uint8_t *)reg_start);
|
|
*tx_buf++ = *((uint8_t *)reg_num + 1);
|
|
*tx_buf++ = *((uint8_t *)reg_num);
|
|
*crc = CRC_Check(modbus_rtu->tx_buf, 6);
|
|
//*crc = CRC16(modbus_rtu->tx_buf, 6);
|
|
*tx_buf++ = *((uint8_t *)crc + 1);
|
|
*tx_buf++ = *(uint8_t *)crc;
|
|
HAL_UART_Transmit(huart, modbus_rtu->tx_buf, 8, 0xFFFF);
|
|
}
|
|
|
|
uint16_t temp_arry[20];
|
|
uint16_t pre_temp[20];
|
|
static void fun_ack_current(cmd_e cmd_type, uint8_t *tx_buf)
|
|
{
|
|
if (!tx_buf)
|
|
return;
|
|
|
|
uint8_t i = 0, j = 0;
|
|
uint16_t data_len = 0X003D;
|
|
motor_t *motor = NULL;
|
|
float32 *prox_switch_X_vol;
|
|
float32 prox_switch_X_cur = 0.0f;
|
|
uint8_t minus = 0;
|
|
float32 temperature, distance;
|
|
|
|
if (motor_X == MOTOR_A)
|
|
{
|
|
motor = &motor_A;
|
|
prox_switch_X_vol = prox_switch_A_vol;
|
|
temperature = ds18b20_A.temperature.f;
|
|
distance = motor->position_pv;
|
|
}
|
|
else if (motor_X == MOTOR_B)
|
|
{
|
|
motor = &motor_B;
|
|
prox_switch_X_vol = prox_switch_B_vol;
|
|
temperature = ds18b20_B.temperature.f;
|
|
distance = motor->position_pv;
|
|
}
|
|
|
|
tx_buf[i++] = PARSE_HEAD;
|
|
tx_buf[i++] = data_len >> 8;
|
|
tx_buf[i++] = data_len;
|
|
tx_buf[i++] = ADDR_LOCAL >> 8;
|
|
tx_buf[i++] = ADDR_LOCAL;
|
|
tx_buf[i++] = ADDR_REMOTE >> 8;
|
|
tx_buf[i++] = ADDR_REMOTE;
|
|
tx_buf[i++] = motor_X;
|
|
tx_buf[i++] = (uint8_t)cmd_type;
|
|
|
|
for (j = 1; j < 21; j++)
|
|
{
|
|
// tx_buf[i++] = ((uint16_t)(prox_switch_X_vol[j] * 1000 * 1000 / 250)) >> 8;
|
|
// tx_buf[i++] = (uint16_t)(prox_switch_X_vol[j] * 1000 * 1000 / 250);
|
|
|
|
if (j < 11)
|
|
{
|
|
tx_buf[i++] = ((uint16_t)(prox_switch_A_vol[j] * 1000 * 1000 / 250)) >> 8;
|
|
tx_buf[i++] = (uint16_t)(prox_switch_A_vol[j] * 1000 * 1000 / 250);
|
|
}
|
|
else
|
|
{
|
|
tx_buf[i++] = ((uint16_t)(prox_switch_B_vol[j - 10] * 1000 * 1000 / 250)) >> 8;
|
|
tx_buf[i++] = (uint16_t)(prox_switch_B_vol[j - 10] * 1000 * 1000 / 250);
|
|
}
|
|
}
|
|
|
|
for (j = 1; j < 11; j++)
|
|
{
|
|
prox_switch_X_cur += prox_switch_X_vol[j];
|
|
}
|
|
prox_switch_X_cur /= 10;
|
|
prox_switch_X_cur /= 250.f;
|
|
tx_buf[i++] = ((uint16_t)prox_switch_X_cur * 1000) >> 8;
|
|
tx_buf[i++] = (uint16_t)prox_switch_X_cur * 1000;
|
|
|
|
float temp;
|
|
|
|
if (fabs(prox_switch_X_vol[0] - 6.92f) < 0.15f)
|
|
temp = 6.97f;
|
|
else if (fabs(prox_switch_X_vol[0] - 8.20f) < 0.10f)
|
|
temp = 8.20f;
|
|
else if (fabs(prox_switch_X_vol[0] - 9.02f) < 0.10f)
|
|
temp = 9.02f;
|
|
else if (prox_switch_X_vol[0] < 6.0f)
|
|
temp = 0.0f;
|
|
else
|
|
temp = prox_switch_X_vol[0];
|
|
|
|
tx_buf[i++] = ((uint16_t)(temp * 1000)) >> 8;
|
|
tx_buf[i++] = (uint16_t)(temp * 1000);
|
|
|
|
/*
|
|
tx_buf[i++] = ((uint16_t)(prox_switch_X_vol[0] * 1000)) >> 8;
|
|
tx_buf[i++] = (uint16_t)(prox_switch_X_vol[0] * 1000);
|
|
*/
|
|
if (temperature < 0)
|
|
{
|
|
minus = 1;
|
|
temperature = -temperature;
|
|
}
|
|
|
|
tx_buf[i++] = minus;
|
|
tx_buf[i++] = ((uint16_t)(temperature * 100) >> 8);
|
|
tx_buf[i++] = (uint16_t)(temperature * 100);
|
|
|
|
tx_buf[i++] = ((uint16_t)(distance * 1000)) >> 8;
|
|
tx_buf[i++] = (uint16_t)(distance * 1000);
|
|
tx_buf[i++] = 0X01;
|
|
|
|
tx_buf[i++] = CHECK_NUM >> 8;
|
|
tx_buf[i++] = (uint8_t)CHECK_NUM;
|
|
tx_buf[i++] = PARSE_TAIL;
|
|
|
|
HAL_UART_Transmit(&modbus_rtu_hmi.huart, tx_buf, i, 0xFFFF);
|
|
}
|
|
|
|
static void fun_ack_time(uint8_t *tx_buf)
|
|
{
|
|
if (!tx_buf)
|
|
return;
|
|
|
|
uint8_t i = 0;
|
|
uint16_t data_len = 0X0047;
|
|
|
|
tx_buf[i++] = PARSE_HEAD;
|
|
tx_buf[i++] = data_len >> 8;
|
|
tx_buf[i++] = data_len;
|
|
tx_buf[i++] = ADDR_LOCAL >> 8;
|
|
tx_buf[i++] = ADDR_LOCAL;
|
|
tx_buf[i++] = ADDR_REMOTE >> 8;
|
|
tx_buf[i++] = ADDR_REMOTE;
|
|
tx_buf[i++] = motor_X;
|
|
tx_buf[i++] = CMD_GET_TIME;
|
|
/*
|
|
uint32_t(*time)[10] = NULL;
|
|
if (motor_X == MOTOR_A)
|
|
{
|
|
time = prox_switch_A_t;
|
|
}
|
|
else if (motor_X == MOTOR_B)
|
|
{
|
|
time = prox_switch_B_t;
|
|
}
|
|
*/
|
|
for (uint8_t j = 0; j < 10; j++)
|
|
{
|
|
tx_buf[i++] = (uint16_t)(prox_switch_A_t[3][j] - prox_switch_A_t[0][j]) >> 8;
|
|
tx_buf[i++] = (uint16_t)(prox_switch_A_t[3][j] - prox_switch_A_t[0][j]);
|
|
}
|
|
|
|
for (uint8_t j = 0; j < 10; j++)
|
|
{
|
|
tx_buf[i++] = (uint16_t)(prox_switch_B_t[3][j] - prox_switch_B_t[0][j]) >> 8;
|
|
tx_buf[i++] = (uint16_t)(prox_switch_B_t[3][j] - prox_switch_B_t[0][j]);
|
|
}
|
|
|
|
for (uint8_t j = 0; j < 10; j++)
|
|
{
|
|
tx_buf[i++] = (uint8_t)(prox_switch_A_t[2][j] - prox_switch_A_t[1][j]);
|
|
}
|
|
|
|
for (uint8_t j = 0; j < 10; j++)
|
|
{
|
|
tx_buf[i++] = (uint8_t)(prox_switch_B_t[2][j] - prox_switch_B_t[1][j]);
|
|
}
|
|
|
|
tx_buf[i++] = CHECK_NUM >> 8;
|
|
tx_buf[i++] = (uint8_t)CHECK_NUM;
|
|
tx_buf[i++] = PARSE_TAIL;
|
|
|
|
HAL_UART_Transmit(&modbus_rtu_hmi.huart, tx_buf, i, 0xFFFF);
|
|
}
|
|
|
|
static void fun_ack_freq(uint8_t *tx_buf)
|
|
{
|
|
if (!tx_buf)
|
|
return;
|
|
|
|
uint8_t i = 0;
|
|
uint16_t data_len = 0X000f;
|
|
|
|
tx_buf[i++] = PARSE_HEAD;
|
|
tx_buf[i++] = data_len >> 8;
|
|
tx_buf[i++] = data_len;
|
|
tx_buf[i++] = ADDR_LOCAL >> 8;
|
|
tx_buf[i++] = ADDR_LOCAL;
|
|
tx_buf[i++] = ADDR_REMOTE >> 8;
|
|
tx_buf[i++] = ADDR_REMOTE;
|
|
tx_buf[i++] = motor_X;
|
|
tx_buf[i++] = CMD_SET_FREQ;
|
|
tx_buf[i++] = (uint16_t)freq_opto.freq >> 8;
|
|
tx_buf[i++] = (uint16_t)freq_opto.freq;
|
|
tx_buf[i++] = (uint16_t)freq_namur.freq >> 8;
|
|
tx_buf[i++] = (uint16_t)freq_namur.freq;
|
|
tx_buf[i++] = (uint8_t)CHECK_NUM >> 8;
|
|
tx_buf[i++] = (uint8_t)CHECK_NUM;
|
|
tx_buf[i++] = PARSE_TAIL;
|
|
|
|
HAL_UART_Transmit(&modbus_rtu_hmi.huart, tx_buf, i, 0xFFFF);
|
|
}
|
|
|
|
static void fun_ack_cmd(uint8_t *motor_X)
|
|
{
|
|
if (!motor_X)
|
|
return;
|
|
|
|
cmd_e cmd_type;
|
|
motor_t *motor = NULL;
|
|
// modbus_rtu_t *modbus_rtu = NULL;
|
|
|
|
if (*motor_X == MOTOR_A)
|
|
{
|
|
motor = &motor_A;
|
|
// modbus_rtu = &modbus_rtu_A;
|
|
}
|
|
else if (*motor_X == MOTOR_B)
|
|
{
|
|
motor = &motor_B;
|
|
// modbus_rtu = &modbus_rtu_B;
|
|
}
|
|
|
|
// if (modbus_rtu->upsend_flag == TRUE)
|
|
if (modbus_rtu_hmi.upsend_flag == TRUE)
|
|
{
|
|
if ((cmd_e)motor->ctr_cmd == CMD_GET_TIME)
|
|
{
|
|
fun_ack_time(modbus_rtu_hmi.tx_buf);
|
|
pars_cmd_time_A.busy_flag = FALSE;
|
|
pars_cmd_time_B.busy_flag = FALSE;
|
|
motor->ctr_cmd = CMD_ZERO_DIST;
|
|
}
|
|
|
|
else if ((cmd_e)motor->ctr_cmd == CMD_SET_FREQ)
|
|
fun_ack_freq(modbus_rtu_hmi.tx_buf);
|
|
else
|
|
{
|
|
if (motor->zero_ack == TRUE)
|
|
{
|
|
motor->zero_ack = FALSE;
|
|
cmd_type = CMD_ZERO_DIST;
|
|
}
|
|
else
|
|
cmd_type = (cmd_e)motor->ctr_cmd;
|
|
fun_ack_current(cmd_type, modbus_rtu_hmi.tx_buf);
|
|
}
|
|
|
|
// modbus_rtu->upsend_flag = FALSE;
|
|
modbus_rtu_hmi.upsend_flag = FALSE;
|
|
|
|
if ((motor->position_sv == motor->Sn * 2) || (cmd_type == CMD_ZERO_DIST))
|
|
{
|
|
beep_1.en = TRUE;
|
|
beep_1.time = xTaskGetTickCount();
|
|
}
|
|
}
|
|
}
|
|
|
|
static void fun_parse_cmd_dist(uint8_t *rx_buf)
|
|
{
|
|
if (!rx_buf)
|
|
return;
|
|
|
|
motor_t *motor = NULL;
|
|
modbus_rtu_t *modbus_rtu;
|
|
prox_swith_pwr_t *prox_swith_pwr_X;
|
|
|
|
if (motor_X == MOTOR_A)
|
|
{
|
|
prox_swith_pwr_X = &prox_swith_pwr_A;
|
|
motor = &motor_A;
|
|
modbus_rtu = &modbus_rtu_A;
|
|
}
|
|
else if (motor_X == MOTOR_B)
|
|
{
|
|
prox_swith_pwr_X = &prox_swith_pwr_B;
|
|
motor = &motor_B;
|
|
modbus_rtu = &modbus_rtu_B;
|
|
}
|
|
|
|
state_e state = (state_e)rx_buf[9];
|
|
switch (state)
|
|
{
|
|
case STATE_MOTOR_STOP:
|
|
motor->en = FALSE;
|
|
break;
|
|
case STATE_MOTOR_RUN:
|
|
motor->en = TRUE;
|
|
break;
|
|
case STATE_MOTOR_NEAR:
|
|
motor->force_near_flag = TRUE;
|
|
motor->force_far_flag = FALSE;
|
|
break;
|
|
case STATE_MOTOR_FAR:
|
|
motor->force_far_flag = TRUE;
|
|
motor->force_near_flag = FALSE;
|
|
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
mode_e mode = (mode_e)rx_buf[10];
|
|
if (mode == MODE_MANUL)
|
|
{
|
|
}
|
|
else if (mode == MODE_AUTO)
|
|
{
|
|
}
|
|
|
|
if (state < STATE_MOTOR_NEAR)
|
|
{
|
|
// motor->direction = rx_buf[11];
|
|
motor->position_sv = (rx_buf[12] << 8 | rx_buf[13]) / 1000.0f;
|
|
}
|
|
|
|
modbus_rtu->upsend_delay = (uint16_t)(rx_buf[14] << 8 | rx_buf[15]);
|
|
prox_swith_pwr_X->voltage_sv = (rx_buf[16] << 8 | rx_buf[17]) * 10;
|
|
|
|
if ((motor->position_sv >= motor->Sn * 2) && (fabs(motor->position_sv - motor->position_pv) > 0.5f))
|
|
{
|
|
motor->ctr_cmd = CMD_RATED_DIST;
|
|
return;
|
|
}
|
|
|
|
if (motor->position_pv <= 0.2f)
|
|
motor->end_flag = TRUE;
|
|
}
|
|
|
|
static void fun_parse_cmd_time(uint8_t *rx_buf)
|
|
{
|
|
if (!rx_buf)
|
|
return;
|
|
|
|
motor_t *motor = NULL;
|
|
|
|
if (motor_X == MOTOR_A)
|
|
motor = &motor_A;
|
|
else if (motor_X == MOTOR_B)
|
|
motor = &motor_B;
|
|
|
|
motor->Sn = (float32)rx_buf[9] / 100.0f;
|
|
}
|
|
|
|
static void fun_parse_cmd_freq(uint8_t *rx_buf)
|
|
{
|
|
if (!rx_buf)
|
|
return;
|
|
|
|
bldc_A.state = (state_e)rx_buf[9];
|
|
|
|
if (bldc_A.state == STATE_MOTOR_STOP)
|
|
bldc_A.velocity_sv = 0;
|
|
else if (bldc_A.state == STATE_MOTOR_RUN)
|
|
bldc_A.velocity_sv = BLDC_DEFAULT_SPEED;
|
|
}
|
|
|
|
static void fun_parse_cmd_zero(uint8_t *rx_buf)
|
|
{
|
|
if (!rx_buf)
|
|
return;
|
|
|
|
hc595display_t *hc595display = NULL;
|
|
motor_t *motor = NULL;
|
|
|
|
if (motor_X == MOTOR_A)
|
|
{
|
|
motor = &motor_A;
|
|
hc595display = &hc595display_A;
|
|
}
|
|
|
|
else if (motor_X == MOTOR_B)
|
|
{
|
|
motor = &motor_B;
|
|
hc595display = &hc595display_B;
|
|
}
|
|
|
|
motor->Sn = rx_buf[9];
|
|
hc595display->err_code = 0;
|
|
}
|
|
|
|
static void fun_parse_err_code(uint8_t *rx_buf)
|
|
{
|
|
if (!rx_buf)
|
|
return;
|
|
hc595display_t *hc595display = NULL;
|
|
motor_t *motor = NULL;
|
|
|
|
if (motor_X == MOTOR_A)
|
|
{
|
|
hc595display = &hc595display_A;
|
|
motor = &motor_A;
|
|
}
|
|
|
|
else if (motor_X == MOTOR_B)
|
|
{
|
|
hc595display = &hc595display_B;
|
|
motor = &motor_B;
|
|
}
|
|
|
|
hc595display->err_code = rx_buf[9];
|
|
hc595display->err_code <<= 8;
|
|
hc595display->err_code |= rx_buf[10];
|
|
|
|
hc595display->err_code = ~hc595display->err_code;
|
|
|
|
motor->ctr_cmd = CMD_RATED_DIST;
|
|
}
|
|
|
|
static void fun_get_cmd_type(uint8_t motor_X, cmd_e cmd_type)
|
|
{
|
|
if (motor_X == MOTOR_A)
|
|
{
|
|
motor_A.ctr_cmd = cmd_type;
|
|
// hc595display_A.step = (uint8_t)cmd_type;
|
|
}
|
|
else if (motor_X == MOTOR_B)
|
|
{
|
|
motor_B.ctr_cmd = cmd_type;
|
|
// hc595display_B.step = (uint8_t)cmd_type;
|
|
}
|
|
}
|
|
|
|
static BOOL fun_parse_hmi(modbus_rtu_t *modbus_rtu)
|
|
{
|
|
if (!modbus_rtu)
|
|
return FALSE;
|
|
|
|
uint8_t *rx_buf = modbus_rtu->rx_buf;
|
|
uint8_t *tx_buf = modbus_rtu->tx_buf;
|
|
uint8_t rx_index = modbus_rtu->rx_index;
|
|
|
|
uint8_t lenth;
|
|
uint8_t local_addr;
|
|
uint8_t remote_addr;
|
|
uint16_t check;
|
|
cmd_e command;
|
|
|
|
if (rx_buf[0] != PARSE_HEAD)
|
|
return FALSE;
|
|
|
|
lenth = rx_buf[1] << 8 | rx_buf[2];
|
|
|
|
if (rx_index < lenth + 1)
|
|
return FALSE;
|
|
|
|
remote_addr = rx_buf[3] << 8 | rx_buf[4];
|
|
if (remote_addr != ADDR_REMOTE)
|
|
return FALSE;
|
|
|
|
local_addr = rx_buf[5] << 8 | rx_buf[6];
|
|
if (local_addr != ADDR_LOCAL)
|
|
return FALSE;
|
|
|
|
check = rx_buf[rx_index - 3] << 8 | rx_buf[rx_index - 2];
|
|
if (check != CHECK_NUM)
|
|
; // return FALSE;
|
|
|
|
if (rx_buf[rx_index - 1] != PARSE_TAIL)
|
|
return FALSE;
|
|
|
|
if ((rx_buf[7] != MOTOR_A) && (rx_buf[7] != MOTOR_B))
|
|
return FALSE;
|
|
|
|
motor_X = rx_buf[7];
|
|
// FlashErase(0X08040000);
|
|
// FlashWrite(0X0804000,motor_X);
|
|
|
|
command = (cmd_e)rx_buf[8];
|
|
|
|
fun_get_cmd_type(motor_X, command);
|
|
|
|
switch (command)
|
|
{
|
|
case CMD_SET_DIST:
|
|
fun_parse_cmd_dist(rx_buf);
|
|
// fun_ack_current(CMD_SET_DIST,tx_buf);
|
|
break;
|
|
case CMD_GET_TIME:
|
|
fun_parse_cmd_time(rx_buf);
|
|
// fun_ack_time(tx_buf);
|
|
break;
|
|
case CMD_SET_FREQ:
|
|
fun_parse_cmd_freq(rx_buf);
|
|
fun_ack_freq(tx_buf);
|
|
break;
|
|
case CMD_ZERO_DIST:
|
|
fun_parse_cmd_zero(rx_buf);
|
|
// fun_ack_current(CMD_ZERO_DIST,tx_buf);
|
|
break;
|
|
case CMD_ERR_COMDE:
|
|
fun_parse_err_code(rx_buf);
|
|
// fun_ack_err_code(CMD_ZERO_DIST,tx_buf);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return TRUE;
|
|
}
|
|
|
|
static BOOL fun_get_lvdt_flag(float32 *distance, const float dis_zero, const float32 dis_rated, BOOL *lvdt_flag)
|
|
{
|
|
if ((*distance <= dis_zero) || (*distance >= dis_rated))
|
|
*lvdt_flag = TRUE;
|
|
else
|
|
*lvdt_flag = FALSE;
|
|
|
|
return *lvdt_flag;
|
|
}
|
|
/*
|
|
02 03 24
|
|
00 00 00 00 距离 v
|
|
FF FF FF C1 温度 x
|
|
00 00 00 00 距离 v
|
|
FF C1 FF FF 温度 x
|
|
B9 0A BF DC 距离 v
|
|
BD 80 00 00 温度 x
|
|
BF DC B9 0A 距离 c
|
|
00 00 BD 80 温度 x
|
|
00 02 地址 x
|
|
25 80 波特率 x
|
|
CC 27 CRC
|
|
*/
|
|
static BOOL fun_parse_lvdt(modbus_rtu_t *modbus_rtu)
|
|
{
|
|
if (!modbus_rtu)
|
|
return 0;
|
|
|
|
BOOL *lvdt_flag = &modbus_rtu->lvdt_flag;
|
|
uint8_t *rx_buf = modbus_rtu->rx_buf;
|
|
uint8_t *addr = &modbus_rtu->addr;
|
|
uint8_t *cmd = &modbus_rtu->cmd;
|
|
uint16_t *reg_num = &modbus_rtu->reg_num;
|
|
uint16_t *crc = &modbus_rtu->crc;
|
|
float32 *distance = &modbus_rtu->distance;
|
|
float32 *temperature = &modbus_rtu->temperature;
|
|
uint16_t data_length;
|
|
|
|
if (rx_buf[0] != *addr)
|
|
return FALSE;
|
|
|
|
if (rx_buf[1] != *cmd)
|
|
return FALSE;
|
|
|
|
data_length = rx_buf[2];
|
|
if (data_length != *reg_num * 2)
|
|
return FALSE;
|
|
|
|
*crc = CRC_Check(rx_buf, data_length + 3);
|
|
if (rx_buf[39] != *((uint8_t *)crc + 1))
|
|
return FALSE;
|
|
if (rx_buf[40] != *((uint8_t *)crc))
|
|
return FALSE;
|
|
|
|
//*distance = (rx_buf[3] << 24 | rx_buf[4] << 16 | rx_buf[5] << 8 | rx_buf[6]) / 1000.0f;
|
|
//*temperature = (rx_buf[7] << 24 | rx_buf[8] << 16 | rx_buf[9] << 8 | rx_buf[10]) / 1000.0f;
|
|
int32_t temp;
|
|
temp = rx_buf[19] << 24 | rx_buf[20] << 16 | rx_buf[21] << 8 | rx_buf[22];
|
|
*distance = *((float *)&temp);
|
|
temp = rx_buf[23] << 24 | rx_buf[24] << 16 | rx_buf[25] << 8 | rx_buf[26];
|
|
*temperature = *((float *)&temp);
|
|
|
|
fun_set_range(distance, DISTANCE_MAX, DISTANCE_MIN);
|
|
fun_set_range(temperature, TEMPERATURE_MAX, TEMPERATURE_MIN);
|
|
|
|
*distance = DISTANCE_RATED - *distance;
|
|
/*
|
|
temp = rx_buf[19] << 27 | rx_buf[20] << 28 | rx_buf[29] << 8 | rx_buf[30];
|
|
*distance = *((float *)&temp);
|
|
temp = rx_buf[31] << 24 | rx_buf[32] << 16 | rx_buf[33] << 8 | rx_buf[34];
|
|
*temperature = *((float *)&temp);
|
|
*/
|
|
fun_get_lvdt_flag(distance, DISTANCE_ZERO + 0.20f, DISTANCE_RATED - 1.00f, lvdt_flag);
|
|
return TRUE;
|
|
}
|
|
|
|
static BOOL fun_parse_uart(modbus_rtu_t *modbus_rtu)
|
|
{
|
|
if (!modbus_rtu)
|
|
return FALSE;
|
|
|
|
if (modbus_rtu == &modbus_rtu_hmi)
|
|
{
|
|
return fun_parse_hmi(modbus_rtu);
|
|
}
|
|
else if ((modbus_rtu == &modbus_rtu_A) || (modbus_rtu == &modbus_rtu_B))
|
|
{
|
|
return fun_parse_lvdt(modbus_rtu);
|
|
}
|
|
else
|
|
return FALSE;
|
|
}
|
|
|
|
static void fun_connect_delayout(modbus_rtu_t *modbus_rtu)
|
|
{
|
|
if (!modbus_rtu)
|
|
return;
|
|
|
|
BOOL *rx_flag = &modbus_rtu->rx_flag;
|
|
BOOL *tx_flag = &modbus_rtu->tx_flag;
|
|
BOOL *con_flag = &modbus_rtu->con_flag;
|
|
uint16_t *delay_out = &modbus_rtu->delay_out;
|
|
// float32 *distance = &modbus_rtu->distance;
|
|
/*
|
|
motor_t *motor = NULL;
|
|
|
|
if (modbus_rtu == &modbus_rtu_A)
|
|
{
|
|
motor = &motor_A;
|
|
}
|
|
else if (modbus_rtu == &modbus_rtu_B)
|
|
{
|
|
motor = &motor_B;
|
|
}
|
|
|
|
if (motor->en != TRUE)
|
|
{
|
|
*delay_out = DELAY_OUT;
|
|
*tx_flag = TRUE;
|
|
return;
|
|
}
|
|
*/
|
|
if (*delay_out)
|
|
{
|
|
(*delay_out)--;
|
|
if ((*delay_out) % 100 == 0)
|
|
*tx_flag = TRUE;
|
|
}
|
|
else
|
|
{
|
|
*delay_out = DELAY_OUT;
|
|
*tx_flag = TRUE;
|
|
//*con_flag = FALSE;
|
|
//*distance = 0.0f;
|
|
}
|
|
|
|
if (*rx_flag == TRUE)
|
|
{
|
|
*delay_out = DELAY_OUT;
|
|
*con_flag = TRUE;
|
|
}
|
|
}
|
|
|
|
static void fun_get_upsend_flag(modbus_rtu_t *modbus_rtu)
|
|
{
|
|
if (!modbus_rtu)
|
|
return;
|
|
|
|
// BOOL *upsend_flag = &modbus_rtu->upsend_flag;
|
|
uint16_t *upsend_delay = &modbus_rtu->upsend_delay;
|
|
uint16_t *upsend_count = &modbus_rtu->upsend_count;
|
|
motor_t *motor = NULL;
|
|
|
|
if (modbus_rtu == &modbus_rtu_A)
|
|
motor = &motor_A;
|
|
if (modbus_rtu == &modbus_rtu_B)
|
|
motor = &motor_B;
|
|
|
|
if (motor->end_flag == TRUE)
|
|
{
|
|
motor->end_flag = FALSE;
|
|
if (*upsend_delay < 150)
|
|
*upsend_delay = 150;
|
|
else if (*upsend_delay > 1000)
|
|
*upsend_delay = 1000;
|
|
|
|
*upsend_count = *upsend_delay;
|
|
}
|
|
|
|
if (*upsend_count)
|
|
(*upsend_count)--;
|
|
else
|
|
*upsend_count = 0;
|
|
|
|
if (*upsend_count == 1)
|
|
//*upsend_flag = TRUE;
|
|
modbus_rtu_hmi.upsend_flag = TRUE;
|
|
}
|
|
|
|
BOOL fun_proc_modbus(modbus_rtu_t *modbus_rtu)
|
|
{
|
|
if (!modbus_rtu)
|
|
return FALSE;
|
|
|
|
BOOL ret;
|
|
BOOL *tx_flag = &modbus_rtu->tx_flag;
|
|
BOOL *rx_flag = &modbus_rtu->rx_flag;
|
|
uint8_t *rx_index = &modbus_rtu->rx_index;
|
|
|
|
fun_connect_delayout(modbus_rtu);
|
|
fun_get_upsend_flag(modbus_rtu);
|
|
fun_ack_cmd(&motor_X);
|
|
|
|
if (*tx_flag == TRUE)
|
|
{
|
|
*tx_flag = FALSE;
|
|
fun_send_modbus(modbus_rtu);
|
|
}
|
|
|
|
if (*rx_flag == TRUE)
|
|
{
|
|
ret = fun_parse_uart(modbus_rtu);
|
|
*rx_flag = FALSE;
|
|
*tx_flag = TRUE;
|
|
*rx_index = 0;
|
|
return ret;
|
|
}
|
|
return TRUE;
|
|
}
|