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

增加串口读MR72 8目标雷达数据

mission-4.1.18
zbr 3 years ago
parent
714f9f4d48
commit
e27ed18049
5 changed files with 282918 additions and 6 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. 4
      libraries/AP_GPS/AP_GPS.cpp
  2. 162
      libraries/AP_Proximity/AP_Proximity_NanaRadar_MR72.cpp
  3. 22
      libraries/AP_Proximity/AP_Proximity_NanaRadar_MR72.h
  4. 39
      tag_list.txt
  5. 282697
      tance() const {return _trigg_dist;}

4
libraries/AP_GPS/AP_GPS.cpp
Unescape View File

@ -60,8 +60,8 @@ @@ -60,8 +60,8 @@
extern const AP_HAL::HAL &hal;
// baudrates to try to detect GPSes with
// const uint32_t AP_GPS::_baudrates[] = {9600U, 115200U, 4800U, 19200U, 38400U, 57600U, 230400U};
const uint32_t AP_GPS::_baudrates[] = { 115200U, 38400U, 57600U, 230400U};
const uint32_t AP_GPS::_baudrates[] = {9600U, 115200U, 4800U, 19200U, 38400U, 57600U, 230400U};
// const uint32_t AP_GPS::_baudrates[] = { 115200U, 38400U, 57600U, 230400U};
// initialisation blobs to send to the GPS to try to get it into the
// right mode

162
libraries/AP_Proximity/AP_Proximity_NanaRadar_MR72.cpp
Unescape View File

