|
|
|
|
@ -4,15 +4,48 @@ static uint8_t data = 0;
@@ -4,15 +4,48 @@ static uint8_t data = 0;
|
|
|
|
|
static uint8_t step = 0; |
|
|
|
|
uint16_t ar1271_rev_flag = 0; |
|
|
|
|
|
|
|
|
|
struct mr72_c{ |
|
|
|
|
uint32_t time_ms; |
|
|
|
|
float range_f; |
|
|
|
|
uint16_t range; |
|
|
|
|
float azimuth; |
|
|
|
|
float vrel; |
|
|
|
|
uint8_t index; |
|
|
|
|
uint8_t rollcount; |
|
|
|
|
uint8_t rcs; |
|
|
|
|
}mr72_c_data; |
|
|
|
|
|
|
|
|
|
uint16_t range_arr[8]; |
|
|
|
|
float last_range; |
|
|
|
|
uint32_t last_time_ms; |
|
|
|
|
uint8_t range_index; |
|
|
|
|
uint8_t index_arr[8]; |
|
|
|
|
void decode_ar1271(char c,uint8_t index); |
|
|
|
|
|
|
|
|
|
bool decode_mr72(int16_t c); |
|
|
|
|
void data_process(void); |
|
|
|
|
void sortA1(uint16_t a[], uint8_t length); |
|
|
|
|
void parse_ar1271_data(usart_data_t *usart_data) |
|
|
|
|
{ |
|
|
|
|
osSemaphoreAcquire(radar_parse_mutexHandle,30); |
|
|
|
|
uint8_t buffer_count; |
|
|
|
|
uint8_t buffer[LIDAR_360_SIZE] = {0}; |
|
|
|
|
uint8_t message_count; |
|
|
|
|
|
|
|
|
|
for (int i = 0; i < usart_data->len; i++) |
|
|
|
|
{ |
|
|
|
|
char c = usart_data->data[i]; |
|
|
|
|
if(1){ |
|
|
|
|
decode_mr72(c); |
|
|
|
|
}else{ |
|
|
|
|
decode_ar1271(c,usart_data->usart_instance); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
data_process(); |
|
|
|
|
osSemaphoreRelease(radar_parse_mutexHandle); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void decode_ar1271(char c,uint8_t index){ |
|
|
|
|
static uint8_t buffer_count; |
|
|
|
|
static uint8_t buffer[LIDAR_360_SIZE] = {0}; |
|
|
|
|
static uint8_t message_count; |
|
|
|
|
if ('T' == c) |
|
|
|
|
{ |
|
|
|
|
buffer_count = 0; |
|
|
|
|
@ -35,17 +68,17 @@ void parse_ar1271_data(usart_data_t *usart_data)
@@ -35,17 +68,17 @@ void parse_ar1271_data(usart_data_t *usart_data)
|
|
|
|
|
{ |
|
|
|
|
proximity_data_t proximity_data = {0}; |
|
|
|
|
proximity_data.d0 = uint16_value(buffer[2], buffer[3]); |
|
|
|
|
proximity_data.d_right = uint16_value(buffer[4], buffer[5]); |
|
|
|
|
proximity_data.d_left = uint16_value(buffer[16], buffer[17]); |
|
|
|
|
proximity_data.d315 = uint16_value(buffer[4], buffer[5]); |
|
|
|
|
proximity_data.d45 = uint16_value(buffer[16], buffer[17]); |
|
|
|
|
|
|
|
|
|
if (usart_data->usart_instance == 1) |
|
|
|
|
if (index == 1) |
|
|
|
|
{ |
|
|
|
|
if (proximity1_queueHandle != NULL) |
|
|
|
|
{ |
|
|
|
|
osMessageQueuePut(proximity1_queueHandle, &proximity_data, 0, 10); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
else if (usart_data->usart_instance == 2) |
|
|
|
|
else if (index == 2) |
|
|
|
|
{ |
|
|
|
|
if (proximity2_queueHandle != NULL) |
|
|
|
|
{ |
|
|
|
|
@ -54,10 +87,145 @@ void parse_ar1271_data(usart_data_t *usart_data)
@@ -54,10 +87,145 @@ void parse_ar1271_data(usart_data_t *usart_data)
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
bool decode_mr72(int16_t c) |
|
|
|
|
{ |
|
|
|
|
bool ret = false; |
|
|
|
|
static uint8_t decode_state; |
|
|
|
|
static uint8_t data_len = 0; |
|
|
|
|
static int8_t data_cnt; |
|
|
|
|
static int16_t value_b[8]; |
|
|
|
|
int16_t temp = c; |
|
|
|
|
switch (decode_state) |
|
|
|
|
{ |
|
|
|
|
case 0: |
|
|
|
|
if(temp == 0xAA) |
|
|
|
|
decode_state = 1; |
|
|
|
|
break; |
|
|
|
|
case 1: |
|
|
|
|
if(temp == 0xAA){ |
|
|
|
|
decode_state = 2; |
|
|
|
|
} |
|
|
|
|
osSemaphoreRelease(radar_parse_mutexHandle); |
|
|
|
|
else |
|
|
|
|
decode_state = 0; |
|
|
|
|
break; |
|
|
|
|
case 2: |
|
|
|
|
if(temp == 0x0c){ |
|
|
|
|
decode_state = 3; |
|
|
|
|
} |
|
|
|
|
else |
|
|
|
|
decode_state = 0; |
|
|
|
|
break; |
|
|
|
|
case 3: |
|
|
|
|
if(temp == 0x07){ |
|
|
|
|
decode_state = 4; |
|
|
|
|
data_len = 8; |
|
|
|
|
} |
|
|
|
|
else |
|
|
|
|
decode_state = 0; |
|
|
|
|
break; |
|
|
|
|
case 4: |
|
|
|
|
data_cnt = 8 - data_len; |
|
|
|
|
if(data_cnt < 7){{ |
|
|
|
|
value_b[data_cnt] = temp; |
|
|
|
|
data_len -= 1; |
|
|
|
|
} |
|
|
|
|
}else{ |
|
|
|
|
decode_state = 5; |
|
|
|
|
} |
|
|
|
|
break; |
|
|
|
|
case 5: |
|
|
|
|
if(temp == 0x55){ |
|
|
|
|
decode_state = 6; |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
else |
|
|
|
|
decode_state = 0; |
|
|
|
|
break; |
|
|
|
|
case 6: |
|
|
|
|
|
|
|
|
|
if(temp == 0x55){ |
|
|
|
|
mr72_c_data.time_ms = HAL_GetTick(); |
|
|
|
|
mr72_c_data.index = value_b[0]; |
|
|
|
|
mr72_c_data.range_f = (value_b[2] * 256 + value_b[3]) * 0.01; |
|
|
|
|
mr72_c_data.range = uint16_value(value_b[2], value_b[3]); |
|
|
|
|
mr72_c_data.azimuth = (value_b[1] * 256 + value_b[4]) * 0.01 - 90; |
|
|
|
|
mr72_c_data.vrel = (value_b[5] * 256 + value_b[6]) * 0.05 - 35; |
|
|
|
|
mr72_c_data.rollcount = value_b[5] & 0x03; |
|
|
|
|
mr72_c_data.rcs = value_b[7] * 0.5 - 50; |
|
|
|
|
ret = true; |
|
|
|
|
if((last_range - mr72_c_data.range_f > 0.5) || (last_range - mr72_c_data.range_f < -0.5)){ |
|
|
|
|
bool had_index = false; |
|
|
|
|
for(uint8_t i=0; i<range_index; i++){ |
|
|
|
|
if(index_arr[i] == mr72_c_data.index){ |
|
|
|
|
had_index = true; |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
last_range = mr72_c_data.range; |
|
|
|
|
if(range_index < 8 && !had_index){ |
|
|
|
|
range_arr[range_index] = mr72_c_data.range; |
|
|
|
|
index_arr[range_index] = mr72_c_data.index; |
|
|
|
|
range_index +=1; |
|
|
|
|
}
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
decode_state = 0; |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
default: |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return ret; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void data_process(void){ |
|
|
|
|
|
|
|
|
|
if(mr72_c_data.time_ms - last_time_ms > 50 || HAL_GetTick() - last_time_ms > 150){ |
|
|
|
|
if(last_time_ms != mr72_c_data.time_ms){ |
|
|
|
|
last_time_ms = mr72_c_data.time_ms; |
|
|
|
|
// gcs().send_text(MAV_SEVERITY_INFO, "a:%d , %d, %d, %d, %d, %d, %d, %d",range_arr[0],range_arr[1],range_arr[2],range_arr[3],range_arr[4],range_arr[5],range_arr[6],range_arr[7]);
|
|
|
|
|
sortA1(range_arr,8); |
|
|
|
|
proximity_t data; |
|
|
|
|
data.d315 = range_arr[0]; |
|
|
|
|
data.d0 = range_arr[1]; |
|
|
|
|
data.d45 = range_arr[2]; |
|
|
|
|
data.d90 = range_arr[3]; |
|
|
|
|
data.d135 = range_arr[4]; |
|
|
|
|
data.d180 = range_arr[5]; |
|
|
|
|
data.d225 = range_arr[6]; |
|
|
|
|
data.d270 = range_arr[7]; |
|
|
|
|
if (proximity1_queueHandle != NULL) |
|
|
|
|
{ |
|
|
|
|
osMessageQueuePut(proximity1_queueHandle, &data, 0, 10); |
|
|
|
|
} |
|
|
|
|
for (uint8_t i = 0; i < 8; i++) |
|
|
|
|
{ |
|
|
|
|
range_arr[i] = 4000; |
|
|
|
|
index_arr[i] = 0; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
range_index = 0; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
void sortA1(uint16_t a[], uint8_t length){ |
|
|
|
|
uint8_t i, j; |
|
|
|
|
float temp; |
|
|
|
|
for(i = 0; i < length; ++i){ |
|
|
|
|
for(j = i + 1; j < length; ++j){ |
|
|
|
|
a[i] = a[i] == 0? 4000:a[i]; |
|
|
|
|
a[j] = a[j] == 0? 4000:a[j]; |
|
|
|
|
if(a[j] < a[i]){ //如果后一个元素小于前一个元素则交换
|
|
|
|
|
temp = a[i]; |
|
|
|
|
a[i] = a[j]; |
|
|
|
|
a[j] = temp; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
static const uint8_t crc8_table[] = { |
|
|
|
|
0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, |
|
|
|
|
0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, |
|
|
|
|
|
