/********************************** 函数实现区 *********************************/
void pid_update(PID_T *pid, f32_T measured)
{
static bool_T s_first_run = TRUE;
f32_T error = 0.0f;
f32_T acc = 0.0f;
f32_T det = 0.0f;
if(NULL == pid)
{
ERR_STR("参数错误.");
}
if(s_first_run)
{
pid->acc = 0;
pid->out = 0;
pid->last = measured;
s_first_run = FALSE;
}
else
{
/* step1: 计算偏差值 */
error = pid->expect - measured;
/* step2: 计算积分值 */
pid->acc += measured; /* 有记忆性 */
acc = pid->acc;
/* step3: 计算微分 */
det += measured - pid->last;
pid->last = measured; /* 有记忆性 */
/* pid公式: */
pid->out = (pid->kp * error) + (pid->ki * acc) + (pid->kd * det);
}
}
static int _parse (SCAN *scan, int dim, double **buf)
{ /* --- parse normalization statistics */
int k, n = 0; /* loop variable, counter */
double *p; /* to access the statistics elements */
assert(scan); /* check the function arguments */
if ((sc_token(scan) != T_ID) /* check whether 'scales' follows */
|| (strcmp(sc_value(scan), "scales") != 0))
ERR_STR("scales"); /* if not, abort the function */
GET_TOK(); /* consume 'scales' */
GET_CHR('='); /* consume '=' */
for (k = 0; (dim <= 0) || (k < dim); k++) {
if (k > 0) { GET_CHR(',');} /* if not first, consume ',' */
if (k >= n) { /* if the statistics vector is full */
if (dim > 0) n = dim; /* compute the new vector size */
else n += (n > BLKSIZE) ? n >> 1 : BLKSIZE;
p = (double*)realloc(*buf, (n+n) *sizeof(double));
if (!p) ERROR(E_NOMEM); /* enlarge the buffer vector */
*buf = p; /* and set the new vector, */
} /* then note factor and offset */
p = *buf +k +k; /* get the element to set */
GET_CHR('['); /* consume '[' */
if (sc_token(scan) != T_NUM) ERROR(E_NUMEXP);
p[0] = strtod(sc_value(scan), NULL);
GET_TOK(); /* consume the offset */
GET_CHR(','); /* consume '[' */
if (sc_token(scan) != T_NUM) ERROR(E_NUMEXP);
p[1] = strtod(sc_value(scan), NULL);
GET_TOK(); /* consume the factor */
GET_CHR(']'); /* consume '[' */
if ((dim <= 0) && (sc_token(scan) != ',')) {
k++; break; } /* check for more scaling params. */
}
GET_CHR(';'); /* consume ';' */
return k; /* return 'ok' */
} /* _parse() */
/********************************** 函数实现区 *********************************/
void pwm_init(void)
{
uint32_T pre_scale_val = 0;
/*
*
* 将计数器时钟设置为:15MHz, 计算预分频值
* PWM_TIMCLK使用 APB1 的时钟 即 HCLK/2
*
* 预分频值:
* pre_scale_val = (APB1_CLK / PWM_TIM_CLK ) - 1
* => pre_scale_val = ((SystemCoreClock / 2 ) /15 MHz) - 1
*
*/
pre_scale_val = (uint32_t)((SystemCoreClock/2) / 15000000) - 1;
/*
* 设置周期为:1000 便于编程
* ARR = (PWM_TIM源时钟/PWM_TIM输出频率) - 1
* => PWM_TIM输出频率 = PWM_TIM源时钟 / (ARR + 1)
* ARR = 1000
* PWM_TIM源时钟:15MHz
* => PWM_TIM输出频率 = 15000000 / (1000 + 1)
* = 14.985 kHz
*
* 设置PWM_TIM输出频率为20 KHz, 周期(ARR)为:
* ARR = (PWM_TIM源时钟/PWM_TIM输出频率) - 1
* = 749
* */
s_period = 1000;
/* 配置PWM_TIM计数器:
* Initialize PWM_TIM peripheral as follows:
* Prescaler = (SystemCoreClock / 2 / 15000000) - 1
* Period = (750 - 1)
* ClockDivision = 0
* Counter direction = Up
* */
s_tim_handle.Instance = PWM_TIM;
s_tim_handle.Init.Prescaler = pre_scale_val;
s_tim_handle.Init.Period = s_period;
s_tim_handle.Init.ClockDivision = 0;
s_tim_handle.Init.CounterMode = TIM_COUNTERMODE_UP;
s_tim_handle.Init.RepetitionCounter = 0;
if (HAL_TIM_PWM_Init(&s_tim_handle) != HAL_OK)
{
ERR_STR("执行失败.");
}
/* 各通道的占空比计算如下:
* PWM_TIM_Channel1 占空比 = (PWM_TIM_CCR1/ PWM_TIM_ARR + 1)* 100
* PWM_TIM Channel2 占空比 = (PWM_TIM_CCR2/ PWM_TIM_ARR + 1)* 100
* PWM_TIM Channel3 占空比 = (PWM_TIM_CCR3/ PWM_TIM_ARR + 1)* 100
* PWM_TIM Channel4 占空比 = (PWM_TIM_CCR4/ PWM_TIM_ARR + 1)* 100
*
* 初始化的时候占空比全0
*/
/* 所有通道都需要 */
s_sConfig.OCMode = TIM_OCMODE_PWM1;
s_sConfig.OCPolarity = TIM_OCPOLARITY_HIGH;
s_sConfig.OCFastMode = TIM_OCFAST_DISABLE;
s_sConfig.OCNPolarity = TIM_OCNPOLARITY_HIGH;
s_sConfig.OCNIdleState = TIM_OCNIDLESTATE_RESET;
s_sConfig.OCIdleState = TIM_OCIDLESTATE_RESET;
/* 测试电机 */
pwm_test();
/* 关闭电机 */
pwm_off();
return;
}
/*ARGSUSED*/
cfga_err_t
cfga_change_state(
cfga_cmd_t state_change_cmd,
const char *ap_id,
const char *options,
struct cfga_confirm *confp,
struct cfga_msg *msgp,
char **errstring,
cfga_flags_t flags)
{
int ret;
int len;
char *msg;
char *devpath;
nvlist_t *nvl = NULL;
ap_rstate_t rstate;
ap_ostate_t ostate;
devctl_hdl_t hdl = NULL;
cfga_sata_ret_t rv = CFGA_SATA_OK;
char *pdyn;
/*
* All sub-commands which can change state of device require
* root privileges.
*/
if (geteuid() != 0) {
rv = CFGA_SATA_PRIV;
goto bailout;
}
if ((rv = verify_params(ap_id, options, errstring)) != CFGA_SATA_OK) {
(void) cfga_help(msgp, options, flags);
goto bailout;
}
if ((rv = setup_for_devctl_cmd(ap_id, &hdl, &nvl,
DC_RDONLY)) != CFGA_SATA_OK) {
goto bailout;
}
switch (state_change_cmd) {
case CFGA_CMD_CONFIGURE:
if ((rv = port_state(hdl, nvl, &rstate, &ostate)) !=
CFGA_SATA_OK)
goto bailout;
if (ostate == AP_OSTATE_CONFIGURED) {
rv = CFGA_SATA_ALREADY_CONFIGURED;
goto bailout;
}
/* Disallow dynamic AP name component */
if (GET_DYN(ap_id) != NULL) {
rv = CFGA_SATA_INVALID_DEVNAME;
goto bailout;
}
if (rstate == AP_RSTATE_EMPTY) {
rv = CFGA_SATA_NOT_CONNECTED;
goto bailout;
}
rv = CFGA_SATA_OK;
if (devctl_ap_configure(hdl, nvl) != 0) {
rv = CFGA_SATA_DEV_CONFIGURE;
goto bailout;
}
devpath = sata_get_devicepath(ap_id);
if (devpath == NULL) {
int i;
/*
* Try for some time as SATA hotplug thread
* takes a while to create the path then
* eventually give up.
*/
for (i = 0; i < 12 && (devpath == NULL); i++) {
(void) sleep(6);
devpath = sata_get_devicepath(ap_id);
}
if (devpath == NULL) {
rv = CFGA_SATA_DEV_CONFIGURE;
break;
}
}
S_FREE(devpath);
break;
case CFGA_CMD_UNCONFIGURE:
if ((rv = port_state(hdl, nvl, &rstate, &ostate)) !=
CFGA_SATA_OK)
goto bailout;
if (rstate != AP_RSTATE_CONNECTED) {
rv = CFGA_SATA_NOT_CONNECTED;
goto bailout;
}
if (ostate != AP_OSTATE_CONFIGURED) {
rv = CFGA_SATA_NOT_CONFIGURED;
goto bailout;
}
/* Strip off AP name dynamic component, if present */
if ((pdyn = GET_DYN(ap_id)) != NULL) {
*pdyn = '\0';
}
rv = CFGA_SATA_OK;
len = strlen(SATA_CONFIRM_DEVICE) +
strlen(SATA_CONFIRM_DEVICE_SUSPEND) +
strlen("Unconfigure") + strlen(ap_id);
if ((msg = (char *)calloc(len +3, 1)) != NULL) {
(void) snprintf(msg, len + 3, "Unconfigure"
" %s%s\n%s",
SATA_CONFIRM_DEVICE, ap_id,
SATA_CONFIRM_DEVICE_SUSPEND);
}
if (!sata_confirm(confp, msg)) {
free(msg);
rv = CFGA_SATA_NACK;
break;
}
free(msg);
devpath = sata_get_devicepath(ap_id);
if (devpath == NULL) {
(void) printf(
"cfga_change_state: get device path failed\n");
rv = CFGA_SATA_DEV_UNCONFIGURE;
break;
}
if ((rv = sata_rcm_offline(ap_id, errstring, devpath, flags))
!= CFGA_SATA_OK) {
break;
}
ret = devctl_ap_unconfigure(hdl, nvl);
if (ret != 0) {
rv = CFGA_SATA_DEV_UNCONFIGURE;
if (errno == EBUSY) {
rv = CFGA_SATA_BUSY;
}
(void) sata_rcm_online(ap_id, errstring, devpath,
flags);
} else {
(void) sata_rcm_remove(ap_id, errstring, devpath,
flags);
}
S_FREE(devpath);
break;
case CFGA_CMD_DISCONNECT:
if ((rv = port_state(hdl, nvl, &rstate, &ostate)) !=
CFGA_SATA_OK)
goto bailout;
if (rstate == AP_RSTATE_DISCONNECTED) {
rv = CFGA_SATA_DISCONNECTED;
goto bailout;
}
/* Strip off AP name dynamic component, if present */
if ((pdyn = GET_DYN(ap_id)) != NULL) {
*pdyn = '\0';
}
rv = CFGA_SATA_OK; /* other statuses don't matter */
/*
* If the port originally with device attached and was
* unconfigured already, the devicepath for the sd will be
* removed. sata_get_devicepath in this case is not necessary.
*/
/* only call rcm_offline if the state was CONFIGURED */
if (ostate == AP_OSTATE_CONFIGURED) {
devpath = sata_get_devicepath(ap_id);
if (devpath == NULL) {
(void) printf(
"cfga_change_state: get path failed\n");
rv = CFGA_SATA_DEV_UNCONFIGURE;
break;
}
len = strlen(SATA_CONFIRM_DEVICE) +
strlen(SATA_CONFIRM_DEVICE_SUSPEND) +
strlen("Disconnect") + strlen(ap_id);
if ((msg = (char *)calloc(len +3, 1)) != NULL) {
(void) snprintf(msg, len + 3,
"Disconnect"
" %s%s\n%s",
SATA_CONFIRM_DEVICE, ap_id,
SATA_CONFIRM_DEVICE_SUSPEND);
}
if (!sata_confirm(confp, msg)) {
free(msg);
rv = CFGA_SATA_NACK;
break;
}
free(msg);
if ((rv = sata_rcm_offline(ap_id, errstring,
devpath, flags)) != CFGA_SATA_OK) {
break;
}
ret = devctl_ap_unconfigure(hdl, nvl);
if (ret != 0) {
(void) printf(
"devctl_ap_unconfigure failed\n");
rv = CFGA_SATA_DEV_UNCONFIGURE;
if (errno == EBUSY)
rv = CFGA_SATA_BUSY;
(void) sata_rcm_online(ap_id, errstring,
devpath, flags);
S_FREE(devpath);
/*
* The current policy is that if unconfigure
* failed, do not continue with disconnect.
* If the port needs to be forced into the
* disconnect (shutdown) state,
* the -x sata_port_poweroff command should be
* used instead of -c disconnect
*/
break;
} else {
(void) printf("%s\n",
ERR_STR(CFGA_SATA_DEVICE_UNCONFIGURED));
(void) sata_rcm_remove(ap_id, errstring,
devpath, flags);
}
S_FREE(devpath);
} else if (rstate == AP_RSTATE_CONNECTED ||
rstate == AP_RSTATE_EMPTY) {
len = strlen(SATA_CONFIRM_PORT) +
strlen(SATA_CONFIRM_PORT_DISABLE) +
strlen("Deactivate Port") + strlen(ap_id);
if ((msg = (char *)calloc(len +3, 1)) != NULL) {
(void) snprintf(msg, len +3,
"Disconnect"
" %s%s\n%s",
SATA_CONFIRM_PORT, ap_id,
SATA_CONFIRM_PORT_DISABLE);
}
if (!sata_confirm(confp, msg)) {
free(msg);
rv = CFGA_SATA_NACK;
break;
}
}
ret = devctl_ap_disconnect(hdl, nvl);
if (ret != 0) {
rv = CFGA_SATA_IOCTL;
if (errno == EBUSY) {
rv = CFGA_SATA_BUSY;
}
}
break;
case CFGA_CMD_CONNECT:
if ((rv = port_state(hdl, nvl, &rstate, &ostate)) !=
CFGA_SATA_OK)
goto bailout;
if (rstate == AP_RSTATE_CONNECTED) {
rv = CFGA_SATA_ALREADY_CONNECTED;
goto bailout;
}
len = strlen(SATA_CONFIRM_PORT) +
strlen(SATA_CONFIRM_PORT_ENABLE) +
strlen("Activate Port") + strlen(ap_id);
if ((msg = (char *)calloc(len +3, 1)) != NULL) {
(void) snprintf(msg, len +3, "Activate"
" %s%s\n%s",
SATA_CONFIRM_PORT, ap_id,
SATA_CONFIRM_PORT_ENABLE);
}
if (!sata_confirm(confp, msg)) {
rv = CFGA_SATA_NACK;
break;
}
/* Disallow dynamic AP name component */
if (GET_DYN(ap_id) != NULL) {
rv = CFGA_SATA_INVALID_DEVNAME;
goto bailout;
}
ret = devctl_ap_connect(hdl, nvl);
if (ret != 0) {
rv = CFGA_SATA_IOCTL;
} else {
rv = CFGA_SATA_OK;
}
break;
case CFGA_CMD_LOAD:
case CFGA_CMD_UNLOAD:
(void) cfga_help(msgp, options, flags);
rv = CFGA_SATA_OPNOTSUPP;
break;
case CFGA_CMD_NONE:
default:
(void) cfga_help(msgp, options, flags);
rv = CFGA_SATA_INTERNAL_ERROR;
}
bailout:
cleanup_after_devctl_cmd(hdl, nvl);
return (sata_err_msg(errstring, rv, ap_id, errno));
}
cfga_err_t
sata_err_msg(
char **errstring,
cfga_sata_ret_t rv,
const char *ap_id,
int l_errno)
{
if (errstring == NULL) {
return (sata_msgs[rv].cfga_err);
}
/*
* Generate the appropriate SATA-specific error message(s) (if any).
*/
switch (rv) {
case CFGA_SATA_OK:
case CFGA_NACK:
/* Special case - do nothing. */
break;
case CFGA_SATA_UNKNOWN:
case CFGA_SATA_DYNAMIC_AP:
case CFGA_SATA_INTERNAL_ERROR:
case CFGA_SATA_OPTIONS:
case CFGA_SATA_ALLOC_FAIL:
case CFGA_SATA_STATE:
case CFGA_SATA_PRIV:
case CFGA_SATA_OPNOTSUPP:
case CFGA_SATA_DATA_ERROR:
/* These messages require no additional strings passed. */
set_msg(errstring, ERR_STR(rv), NULL);
break;
case CFGA_SATA_HWOPNOTSUPP:
/* hardware-specific help needed */
set_msg(errstring, ERR_STR(rv), NULL);
set_msg(errstring, "\n",
dgettext(TEXT_DOMAIN, sata_help[HELP_HEADER]), NULL);
set_msg(errstring, sata_help[HELP_RESET_PORT], NULL);
set_msg(errstring, sata_help[HELP_RESET_DEVICE], NULL);
set_msg(errstring, sata_help[HELP_RESET_ALL], NULL);
set_msg(errstring, sata_help[HELP_PORT_ACTIVATE], NULL);
set_msg(errstring, sata_help[HELP_PORT_DEACTIVATE], NULL);
set_msg(errstring, sata_help[HELP_PORT_SELF_TEST], NULL);
set_msg(errstring, sata_help[HELP_CNTRL_SELF_TEST], NULL);
break;
case CFGA_SATA_AP:
case CFGA_SATA_PORT:
case CFGA_SATA_NOT_CONNECTED:
case CFGA_SATA_NOT_CONFIGURED:
case CFGA_SATA_ALREADY_CONNECTED:
case CFGA_SATA_ALREADY_CONFIGURED:
case CFGA_SATA_BUSY:
case CFGA_SATA_DEVLINK:
case CFGA_SATA_RCM_HANDLE:
case CFGA_SATA_RCM_ONLINE:
case CFGA_SATA_RCM_OFFLINE:
case CFGA_SATA_RCM_INFO:
case CFGA_SATA_DEV_CONFIGURE:
case CFGA_SATA_DEV_UNCONFIGURE:
case CFGA_SATA_DISCONNECTED:
/* These messages also print ap_id. */
set_msg(errstring, ERR_STR(rv), "ap_id: ", ap_id, "", NULL);
break;
case CFGA_SATA_IOCTL:
case CFGA_SATA_NVLIST:
/* These messages also print errno. */
{
char *errno_str = l_errno ? strerror(l_errno) : "";
set_msg(errstring, ERR_STR(rv), errno_str,
l_errno ? "\n" : "", NULL);
break;
}
case CFGA_SATA_OPEN:
/* These messages also apid and errno. */
{
char *errno_str = l_errno ? strerror(l_errno) : "";
set_msg(errstring, ERR_STR(rv), "ap_id: ", ap_id, "\n",
errno_str, l_errno ? "\n" : "", NULL);
break;
}
default:
set_msg(errstring, ERR_STR(CFGA_SATA_INTERNAL_ERROR), NULL);
} /* end switch */
/*
* Determine the proper error code to send back to the cfgadm library.
*/
return (sata_msgs[rv].cfga_err);
}
static int _domains (ATTSET *set, SCAN *scan, int tflags)
{ /* --- parse attribute domains */
ATT *att; /* attribute read */
int type; /* attribute type */
int t; /* temporary buffer */
double wgt; /* buffer for attribute weight */
const char *v; /* token value */
while ((sc_token(scan) == T_ID) /* parse domain definitions */
&& ((strcmp(sc_value(scan), "dom") == 0)
|| (strcmp(sc_value(scan), "domain") == 0))) {
GET_TOK(); /* consume 'dom' */
GET_CHR('('); /* consume '(' */
t = sc_token(scan); /* check next token for a valid name */
if ((t != T_ID) && (t != T_NUM)) ERROR(E_ATTEXP);
att = att_create(sc_value(scan), AT_NOM);
if (!att) ERROR(E_NOMEM); /* create an attribute and */
t = as_attadd(set, att); /* add it to the attribute set */
if (t) { att_delete(att); ERROR((t > 0) ? E_DUPATT : E_NOMEM); }
GET_TOK(); /* consume attribute name */
GET_CHR(')'); /* consume ')' */
GET_CHR('='); /* consume '=' */
type = -1; /* init. attribute type to 'none' */
t = sc_token(scan); /* test next token */
if (t == '{') /* if a set of values follows, */
type = tflags & AT_NOM; /* attribute is nominal */
else if (t == T_ID) { /* if an identifier follows */
v = sc_value(scan); /* get it for simpler comparisons */
if ((strcmp(v, "ZZ") == 0)
|| (strcmp(v, "Z") == 0)
|| (strcmp(v, "int") == 0)
|| (strcmp(v, "integer") == 0))
type = tflags & AT_INT; /* attribute is integer-valued */
else if ((strcmp(v, "IR") == 0)
|| (strcmp(v, "R") == 0)
|| (strcmp(v, "real") == 0)
|| (strcmp(v, "float") == 0))
type = tflags & AT_REAL;/* attribute is real-valued */
} /* (get and check attribute type) */
if (type <= 0) ERROR(E_DOMAIN);
att->type = type; /* set attribute type */
if (type != AT_NOM) { /* if attribute is numeric */
GET_TOK(); /* consume type indicator */
if (type == AT_INT) { /* if attribute is integer-valued */
att->min.i = INT_MAX; /* initialize minimal */
att->max.i = -INT_MAX;} /* and maximal value */
else { /* if attribute is real-valued */
att->min.f = FLT_MAX; /* initialize minimal */
att->max.f = -FLT_MAX; /* and maximal value */
}
if (sc_token(scan) == '[') { /* if a range of values is given */
GET_TOK(); /* consume '[' */
if (sc_token(scan) != T_NUM) ERROR(E_NUMEXP);
if (att_valadd(att, sc_value(scan), NULL) != 0)
ERROR(E_NUMBER); /* get and check lower bound */
GET_TOK(); /* consume lower bound */
GET_CHR(','); /* consume ',' */
if (sc_token(scan) != T_NUM) ERROR(E_NUMEXP);
if (att_valadd(att, sc_value(scan), NULL) != 0)
ERROR(E_NUMBER); /* get and check upper bound */
GET_TOK(); /* consume upper bound */
GET_CHR(']'); /* consume ']' */
} }
else { /* if attribute is nominal */
GET_CHR('{'); /* consume '{' */
if (sc_token(scan) != '}') {
while (1) { /* read a list of values */
t = sc_token(scan); /* check for a name */
if ((t != T_ID) && (t != T_NUM)) ERROR(E_VALEXP);
t = att_valadd(att, sc_value(scan), NULL);
if (t) ERROR((t > 0) ? E_DUPVAL : E_NOMEM);
GET_TOK(); /* get and consume attribute value */
if (sc_token(scan) != ',') break;
GET_TOK(); /* if at end of list, abort loop, */
} /* otherwise consume ',' */
}
GET_CHR('}'); /* consume '}' */
}
if (sc_token(scan) == ':'){ /* if a direction indication follows */
GET_TOK(); /* consume ',' */
if (sc_token(scan) != T_ID) ERR_STR("in");
v = sc_value(scan); /* get the direction indicator */
if (strcmp(v, "none") == 0) att->dir = DIR_NONE;
else if (strcmp(v, "in") == 0) att->dir = DIR_IN;
else if (strcmp(v, "out") == 0) att->dir = DIR_OUT;
else if (strcmp(v, "id") == 0) att->dir = DIR_ID;
else if (strcmp(v, "wgt") == 0) att->dir = DIR_WGT;
else ERR_STR("in"); /* get the direction code */
GET_TOK(); /* and consume the token */
}
if (sc_token(scan) == ','){ /* if a weight indication follows */
if (sc_token(scan) != T_NUM) ERROR(E_NUMEXP);
wgt = atof(sc_value(scan)); /* get the attribute weight */
if ((wgt <= NV_REAL) || (wgt > FLT_MAX)) ERROR(E_NUMBER);
att->weight = (float)wgt; /* check and set attribute weight */
GET_TOK(); /* and consume the token */
}
GET_CHR(';'); /* consume ';' */
} /* while ((sc_token(scan) == T_ID) .. */
return 0; /* return 'ok' */
} /* _domains() */
/* 发送采样数据 */
static void send_capture_data(void)
{
uint32_T type = 0;
/* 避免函数退出 栈内存被破坏(DMA传输 要求内存数据保持) */
static uint8_T frame_buf[COMM_FRAME_CAPTURE_FRAME_MAX_SIZE] = {0};
uint32_T len = 0;
uint32_T i = 0;
uint32_T fill_bytes_count = 0;
uint32_T crc32_calculated = 0;
uint32_T now_ms = 0;
f32_T quat[4] = {0.0f};
f32_T euler[CTRL_EULER_MAX] = {0.0f};
f32_T pid_out[CTRL_EULER_MAX] = {0.0f};
int32_T accelerator_max = 0;
int32_T accelerator[PWM_MAX] = {0};
uint32_T *p_ui32 = NULL;
static uint32_T last_ms = 0;
/* 无帧可发 */
if(!(s_send_interval
|| s_send_accelerator_flag
|| s_send_dmp_quat_flag
|| s_send_euler_flag
|| s_send_pid_flag))
{
return;
}
/* 可以发送 */
now_ms = HAL_GetTick();
if(now_ms - last_ms > s_send_interval)
{
/* 上行且有传感数据长度32 */
type = COMM_FRAME_SENSOR_DATA_BIT
| COMM_FRAME_DIRECTION_BIT;
/* 跳过type和len域 */
len += 8;
/* 填充时间 */
frame_buf[len++] = (uint8_T)(now_ms >> 24);
frame_buf[len++] = (uint8_T)((now_ms >> 16));
frame_buf[len++] = (uint8_T)((now_ms >> 8));
frame_buf[len++] = (uint8_T)(now_ms);
if(s_send_dmp_quat_flag)
{
mpu9250_get_quat(quat);
#if 0
/* 调试上位机绘图基准 */
quat[0] = 1.0f;
quat[1] = 0.0f;
quat[2] = 0.0f;
quat[3] = 0.0f;
#endif
p_ui32 = (uint32_T *)quat;
for(i = 0; i < 4; i++)
{
frame_buf[len++] = (uint8_T)( (uint32_T)p_ui32[i] >> 24);
frame_buf[len++] = (uint8_T)(((uint32_T)p_ui32[i] >> 16));
frame_buf[len++] = (uint8_T)(((uint32_T)p_ui32[i] >> 8));
frame_buf[len++] = (uint8_T)( (uint32_T)p_ui32[i]);
}
type |= COMM_FRAME_DMP_QUAT_BIT;
}
/* 油门数据 */
if(s_send_accelerator_flag)
{
ctrl_get_acceleralor(accelerator, &accelerator_max);
for(i = 0; i < PWM_MAX; i++)
{
frame_buf[len++] = (uint8_T)(accelerator[i] >> 24);
frame_buf[len++] = (uint8_T)(accelerator[i] >> 16);
frame_buf[len++] = (uint8_T)(accelerator[i] >> 8);
frame_buf[len++] = (uint8_T)(accelerator[i]);
}
frame_buf[len++] = (uint8_T)(accelerator_max >> 24);
frame_buf[len++] = (uint8_T)(accelerator_max >> 16);
frame_buf[len++] = (uint8_T)(accelerator_max >> 8);
frame_buf[len++] = (uint8_T)(accelerator_max);
type |= COMM_FRAME_ACCELERATOR_DATA_BIT;
}
/* 欧拉角 */
if(s_send_euler_flag)
{
mpu9250_get_quat(quat);
math_quaternion2euler(euler, quat);
p_ui32 = (uint32_T *)euler;
for(i = 0; i < CTRL_EULER_MAX; i++)
{
frame_buf[len++] = (uint8_T)(p_ui32[i] >> 24);
frame_buf[len++] = (uint8_T)(p_ui32[i] >> 16);
frame_buf[len++] = (uint8_T)(p_ui32[i] >> 8);
frame_buf[len++] = (uint8_T)(p_ui32[i]);
}
type |= COMM_FRAME_EULER_DATA_BIT;
}
/* pid数据 */
if(s_send_pid_flag)
{
ctrl_get_pid_out(pid_out);
p_ui32 = (uint32_T *)pid_out;
for(i = 0; i < CTRL_EULER_MAX; i++)
{
frame_buf[len++] = (uint8_T)(p_ui32[i] >> 24);
frame_buf[len++] = (uint8_T)(p_ui32[i] >> 16);
frame_buf[len++] = (uint8_T)(p_ui32[i] >> 8);
frame_buf[len++] = (uint8_T)(p_ui32[i]);
}
type |= COMM_FRAME_PID_DATA_BIT;
}
/* 填充 */
fill_bytes_count = len & 0x03;
for(i = 0; i < fill_bytes_count; i++)
{
frame_buf[len++] = COMM_FRAME_FILLED_VAL;
}
/* 填写type域 */
frame_buf[0] = (uint8_T)(type >> 24);
frame_buf[1] = (uint8_T)(type >> 16);
frame_buf[2] = (uint8_T)(type >> 8);
frame_buf[3] = (uint8_T)(type);
/* 计入crc长度 */
len += 4;
len -= 8; /* len中长度值不包括type+len */
/* 填写len域 */
frame_buf[4] = (uint8_T)(len >> 24);
frame_buf[5] = (uint8_T)(len >> 16);
frame_buf[6] = (uint8_T)(len >> 8);
frame_buf[7] = (uint8_T)(len);
/* len偏移到crc位置 */
len += 4;
/* 计算校验 */
crc32_calculated = HAL_CRC_Calculate(&s_crc, (uint32_T *)frame_buf, len / 4);
frame_buf[len++] = (uint8_T)(crc32_calculated >> 24);
frame_buf[len++] = (uint8_T)(crc32_calculated >> 16);
frame_buf[len++] = (uint8_T)(crc32_calculated >> 8);
frame_buf[len++] = (uint8_T)(crc32_calculated);
/* 发帧 */
if(len > COMM_FRAME_SENDED_MIN)
{
#if 0
/* 出错 用于检查 crc是否为0 */
if(0 == frame_buf[n-1])
{
ERR_STR("crc错误.");
}
#endif
uart_send_bytes((drv_uart_T *)s_comm_uart, frame_buf, len);
last_ms = now_ms;
}
}
