|
|
|
@ -155,44 +155,46 @@ void task_proximity(void)
@@ -155,44 +155,46 @@ void task_proximity(void)
|
|
|
|
|
{ |
|
|
|
|
os_p1 = osMessageQueueGet(proximity1_queueHandle, &proximity1, NULL, 120); |
|
|
|
|
os_p2 = osMessageQueueGet(proximity2_queueHandle, &proximity2, NULL, 120); |
|
|
|
|
if((os_p1 == osOK) || (os_p2 == osOK)){ |
|
|
|
|
uavcan_proximity_t data = {0}; |
|
|
|
|
data.d0 = proximity1.d0; |
|
|
|
|
data.d45 = proximity1.d45; |
|
|
|
|
data.d90 = proximity1.d90; |
|
|
|
|
data.d135 = proximity1.d135; |
|
|
|
|
data.d180 = proximity1.d180; |
|
|
|
|
data.d225 = proximity1.d225; |
|
|
|
|
data.d270 = proximity1.d270; |
|
|
|
|
data.d315 = proximity1.d315; |
|
|
|
|
send_uavcan_proximity(&data); |
|
|
|
|
} |
|
|
|
|
// if((os_p1 == osOK) || (os_p2 == osOK)){
|
|
|
|
|
// uavcan_proximity_t data = {0};
|
|
|
|
|
// data.d0 = proximity1.d0;
|
|
|
|
|
// data.d45 = proximity1.d45;
|
|
|
|
|
// data.d90 = proximity1.d90;
|
|
|
|
|
// data.d135 = proximity1.d135;
|
|
|
|
|
// data.d180 = proximity1.d180;
|
|
|
|
|
// data.d225 = proximity1.d225;
|
|
|
|
|
// data.d270 = proximity1.d270;
|
|
|
|
|
// data.d315 = proximity1.d315;
|
|
|
|
|
// send_uavcan_proximity(&data);
|
|
|
|
|
// }
|
|
|
|
|
// if ((os_p1 == osOK) && (os_p2 == osOK))
|
|
|
|
|
// {
|
|
|
|
|
// uavcan_proximity_t data = {0};
|
|
|
|
|
// data.d0 = min(proximity1.d0, proximity2.d0);
|
|
|
|
|
// data.d315 = proximity1.d315;
|
|
|
|
|
// data.d90 = proximity1.d90;
|
|
|
|
|
// data.d45 = proximity2.d45;
|
|
|
|
|
// send_uavcan_proximity(&data);
|
|
|
|
|
// }
|
|
|
|
|
// else
|
|
|
|
|
// {
|
|
|
|
|
// if (os_p1 == osOK) //左边的雷达是反的
|
|
|
|
|
// {
|
|
|
|
|
// uavcan_proximity_t data = {0};
|
|
|
|
|
// data.d0 = proximity1.d0;
|
|
|
|
|
// data.d315 = proximity1.d315;
|
|
|
|
|
// data.d45 = proximity1.d45;
|
|
|
|
|
// send_uavcan_proximity(&data);
|
|
|
|
|
// }
|
|
|
|
|
// else if (os_p2 == osOK)
|
|
|
|
|
// {
|
|
|
|
|
// uavcan_proximity_t data = {0};
|
|
|
|
|
// data.d0 = proximity2.d0;
|
|
|
|
|
// data.d315 = proximity2.d315;
|
|
|
|
|
// data.d45 = proximity2.d45;
|
|
|
|
|
// send_uavcan_proximity(&data);
|
|
|
|
|
// }
|
|
|
|
|
uavcan_proximity_t data = {0}; |
|
|
|
|
data.d0 = min(proximity1.d0, proximity2.d0); |
|
|
|
|
if (os_p1 == osOK) //左边的雷达是反的
|
|
|
|
|
{ |
|
|
|
|
// uavcan_proximity_t data = {0};
|
|
|
|
|
data.d0 = proximity1.d0; |
|
|
|
|
// data.d45 = proximity1.d90;
|
|
|
|
|
data.d90 = proximity1.d45; |
|
|
|
|
send_uavcan_proximity(&data); |
|
|
|
|
} |
|
|
|
|
else if (os_p2 == osOK) |
|
|
|
|
{ |
|
|
|
|
// uavcan_proximity_t data = {0};
|
|
|
|
|
data.d0 = proximity2.d0; |
|
|
|
|
data.d45 = proximity2.d45; |
|
|
|
|
// data.d90 = proximity2.d90;
|
|
|
|
|
send_uavcan_proximity(&data); |
|
|
|
|
} |
|
|
|
|
// }
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|