zrzk
/
proximity_uavcan_converter
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

区分扇区测距，未成功

uav-1-target
zbr 3 years ago
parent
bc6128bad7
commit
2f0b142ad0
5 changed files with 2052 additions and 242 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 122
      BSP/insifhtica_QOAR1271.c
  2. 2015
      MDK-ARM/proximity_uavcan_converter.uvguix.zengb
  3. 49
      MDK-ARM/proximity_uavcan_converter.uvoptx
  4. 15
      MDK-ARM/proximity_uavcan_converter.uvprojx
  5. 93
      MDK-ARM/startup_stm32f103xb.lst

122
BSP/insifhtica_QOAR1271.c
Unescape View File

@ -1,7 +1,8 @@ @@ -1,7 +1,8 @@
#include "insifhtica_QOAR1271.h"
#include "uavcan.h"
#include "custom_data_def.h"
#define MAX_MR71_DIST_CM 6000
#include <math.h>
#define MAX_MR71_DIST_CM 4000
uint8_t ar1271_data[LIDAR_360_SIZE];
//static uint8_t data = 0;
@ -10,6 +11,8 @@ uint16_t ar1271_rev_flag = 0; @@ -10,6 +11,8 @@ uint16_t ar1271_rev_flag = 0;
uint32_t last_rev_usart1_tick;
uint32_t last_rev_usart2_tick;
uint32_t last_data_tick;
uint32_t uavcan_data_tick;
struct mr72_c{
@ -23,6 +26,17 @@ struct mr72_c{ @@ -23,6 +26,17 @@ struct mr72_c{
uint8_t rcs;
}mr72_c_data;
typedef struct dist_area_s
{
uint16_t mid;
uint16_t left;
uint16_t right;
};
struct dist_area_s min_dist;
struct dist_area_s new_dist;
struct dist_area_s last_dist;
uint16_t range_arr[8];
float last_range;
uint32_t last_time_ms;
@ -31,6 +45,7 @@ uint8_t index_arr[8]; @@ -31,6 +45,7 @@ uint8_t index_arr[8];
void decode_ar1271(char c,uint8_t index);
bool decode_mr72(int16_t c);
void data_process_old(void);
void data_process(void);
void sortA1(uint16_t a[], uint8_t length);
@ -164,21 +179,45 @@ bool decode_mr72(int16_t c) @@ -164,21 +179,45 @@ bool decode_mr72(int16_t c)
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;
}
// 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_f;
// 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;
// }
// }
// if((last_range - mr72_c_data.range_f > 0.5) || (last_range - mr72_c_data.range_f < -0.5)){
// if(mr72_c_data.azimuth < -30.0){ // 左边区域
// if((abs(new_dist.left - mr72_c_data.range) > 10)){
// new_dist.left = mr72_c_data.range;
// }
// }else if(mr72_c_data.azimuth < 30.0){ // 中间区域
// if((abs(new_dist.mid - mr72_c_data.range) > 10)){
// new_dist.mid = mr72_c_data.range;
// }
// }else{ // 右边区域
// if((abs(new_dist.right - mr72_c_data.range) > 10)){
// new_dist.right = mr72_c_data.range;
// }
// }
// last_range = mr72_c_data.range_f;
if(fabs(mr72_c_data.azimuth) < 30.0 ){ // 中间区域
if((abs(new_dist.mid - mr72_c_data.range) > 10)){
new_dist.mid = mr72_c_data.range;
}
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;
@ -191,6 +230,59 @@ bool decode_mr72(int16_t c) @@ -191,6 +230,59 @@ bool decode_mr72(int16_t c)
}
void data_process(void){
// 其中一个区块数据更新
if (abs(new_dist.mid - last_dist.mid)>10 || abs(new_dist.left - last_dist.left)>10 || abs(new_dist.right - last_dist.right)>10)
{
// 更新最后的数据
last_dist.mid = new_dist.mid;
last_dist.left = new_dist.left;
last_dist.right = new_dist.right;
if(min_dist.mid < 10){ // 如果最小值是0，则直接赋值
min_dist.mid = last_dist.mid;
}else{
min_dist.mid = min_dist.mid < last_dist.mid?min_dist.mid : last_dist.mid;
}
if(min_dist.left < 10){
min_dist.left = last_dist.left;
}else{
min_dist.left = min_dist.left < last_dist.left?min_dist.left : last_dist.left;
}
if(min_dist.right < 10){
min_dist.right = last_dist.right;
}else{
min_dist.right = min_dist.right < last_dist.right?min_dist.right : last_dist.right;
}
last_data_tick = HAL_GetTick();
}else{
if (HAL_GetTick() - last_data_tick > 500) // 数据0.5s没有更新，最小值赋值个大的值
{
min_dist.mid = 3900;
min_dist.left = 3900;
min_dist.right = 3900;
last_data_tick = HAL_GetTick();
}
}
if (proximity1_queueHandle != NULL && HAL_GetTick() - uavcan_data_tick > 50) // uavcan发送，50ms间隔
{
proximity_t data;
data.d0 = min_dist.mid;
data.d45 = min_dist.left;
data.d90 = min_dist.right;
data.d135 = 4500;
data.d180 = 4500;
data.d225 = 4500;
data.d270 = 4500;
data.d315 = 4500;
myOsMessageQueuePut(proximity1_queueHandle, &data, 0, 10);
uavcan_data_tick = HAL_GetTick();
}
}
void data_process_old(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){

2015
MDK-ARM/proximity_uavcan_converter.uvguix.zengb
View File

File diff suppressed because one or more lines are too long

49
MDK-ARM/proximity_uavcan_converter.uvoptx
Unescape View File

@ -152,9 +152,9 @@ @@ -152,9 +152,9 @@
<Bp>
<Number>0</Number>
<Type>0</Type>
<LineNumber>573</LineNumber>
<LineNumber>118</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>134250792</Address>
<Address>134256654</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
@ -163,64 +163,55 @@ @@ -163,64 +163,55 @@
<BreakIfRCount>1</BreakIfRCount>
<Filename>..\UAVCAN\uavcan.c</Filename>
<ExecCommand></ExecCommand>
<Expression>\\proximity_uavcan_converter\../UAVCAN/uavcan.c\573</Expression>
<Expression>\\proximity_uavcan_converter\../UAVCAN/uavcan.c\118</Expression>
</Bp>
<Bp>
<Number>1</Number>
<Type>0</Type>
<LineNumber>118</LineNumber>
<LineNumber>574</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>0</Address>
<Address>134251600</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>0</BreakIfRCount>
<BreakIfRCount>1</BreakIfRCount>
<Filename>..\UAVCAN\uavcan.c</Filename>
<ExecCommand></ExecCommand>
<Expression></Expression>
<Expression>\\proximity_uavcan_converter\../UAVCAN/uavcan.c\574</Expression>
</Bp>
<Bp>
<Number>2</Number>
<Type>0</Type>
<LineNumber>119</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>0</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>0</BreakIfRCount>
<Filename>..\UAVCAN\uavcan.c</Filename>
<ExecCommand></ExecCommand>
<Expression></Expression>
</Bp>
<Bp>
<Number>3</Number>
<Type>0</Type>
<LineNumber>574</LineNumber>
<LineNumber>127</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>0</Address>
<Address>134250276</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>0</BreakIfRCount>
<Filename>..\UAVCAN\uavcan.c</Filename>
<BreakIfRCount>1</BreakIfRCount>
<Filename>..\BSP\insifhtica_QOAR1271.c</Filename>
<ExecCommand></ExecCommand>
<Expression></Expression>
<Expression>\\proximity_uavcan_converter\../BSP/insifhtica_QOAR1271.c\127</Expression>
</Bp>
</Breakpoint>
<WatchWindow1>
<Ww>
<count>0</count>
<WinNumber>1</WinNumber>
<ItemText>mr72_c_data.range,0x0A</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>
</Tracepoint>
<DebugFlag>
<trace>0</trace>
<periodic>1</periodic>
<aLwin>0</aLwin>
<aLwin>1</aLwin>
<aCover>0</aCover>
<aSer1>0</aSer1>
<aSer2>0</aSer2>

15
MDK-ARM/proximity_uavcan_converter.uvprojx
Unescape View File

@ -10,13 +10,13 @@ @@ -10,13 +10,13 @@
<TargetName>proximity_uavcan_converter</TargetName>
<ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName>
<pCCUsed>5060960::V5.06 update 7 (build 960)::.\ARMCC</pCCUsed>
<pCCUsed>5060750::V5.06 update 6 (build 750)::ARMCC</pCCUsed>
<uAC6>0</uAC6>
<TargetOption>
<TargetCommonOption>
<Device>STM32F103T8</Device>
<Vendor>STMicroelectronics</Vendor>
<PackID>Keil.STM32F1xx_DFP.2.3.0</PackID>
<PackID>Keil.STM32F1xx_DFP.2.2.0</PackID>
<PackURL>http://www.keil.com/pack/</PackURL>
<Cpu>IRAM(0x20000000-0x20004FFF) IROM(0x8000000-0x800FFFF) CLOCK(8000000) CPUTYPE("Cortex-M3") TZ</Cpu>
<FlashUtilSpec></FlashUtilSpec>
@ -185,11 +185,10 @@ @@ -185,11 +185,10 @@
<uocXRam>0</uocXRam>
<RvdsVP>0</RvdsVP>
<RvdsMve>0</RvdsMve>
<RvdsCdeCp>0</RvdsCdeCp>
<hadIRAM2>0</hadIRAM2>
<hadIROM2>0</hadIROM2>
<StupSel>8</StupSel>
<useUlib>0</useUlib>
<useUlib>1</useUlib>
<EndSel>0</EndSel>
<uLtcg>0</uLtcg>
<nSecure>0</nSecure>
@ -352,7 +351,7 @@ @@ -352,7 +351,7 @@
<NoWarn>0</NoWarn>
<uSurpInc>0</uSurpInc>
<useXO>0</useXO>
<ClangAsOpt>4</ClangAsOpt>
<uClangAs>0</uClangAs>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
@ -566,7 +565,7 @@ @@ -566,7 +565,7 @@
<NoWarn>2</NoWarn>
<uSurpInc>2</uSurpInc>
<useXO>2</useXO>
<ClangAsOpt>0</ClangAsOpt>
<uClangAs>2</uClangAs>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
@ -1480,7 +1479,7 @@ @@ -1480,7 +1479,7 @@
<NoWarn>2</NoWarn>
<uSurpInc>2</uSurpInc>
<useXO>2</useXO>
<ClangAsOpt>0</ClangAsOpt>
<uClangAs>2</uClangAs>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
@ -1610,7 +1609,7 @@ @@ -1610,7 +1609,7 @@
<NoWarn>2</NoWarn>
<uSurpInc>2</uSurpInc>
<useXO>2</useXO>
<ClangAsOpt>0</ClangAsOpt>
<uClangAs>2</uClangAs>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>

93
MDK-ARM/startup_stm32f103xb.lst
Unescape View File

@ -216,9 +216,9 @@ ARM Macro Assembler Page 4 @@ -216,9 +216,9 @@ ARM Macro Assembler Page 4
]
131 00000000 IMPORT __main
132 00000000 IMPORT SystemInit
133 00000000 4809 LDR R0, =SystemInit
133 00000000 4806 LDR R0, =SystemInit
134 00000002 4780 BLX R0
135 00000004 4809 LDR R0, =__main
135 00000004 4806 LDR R0, =__main
136 00000006 4700 BX R0
137 00000008 ENDP
138 00000008
@ -444,43 +444,31 @@ ARM Macro Assembler Page 7 @@ -444,43 +444,31 @@ ARM Macro Assembler Page 7
281 0000001C ;*******************************************************
************************
282 0000001C IF :DEF:__MICROLIB
289 0000001C
290 0000001C IMPORT __use_two_region_memory
291 0000001C EXPORT __user_initial_stackheap
292 0000001C
293 0000001C __user_initial_stackheap
294 0000001C
295 0000001C 4804 LDR R0, = Heap_Mem
296 0000001E 4905 LDR R1, =(Stack_Mem + Stack_Size)
297 00000020 4A05 LDR R2, = (Heap_Mem + Heap_Size)
283 0000001C
284 0000001C EXPORT __initial_sp
285 0000001C EXPORT __heap_base
286 0000001C EXPORT __heap_limit
287 0000001C
288 0000001C ELSE
303 ENDIF
304 0000001C
305 0000001C END
ARM Macro Assembler Page 8
298 00000022 4B06 LDR R3, = Stack_Mem
299 00000024 4770 BX LR
300 00000026
301 00000026 00 00 ALIGN
302 00000028
303 00000028 ENDIF
304 00000028
305 00000028 END
00000000
00000000
00000000
00000400
00000200
00000000
Command Line: --debug --xref --diag_suppress=9931 --cpu=Cortex-M3 --apcs=interw
ork --depend=proximity_uavcan_converter\startup_stm32f103xb.d -oproximity_uavca
n_converter\startup_stm32f103xb.o -I..\Core\Inc -I.\RTE\_proximity_uavcan_conve
rter -IC:\Users\ZRZK\AppData\Local\Arm\Packs\ARM\CMSIS\5.8.0\CMSIS\Core\Include
-IC:\Users\ZRZK\AppData\Local\Arm\Packs\Keil\STM32F1xx_DFP\2.3.0\Device\Includ
e --predefine="__UVISION_VERSION SETA 536" --predefine="_RTE_ SETA 1" --predefi
ne="STM32F10X_MD SETA 1" --predefine="_RTE_ SETA 1" --list=startup_stm32f103xb.
lst startup_stm32f103xb.s
rter -I"D:\Program Files (x86)\Keil_v5\Arm\Packs\ARM\CMSIS\5.6.0\CMSIS\Core\Inc
lude" -I"D:\Program Files (x86)\Keil_v5\Arm\Packs\Keil\STM32F1xx_DFP\2.2.0\Devi
ce\Include" --predefine="__MICROLIB SETA 1" --predefine="__UVISION_VERSION SETA
529" --predefine="_RTE_ SETA 1" --predefine="STM32F10X_MD SETA 1" --list=start
up_stm32f103xb.lst startup_stm32f103xb.s
@ -501,9 +489,8 @@ Symbol: Stack_Mem @@ -501,9 +489,8 @@ Symbol: Stack_Mem
Definitions
At line 36 in file startup_stm32f103xb.s
Uses
At line 296 in file startup_stm32f103xb.s
At line 298 in file startup_stm32f103xb.s
None
Comment: Stack_Mem unused
__initial_sp 00000400
Symbol: __initial_sp
@ -511,7 +498,8 @@ Symbol: __initial_sp @@ -511,7 +498,8 @@ Symbol: __initial_sp
At line 37 in file startup_stm32f103xb.s
Uses
At line 61 in file startup_stm32f103xb.s
Comment: __initial_sp used once
At line 284 in file startup_stm32f103xb.s
3 symbols
@ -533,25 +521,24 @@ Symbol: Heap_Mem @@ -533,25 +521,24 @@ Symbol: Heap_Mem
Definitions
At line 48 in file startup_stm32f103xb.s
Uses
At line 295 in file startup_stm32f103xb.s
At line 297 in file startup_stm32f103xb.s
None
Comment: Heap_Mem unused
__heap_base 00000000
Symbol: __heap_base
Definitions
At line 47 in file startup_stm32f103xb.s
Uses
None
Comment: __heap_base unused
At line 285 in file startup_stm32f103xb.s
Comment: __heap_base used once
__heap_limit 00000200
Symbol: __heap_limit
Definitions
At line 49 in file startup_stm32f103xb.s
Uses
None
Comment: __heap_limit unused
At line 286 in file startup_stm32f103xb.s
Comment: __heap_limit used once
4 symbols
@ -1133,15 +1120,7 @@ Symbol: WWDG_IRQHandler @@ -1133,15 +1120,7 @@ Symbol: WWDG_IRQHandler
At line 79 in file startup_stm32f103xb.s
At line 185 in file startup_stm32f103xb.s
__user_initial_stackheap 0000001C
Symbol: __user_initial_stackheap
Definitions
At line 293 in file startup_stm32f103xb.s
Uses
At line 291 in file startup_stm32f103xb.s
Comment: __user_initial_stackheap used once
56 symbols
55 symbols
@ -1155,8 +1134,7 @@ Symbol: Heap_Size @@ -1155,8 +1134,7 @@ Symbol: Heap_Size
At line 44 in file startup_stm32f103xb.s
Uses
At line 48 in file startup_stm32f103xb.s
At line 297 in file startup_stm32f103xb.s
Comment: Heap_Size used once
Stack_Size 00000400
Symbol: Stack_Size
@ -1164,8 +1142,7 @@ Symbol: Stack_Size @@ -1164,8 +1142,7 @@ Symbol: Stack_Size
At line 33 in file startup_stm32f103xb.s
Uses
At line 36 in file startup_stm32f103xb.s
At line 296 in file startup_stm32f103xb.s
Comment: Stack_Size used once
__Vectors_Size 000000EC
Symbol: __Vectors_Size
@ -1197,13 +1174,5 @@ Symbol: __main @@ -1197,13 +1174,5 @@ Symbol: __main
Uses
At line 135 in file startup_stm32f103xb.s
Comment: __main used once
__use_two_region_memory 00000000
Symbol: __use_two_region_memory
Definitions
At line 290 in file startup_stm32f103xb.s
Uses
None
Comment: __use_two_region_memory unused
3 symbols
409 symbols in table
2 symbols
406 symbols in table

Write Preview
Loading…
Cancel
Save