问题出现在uwb_loc.c文件中,UWB_Sensor_Collect()计算出的变量无法在Get_UWB_DOUBLE_Move_Data()函数中使用。
freertos实现机器人UWB定位(定位只是一部分)。在UWB_Sensor_Collect()函数中打印计算出的变量,可以正确显示,如图1。但是在Get_UWB_DOUBLE_Move_Data()函数中使用这几个变量时他们都变成0了。相关代码如下
freertos.c
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"
#include "key.h"
#include "data.h"
#include "timers.h"
#include "event_groups.h"
#include "basic.h"
#include "sense.h"
#include "act.h"
#include "bt_data.h"
#define SOFTWARE_TIMER_REACH_EVENT (1 << 1)
#define ROBOT_FLY_BEGIN_TIME 6000
osThreadId_t KeyScanTaskHandle;
const osThreadAttr_t KeyScanTask_attributes = {
.name = "KeyScanTask",
.stack_size = 512 * 4,
.priority = (osPriority_t) osPriorityHigh,
};
osThreadId_t SensorWorkTaskHandle;
const osThreadAttr_t SensorWorkTask_attributes = {
.name = "SensorWorkTask",
.stack_size = 2560 * 4,
.priority = (osPriority_t) osPriorityNormal,
};
osThreadId_t MotorWorkTaskHandle;
const osThreadAttr_t MotorWorkTask_attributes = {
.name = "MotorWorkTask",
.stack_size = 2560 * 4,
.priority = (osPriority_t) osPriorityNormal,
};
void KeyScanTaskFun(void *argument);
void SensorWorkTaskFun(void *argument);
void MotorWorkTaskFun(void *argument);
void FlyBeginTimerCallback(void *argument);
void MX_FREERTOS_Init(void);
void MX_FREERTOS_Init(void) {
FlyBeginTimerHandle = osTimerNew(FlyBeginTimerCallback, osTimerOnce, NULL, &FlyBeginTimer_attributes);
KeyScanTaskHandle = osThreadNew(KeyScanTaskFun, NULL, &KeyScanTask_attributes);
SensorWorkTaskHandle = osThreadNew(SensorWorkTaskFun, NULL, &SensorWorkTask_attributes);
MotorWorkTaskHandle = osThreadNew(MotorWorkTaskFun, NULL, &MotorWorkTask_attributes);
}
void KeyScanTaskFun(void *argument)
{
/* USER CODE BEGIN KeyScanTaskFun */
TickType_t keyscantick = pdMS_TO_TICKS(KEYPRESS_ONCE_TIME);
TickType_t prekeyscantime = xTaskGetTickCount();
/* Infinite loop */
for(;;)
{
Key_State_Machine(); //实现按键扫描和消抖
vTaskDelayUntil(&prekeyscantime, keyscantick);
}
}
void SensorWorkTaskFun(void *argument)
{
/* USER CODE BEGIN SensorWorkTaskFun */
TickType_t sensorworktick = pdMS_TO_TICKS(SENSOR_WORKING_CYCLE);
TickType_t presensorworktime = xTaskGetTickCount();
taskENTER_CRITICAL(); //进入临界区
taskEXIT_CRITICAL();
/* Infinite loop */
for(;;)
{
taskENTER_CRITICAL();
UWB_Sensor_Collect();
taskEXIT_CRITICAL(); //出临界区
vTaskDelayUntil(&presensorworktime, sensorworktick);
}
/* USER CODE END SensorWorkTaskFun */
}
void MotorWorkTaskFun(void *argument)
{
/* USER CODE BEGIN MotorWorkTaskFun */
TickType_t motorworktick = pdMS_TO_TICKS(MOTOR_WORKING_CYCLE);
TickType_t premotorworktime = xTaskGetTickCount();
taskENTER_CRITICAL();
struct Double_UWB_Data move_data_m;
taskEXIT_CRITICAL();
/* Infinite loop */
for(;;)
{
taskENTER_CRITICAL();
Get_UWB_DOUBLE_Move_Data(&move_data_m);
taskEXIT_CRITICAL();
vTaskDelayUntil(&premotorworktime, motorworktick);
}
/* USER CODE END MotorWorkTaskFun */
}
void FlyBeginTimerCallback(void *argument) //FreeRTOS事件组
{
/* USER CODE BEGIN FlyBeginTimerCallback */
xEventGroupSetBits(GaussVectorEventHandle, SOFTWARE_TIMER_REACH_EVENT);
/* USER CODE END FlyBeginTimerCallback */
}
uwb_loc.c
#include "uwb_loc.h"
#include "basic.h"
static unsigned char dis_str[40] = {0};
static double dis_4[4] = {0,0,0,0};
static unsigned char uwb_ready_flag = 0;
static vec3d anchor[4]; //四个基站坐标
static vec3d current[1]; //机器人当前坐标
static vec3d current_spd[1]; //机器人当前速度
static double copeuwb(unsigned char * head)
{
double sum = 0;
double temp_uwb = 0;
int8_t i,k;
for(i = 7,k = 0; i >= 0; i--,k++)
{
switch(head[k])
{
case '0':temp_uwb = 0;break;
case '1':temp_uwb = 1;break;
case '2':temp_uwb = 2;break;
case '3':temp_uwb = 3;break;
case '4':temp_uwb = 4;break;
case '5':temp_uwb = 5;break;
case '6':temp_uwb = 6;break;
case '7':temp_uwb = 7;break;
case '8':temp_uwb = 8;break;
case '9':temp_uwb = 9;break;
case 'a':temp_uwb = 10;break;
case 'b':temp_uwb = 11;break;
case 'c':temp_uwb = 12;break;
case 'd':temp_uwb = 13;break;
case 'e':temp_uwb = 14;break;
case 'f':temp_uwb = 15;break;
default:break;
}
sum += temp_uwb * pow(16,i);
}
sum /= 10; //毫米mm 转换为 厘米cm
return sum;
}
void Cope_UWB_Loc_Serial_Data(unsigned char ucData)
{
unsigned char temp = 0;
static USART_STATE uwb_state = head;
static uint8_t date_cnt = 0;
uint8_t i;
temp = ucData;
if(uwb_state == head)
{
if(temp == 'm')
uwb_state = m_c_begin;
else
uwb_state = head;
}
else if(uwb_state == m_c_begin)
{
dis_str[date_cnt++] = temp;
if(date_cnt == 42)
{
date_cnt = 0;
uwb_state = m_c_check;
}
}
else if(uwb_state == m_c_check)
{
if(dis_str[0] == 'c' && dis_str[31] == ' ')
uwb_state = m_c_ready;
else
{
uwb_state = head;
for(i = 0; i < 40 ; i++)
dis_str[i] = 0;
}
}
else if(uwb_state == m_c_ready)
{
dis_4[0] = copeuwb(dis_str+5);
dis_4[1] = copeuwb(dis_str+14);
dis_4[2] = copeuwb(dis_str+23);
dis_4[3] = copeuwb(dis_str+32);
// printf("UWB_LOC: %f %f %f %f\r\n", dis_4[0], dis_4[1], dis_4[2], dis_4[3]);
uwb_ready_flag = 1;
uwb_state = head;
}
else {
}
}
void UWB_AnchorInit(void)
{
//-----锚点坐标设置cm----//
anchor[0].x = 0;
anchor[0].y = 0;
anchor[0].z = 17;//17cm
anchor[1].x = 280;
anchor[1].y = 320;
anchor[1].z = 17;
anchor[2].x = 0;
anchor[2].y = 320;
anchor[2].z = 17;
anchor[3].x = 320;
anchor[3].y = 280;
anchor[3].z = 17;
current[0].x = 0;
current[0].y = 0;
current[0].z = 0;
current_spd[0].x = 0;
current_spd[0].y = 0;
current_spd[0].z = 0;
}
void UWB_Sensor_Collect(void)
{
if (uwb_ready_flag == 1) {
GetLocation(current, 1, anchor, dis_4);
printf("current->x: %f current->y: %f current->z: %f\r\n", current->x, current->y, current->z);
uwb_ready_flag = 0;
}
}
void Get_UWB_DOUBLE_Move_Data(struct Double_UWB_Data *uwb_inf)
{
static double distance_x_pre = 0;
static double distance_y_pre = 0;
static double distance_z_pre = 0;
printf("current->x: %f current->y: %f current->z: %f\r\n", current->x, current->y, current->z);
uwb_inf->x_loc = current->x;
uwb_inf->y_loc = current->y;
uwb_inf->z_loc = current->z;
current_spd->x = 10 * current->x - 10 * distance_x_pre;
current_spd->y = 10 * current->y - 10 * distance_y_pre;
current_spd->z = 10 * current->z - 10 * distance_z_pre;
distance_x_pre = current->x;
distance_y_pre = current->y;
distance_z_pre = current->z;
if (current_spd->x > 999.0) {
current_spd->x = 999.0;
}else if(current_spd->x < -999.0){
current_spd->x = -999.0;
}
else {
uwb_inf->x_spd = current_spd->x;
}
if (current_spd->y > 999.0) {
current_spd->y = 999.0;
}else if(current_spd->y < -999.0){
current_spd->y = -999.0;
}else {
uwb_inf->y_spd = current_spd->y;
}
if (current_spd->z > 999.0) {
current_spd->z = 999.0;
}else if(current_spd->z < -999.0){
current_spd->z = -999.0;
}else {
uwb_inf->z_spd = current_spd->z;
}
uwb_inf->x_spd = current_spd->x;
uwb_inf->y_spd = current_spd->y;
uwb_inf->z_spd = current_spd->z;
}
图1
相似问题