@ -58,7 +58,7 @@ void AP_Proximity_NanaRadar_MR72::update(void) @@ -58,7 +58,7 @@ void AP_Proximity_NanaRadar_MR72::update(void)
// process incoming messages
read_sensor_data();
data_process();
// check for timeout and set health status
if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_TRTOWER_TIMEOUT_MS)) {
set_status(AP_Proximity::Status::NoData);
@ -89,7 +89,12 @@ bool AP_Proximity_NanaRadar_MR72::read_sensor_data() @@ -89,7 +89,12 @@ bool AP_Proximity_NanaRadar_MR72::read_sensor_data()
while (nbytes-- > 0) {
// uint8_t c = uart->read();
// gcs().send_text(MAV_SEVERITY_INFO, "getc 0x%02x",c);
if (uart->read() == Head1)
if(decode(uart->read())){
// gcs().send_text(MAV_SEVERITY_INFO, " ");
;
}
// if (uart->read() == Head1)
if (0)
{ //判断数据包帧头0x54
buffer[0] = Head1;
// gcs().send_text(MAV_SEVERITY_INFO, "getc 0x%02x",buffer[0]);
@ -123,14 +128,165 @@ bool AP_Proximity_NanaRadar_MR72::read_sensor_data() @@ -123,14 +128,165 @@ bool AP_Proximity_NanaRadar_MR72::read_sensor_data()
}
return (message_count > 0);
}
bool AP_Proximity_NanaRadar_MR72::decode(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;
// gcs().send_text(MAV_SEVERITY_INFO, "ccc:%02x ",c);
switch (decode_state)
{
case 0:
if(temp == 0xAA)
decode_state = 1;
break;
case 1:
if(temp == 0xAA){
decode_state = 2;
// gcs().send_text(MAV_SEVERITY_INFO, "%d:%02x ",decode_state,temp);
}
else
decode_state = 0;
break;
case 2:
if(temp == 0x0c){
decode_state = 3;
// gcs().send_text(MAV_SEVERITY_INFO, "%d:%02x ",decode_state,temp);
}
else
decode_state = 0;
break;
case 3:
if(temp == 0x07){
decode_state = 4;
data_len = 8;
// gcs().send_text(MAV_SEVERITY_INFO, "%d:%02x ",decode_state,temp);
}
else
decode_state = 0;
break;
case 4:
data_cnt = 8 - data_len;
// gcs().send_text(MAV_SEVERITY_INFO, "%d: c[%d] = %02x, ",decode_state,data_cnt,temp);
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;
// gcs().send_text(MAV_SEVERITY_INFO, "%d:%02x ",decode_state,temp);
}
else
decode_state = 0;
break;
case 6:
if(temp == 0x55){
mr72_c_data.time_ms = AP_HAL::millis();
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(fabs(last_range - mr72_c_data.range) > 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;
// gcs().send_text(MAV_SEVERITY_INFO, "t:%d, i:%d, r:%.2f ",mr72_c_data.time_ms,mr72_c_data.index,mr72_c_data.range_f);
}
}
}else{
;
// gcs().send_text(MAV_SEVERITY_INFO, "-- err:%d:%02x ",decode_state,temp);
}
decode_state = 0;
break;
default:
break;
}
return ret;
}
void AP_Proximity_NanaRadar_MR72::data_process(void){
if(mr72_c_data.time_ms - last_time_ms > 20 && AP_HAL::millis() - last_time_ms > 100){
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);
update_sector_data(315, range_arr[0]); // d8
update_sector_data(0, range_arr[1]); // d1
update_sector_data(45, range_arr[2]); // d8
update_sector_data(90, range_arr[3]); // d7
update_sector_data(135, range_arr[4]); // d6
update_sector_data(180, range_arr[5]); // d5
update_sector_data(225, range_arr[6]); // d4
update_sector_data(270, range_arr[7]); // d3
// update_sector_data(315, UINT16_VALUE(buffer[16], buffer[17])); // d2
// gcs().send_text(MAV_SEVERITY_INFO, "b:%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]);
// gcs().send_text(MAV_SEVERITY_INFO, "a:%.2f,b:%.2f,c:%.2f",range_arr[1]/100.0,range_arr[1]/100.0,range_arr[2]/100.0);
// memset(range_arr,4000,8);
for (uint8_t i = 0; i < 8; i++)
{
range_arr[i] = 4000;
index_arr[i] = 0;
}
}
range_index = 0;
}
}
void AP_Proximity_NanaRadar_MR72::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;
}
}
}
}
// process reply
void AP_Proximity_NanaRadar_MR72::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
{
uint8_t sector;
if (convert_angle_to_sector(angle_deg, sector)) {
_angle[sector] = angle_deg;
_distance[sector] = ((float) distance_cm) / 100;
_distance[sector] = ((float) distance_cm) / 100.0;
_distance_valid[sector] = distance_cm != 0xffff;
_last_distance_received_ms = AP_HAL::millis();
// update boundary used for avoidance

22
libraries/AP_Proximity/AP_Proximity_NanaRadar_MR72.h
Unescape View File

@ -27,7 +27,9 @@ private: @@ -27,7 +27,9 @@ private:
// check and process replies from sensor
bool read_sensor_data();
void update_sector_data(int16_t angle_deg, uint16_t distance_cm);
bool decode(int16_t c);
void data_process(void);
void sortA1(uint16_t a[], uint8_t length);
// reply related variables
AP_HAL::UARTDriver *uart = nullptr;
uint8_t buffer[20]; // buffer where to store data from serial
@ -39,4 +41,22 @@ private: @@ -39,4 +41,22 @@ private:
// request related variables
uint32_t _last_distance_received_ms; // system time of last distance measurement received from sensor
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];
uint16_t last_range;
uint32_t last_time_ms;
uint8_t range_index;
uint8_t index_arr[8];
// struct mr72_c mr72_c_data;
};

39
tag_list.txt
Unescape View File

@ -0,0 +1,39 @@ @@ -0,0 +1,39 @@
-2.27-Alpha
-2.28
-2.30
-2.33
-2.40
-2.40-beta
-2.50
-2.60
-2.61
-2.62
-2.63
-2.64
-2.65
-2.66
-2.68
-2.69
-2.70
-2.72
-2.74
-2.74b
-2.75
-2.78
-2.78b
-3.0.0
-3.0.1
-3.0.2
-3.0.3
-3.1.0
-3.2.0
-3.3.0
-3.5.1
-3.5.2
-3.6.0
-3.7.0
-3.7.0-beta1
-3.8.0
-3.8.1
-3.8.2

282697
tance() const {return _trigg_dist;}
View File

File diff suppressed because it is too large Load Diff
Write Preview
Loading…
Cancel
Save