yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

纳雷雷达驱动完成.低空2m以下不使用雷达数据.雷达数据的滤波写了但有问题,需要修改,见src/drivers/distance_sensor/nanoradar_mr72/nanoradar_mr72.cpp

Browse Source 
修改一点数据错误
This commit is contained in:
wangpeng
2021-12-27 09:10:47 +08:00
parent b68243380e
commit bc3eae7a64
14 changed files with 488 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
boards/px4/fmu-v5/init/rc.board_sensors
View File

@@ -32,4 +32,5 @@ ist8310 -I -R 10 start
# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
ist8310 -X -b 1 -R 10 start
upatch_radar start -d /dev/ttyS2
# upatch_radar start -d /dev/ttyS2
nanoradar_mr72 start -d /dev/ttyS2

2
msg/collision_constraints.msg
View File

@@ -5,3 +5,5 @@ uint64 timestamp # time since system start (microseconds)
float32[2] original_setpoint # velocities demanded
float32[2] adapted_setpoint # velocities allowed
bool interfering
bool interfering_fail

1
src/drivers/distance_sensor/CMakeLists.txt
View File

@@ -49,3 +49,4 @@ add_subdirectory(upatch_radar)
add_subdirectory(vl53l0x)
add_subdirectory(vl53l1x)
add_subdirectory(gy_us42)
add_subdirectory(nanoradar_mr72)

408
src/drivers/distance_sensor/nanoradar_mr72/nanoradar_mr72.cpp
View File

@@ -34,16 +34,24 @@
#include "nanoradar_mr72.hpp"
#include <lib/drivers/device/Device.hpp>
#include <matrix/matrix/math.hpp>
nanoradar_mr72::nanoradar_mr72(const char *port, uint8_t rotation) :
nanoradar_mr72::nanoradar_mr72(const char *port, uint16_t rotation) :
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
_px4_rangefinder(0, rotation)
_px4_rangefinder{
PX4Rangefinder(0, rotation),
PX4Rangefinder(1, rotation),
PX4Rangefinder(2, rotation)
}
{
/* store port name */
strncpy(_port, port, sizeof(_port) - 1);
/* enforce null termination */
_port[sizeof(_port) - 1] = '\0';
_output_mode = UART_SECTION;
device::Device::DeviceId device_id;
device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL;
@@ -53,17 +61,53 @@ nanoradar_mr72::nanoradar_mr72(const char *port, uint8_t rotation) :
if (bus_num < 10) {
device_id.devid_s.bus = bus_num;
}
_px4_rangefinder.set_device_id(device_id.devid);
_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_MR72);
_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR);
_px4_rangefinder.set_orientation(distance_sensor_s::ROTATION_FORWARD_FACING);
_px4_rangefinder.set_hfov(math::radians(112.f));
_px4_rangefinder.set_vfov(math::radians(14.f));
for(int i=0;i<3;i++){
_px4_rangefinder[i].set_device_id(device_id.devid);
_px4_rangefinder[i].set_device_type(DRV_DIST_DEVTYPE_MR72);
_px4_rangefinder[i].set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR);
_px4_rangefinder[i].set_vfov(math::radians(14.f));
_px4_rangefinder.set_min_distance(MR72_MIN_DISTANCE);
_px4_rangefinder.set_max_distance(MR72_MAX_DISTANCE);
_px4_rangefinder[i].set_min_distance(MR72_MIN_DISTANCE);
_px4_rangefinder[i].set_max_distance(MR72_MAX_DISTANCE);
}
_px4_rangefinder[0].set_hfov(math::radians(37.5f));
_px4_rangefinder[1].set_hfov(math::radians(45.f));
_px4_rangefinder[2].set_hfov(math::radians(37.5f));
// for 3扇区模式，37.5/45/37.3度，225+375/2···41度
// Quatf(Eulerf(0, -M_PI_2_F, 0))
float left_deg[4] = {}; matrix::Quatf middle_quat ;
float middle_deg[4] = {}; matrix::Quatf left_quat ;
float right_deg[4] = {}; matrix::Quatf right_quat ;
if(rotation<360){
uint16_t rotation_deg = rotation * 45;
left_quat = matrix::Quatf(matrix::Eulerf(0, 0, (float)rotation_deg - math::radians(41.0f) ));
right_quat = matrix::Quatf(matrix::Eulerf(0, 0, (float)rotation_deg + math::radians(41.0f) ));
left_deg[0] = left_quat(0); right_deg[0] = right_quat(0);
left_deg[1] = left_quat(1); right_deg[1] = right_quat(1);
left_deg[2] = left_quat(2); right_deg[2] = right_quat(2);
left_deg[3] = left_quat(3); right_deg[3] = right_quat(3);
// rotation - 41 deg
_px4_rangefinder[0].set_orientation(left_deg);//left
// rotation + 41 deg
_px4_rangefinder[2].set_orientation(right_deg);//right
}else{
middle_quat = matrix::Quatf(matrix::Eulerf(0, 0, math::radians( (float)rotation - 360.0f) )) ;
left_quat = matrix::Quatf(matrix::Eulerf(0, 0, math::radians( (float)rotation - 360.0f - 41.0f) ));
right_quat = matrix::Quatf(matrix::Eulerf(0, 0, math::radians( (float)rotation - 360.0f + 41.0f) ));
left_deg[0] = left_quat(0); middle_deg[0] = middle_quat(0); right_deg[0] = right_quat(0);
left_deg[1] = left_quat(1); middle_deg[1] = middle_quat(1); right_deg[1] = right_quat(1);
left_deg[2] = left_quat(2); middle_deg[2] = middle_quat(2); right_deg[2] = right_quat(2);
left_deg[3] = left_quat(3); middle_deg[3] = middle_quat(3); right_deg[3] = right_quat(3);
_px4_rangefinder[0].set_orientation(left_deg);//left
_px4_rangefinder[1].set_orientation(middle_deg);//middle
_px4_rangefinder[2].set_orientation(right_deg);//right
}
}
nanoradar_mr72::~nanoradar_mr72()
@@ -81,6 +125,41 @@ int nanoradar_mr72::init()
return PX4_OK;
}
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,0x48, 0x4f, 0x46, 0x41, 0x54, 0x53,
0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3,
0xca, 0xcd, 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3,
0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4,
0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94,
0x9d, 0x9a, 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16,0x03, 0x04,
0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42,0x6f, 0x68, 0x61, 0x66, 0x73,
0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80,0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8,
0xad, 0xaa, 0xa3, 0xa4, 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8,
0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f,
0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10,0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26,
0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76,
0x71, 0x78, 0x7f,0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b,
0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc,
0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5,
0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3
};
/*
* crc8 from trone driver by Luis Rodrigues
* crc8 = crc_crc8(buffer, 18)；//buffer 为数据接收缓存数组
*/
uint8_t crc_crc8(const uint8_t *p, uint8_t len)
{
uint16_t i;
uint16_t crc = 0x0;
while (len--) {
i = (crc ^ *p++) & 0xFF;
crc = (crc8_table[i] ^ (crc << 8)) & 0xFF;
}
return crc & 0xFF;
}
int nanoradar_mr72::collect()
{
perf_begin(_sample_perf);
@@ -89,7 +168,7 @@ int nanoradar_mr72::collect()
// int distance_cm = -1;
int index = 0;
float distance_m = -1.0f;
float distance_m[3] = {-1.0f};
bool checksum_passed = false;
@@ -97,8 +176,8 @@ int nanoradar_mr72::collect()
const hrt_abstime timestamp_sample = hrt_absolute_time();
int bytes_read = ::read(_file_descriptor, &_buffer[0], sizeof(_buffer));
float distances_m[5] = {0.0f}; //测量得到的5个距离
float snr[5] = {0.0f}; //测量得到的5个距离的信噪比
// float distances_m[5] = {0.0f}; //测量得到的5个距离
// float snr[5] = {0.0f}; //测量得到的5个距离的信噪比
uint8_t number_of_detection = 0; // 目标输出状态1，探测到目标的个数
uint8_t roller_count = 0; // 目标输出状态2，循环技术0-1-2-3，每个周期改变一次
struct radar_data{
@@ -120,7 +199,7 @@ int nanoradar_mr72::collect()
uint8_t Rsvdl; // --
uint8_t RollCount; // 计数位
uint8_t rcs; // 目前固定为0
}mr72_data_cal; //目标输出信息真实值
}mr72_data_cal[8]; //目标输出信息真实值
struct radar_data_2{
uint8_t index;
uint8_t AzimuthH;
@@ -132,120 +211,257 @@ int nanoradar_mr72::collect()
uint8_t RollCount:2;
uint8_t VreL;
uint8_t Rcs;
}mr72_data_2;
}; // no use
union radar_union{
uint8_t[8] data;
struct radar_data_2;
uint8_t data[8];
struct radar_data_2 mr72_data_2;
};
}
struct mr72_data_cal mr72_data_cal[8];
uint8_t counter =0;
enum Receive_State{
Receive_idle = 0;
Receive_sys_state = 1;
Receiving_targets = 2;
Receive_end = 3;
}
Receive_idle = 0,
Receive_sys_state = 1,
Receiving_targets = 2,
Receive_end = 3
};
uint8_t receive_state = Receive_State::Receive_idle;
uint16_t distance_left_middle_right[3] = {0,0,0}; //cm
if (bytes_read > 0) {
index = bytes_read - 14; // 14bytes
while (index >= 0 && !checksum_passed) {
if (_buffer[index] == MR72_PACKET_HDR && _buffer[index+1] == MR72_PACKET_HDR) { // 0XAA 0XAA 开头
bytes_processed = index;
switch(_output_mode){
case UART_POINTS:
while (bytes_processed < bytes_read && !checksum_passed) {
{
if( _buffer[index+12] == _buffer[index+13] && _buffer[index+12] == MR72_PACKET_END ){ //与第18个数相等 85
index = bytes_read - 14; // 14bytes
checksum_passed = true;
while (index >= 0 && !checksum_passed) {
if (_buffer[index] == MR72_PACKET_HDR && _buffer[index+1] == MR72_PACKET_HDR) { // 0XAA 0XAA 开头
bytes_processed = index;
while (bytes_processed < bytes_read && !checksum_passed) {
{
if( _buffer[index+12] == _buffer[index+13] && _buffer[index+12] == MR72_PACKET_END ){ //与第18个数相等 85
checksum_passed = true;
uint16_t message_id = (uint16_t)_buffer[index+2] + ( (uint16_t)_buffer[index+3] << 8);
switch(message_id){
case 0x200 :// MR72配置
break;
case 0x201 :// MR72返回值
break; //以上两个与CAN协议的相同
case 0x60A :// MR72系统状态
break;
case 0x70B :// 目标输出状态
number_of_detection = _buffer[index+4];
roller_count = _buffer[index+5] >> 6; // 8、9位
roller_count = roller_count;
for(int i=0;i<7;i++){ // reset valid flag
mr72_data_cal[i].valid = false;
}
if(mr72_data_cal[0].valid){}// do nothing
counter=0;
receive_state = Receive_State::Receive_sys_state;
break;
case 0x70C :// 目标输出信息
mr72_data.index = _buffer[index+4];
mr72_data.azimuth = ( (uint16_t)_buffer[index+5] << 8 ) + _buffer[index+8];
mr72_data.range = ( (uint16_t)_buffer[index+6] << 8 ) + _buffer[index+7];
mr72_data.speed = ( ((_buffer[index+9] & 0b11100000)>>5) << 8 ) + _buffer[index+10];
mr72_data.Rsvdl = ( (_buffer[index+9] & 0b00011100)>>2 );
mr72_data.RollCount = ( _buffer[index+9] & 0b00000011);
mr72_data.rcs = _buffer[index+11];
mr72_data_cal[counter].index = mr72_data.index;
mr72_data_cal[counter].RollCount = mr72_data.RollCount;
mr72_data_cal[counter].range_m = mr72_data.range * 0.01f;
mr72_data_cal[counter].speed_ms = mr72_data.speed * 0.05f - 35.0f;
mr72_data_cal[counter].azimuth_deg = mr72_data.azimuth * 0.01f - 90.0f;
mr72_data_cal[counter].rcs = 0;
mr72_data_cal[counter].valid = true;
if(counter<number_of_detection-1) {
receive_state = Receive_State::Receiving_targets;
}else if(counter == number_of_detection-1) {
receive_state = Receive_State::Receive_end;
}
counter++;
receive_state = receive_state;
break;
default: break;
uint16_t message_id = (uint16_t)_buffer[index+2] + (uint16_t)_buffer[index+3] << 8;
switch(message_id){
case 0x200 :// MR72配置
break;
case 0x201 :// MR72返回值
break; //以上两个与CAN协议的相同
case 0x60A :// MR72系统状态
break;
case 0x70B :// 目标输出状态
number_of_detection = _buffer[index+4];
roller_count = _buffer[index+5] >> 6; // 8、9位
for(int i=0;i<7;i++){ // reset valid flag
mr72_data_cal[i].valid = false;
}
counter=0;
receive_state = Receive_State::Receive_sys_state;
break;
case 0x70C :// 目标输出信息
mr72_data.index = _buffer[index+4];
mr72_data.azimuth = (uint16_t)_buffer[index+5] << 8 + _buffer[index+8];
mr72_data.range = (uint16_t)_buffer[index+6] << 8 + _buffer[index+7];
mr72_data.speed = ((_buffer[index+9]&&0b11100000)>>5)<<8 + _buffer[index+10];
mr72_data.Rsvdl = ((_buffer[index+9]&&0b00011100)>>2);
mr72_data.RollCount = (_buffer[index+9]&&0b00000011);
mr72_data.rcs = _buffer[index+11];
mr72_data_cal[counter].index = mr72_data.index;
mr72_data_cal[counter].RollCount = mr72_data.RollCount;
mr72_data_cal[counter].range_m = mr72_data.range * 0.01f;
mr72_data_cal[counter].speed_ms = mr72_data.speed * 0.05f - 35.0f;
mr72_data_cal[counter].azimuth_deg = mr72_data.azimuth * 0.01f - 90.0f;
mr72_data_cal[counter].rcs = 0;
mr72_data_cal[counter].valid = true;
if(counter<number_of_detection-1) {
receive_state = Receive_State::Receive_targets;
}else if(counter == number_of_detection-1) {
receive_state = Receive_State::Receive_end;
}
counter++;
break;
}
}
bytes_processed++;
}
}
bytes_processed++;
index--;
}
}
index--;
break;
default:
case UART_SECTION:
}
}
// 19bytes
index = bytes_read - 19; // 19bytes
uint8_t buffer_copy[19] = {};
while (index >= 0 && !checksum_passed) {
if (_buffer[index] == MR72_PACKET_UART_SEC_HDR && _buffer[index+1] == MR72_PACKET_UART_SEC_2HDR) { // 0X54 0X48 开头
bytes_processed = index;
memcpy(buffer_copy, _buffer+index,19);
uint8_t crc_value = crc_crc8(buffer_copy, 18);
while (bytes_processed < bytes_read && !checksum_passed) {
{
if( crc_value == _buffer[index+18] ){ //
checksum_passed = true;
if(_buffer[index+2] == 0xff && _buffer[index+3] == 0xff ){
distance_left_middle_right[1] = MR72_MAX_DISTANCE * 100 + 1;//cm
}else{ //2 3
distance_left_middle_right[1] = ( (uint16_t)_buffer[index+2] << 8 ) + _buffer[index+3];
}
if(_buffer[index+4] == 0xff && _buffer[index+5] == 0xff ){
distance_left_middle_right[2] = MR72_MAX_DISTANCE * 100 + 1;//cm
}else{ // 4 5
distance_left_middle_right[2] = ( (uint16_t)_buffer[index+4] << 8 ) + _buffer[index+5];
}
if(_buffer[index+16] == 0xff && _buffer[index+17] == 0xff ){
distance_left_middle_right[0] = MR72_MAX_DISTANCE * 100 + 1;//cm
}else{ // 16 17
distance_left_middle_right[0] = ( (uint16_t)_buffer[index+16] << 8 ) + _buffer[index+17];
}
printf("buffer %X %X, %X %X, %X %X \n",_buffer[index+2], _buffer[index+3],
_buffer[index+4], _buffer[index+5],
_buffer[index+16], _buffer[index+17] );
// 如果有两个纳雷雷达就建议使用distance_sensor的发布方式
obstacle_distance_s ob_distance{};
ob_distance.timestamp = hrt_absolute_time();
ob_distance.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD;
ob_distance.increment = 0;
ob_distance.min_distance = 0;
ob_distance.max_distance = 0;
ob_distance.angle_offset = 0;
ob_distance.distances[0] = distance_left_middle_right[0]; //72ge todo
ob_distance.distances[1] = distance_left_middle_right[1]; //72ge todo
ob_distance.distances[2] = distance_left_middle_right[2]; //72ge todo
ob_distance = ob_distance; // incase 报错
_obstacle_distance_topic.publish(ob_distance);
}
}
bytes_processed++;
}
}
index--;
}
break;
// default:
// break;
}// end of _output_mode
}// bytes read > 0
if (!checksum_passed) {
return -EAGAIN;
}
float min = distances_m[0];
int8_t min_index = 1;
int8_t empty_index = -1;
for(int i = 0; i < 5; i++){
if(distances_m[i] > 0.01f){
if(min > distances_m[i]){
min = distances_m[i];
min_index = i;
}
empty_index = 1; //排除5个数都是0
for(int i=0;i<3;i++){
if(distance_left_middle_right[i]< MR72_MIN_DISTANCE * 100)
distance_left_middle_right[i] = 0;
else if (distance_left_middle_right[i] > MR72_MAX_DISTANCE *100){
distance_left_middle_right[i] = MR72_MAX_DISTANCE * 100 + 1;
}
distance_m[i] = distance_left_middle_right[i] / 100.0f;
}
distance_m = min;
if (empty_index == -1){
distance_m = MR72_MAX_DISTANCE + 2.0f;// 以上5个数值都是0，即没有障碍物，则直接给最大距离即可。
}
// TODO
// 给个滤波？ 在位置较低的时候不使用此数据？
// 滤波，野值剔除与低通滤波
float param_cutoff_freq = 0.0f;
_param_handle_freq = param_find("NANO_MR_FILT");
param_get(_param_handle_freq, &param_cutoff_freq);
// 这里给的信噪比原始值，与其规定的有所区别
_px4_rangefinder.update(timestamp_sample, distance_m, min_index==-1 ? 98 : ((int8_t)snr[min_index]) );
static matrix::Vector3f output_distance_m ( distance_m[0],distance_m[1],distance_m[2] );
// 记录剔除野值后待滤波的值
static matrix::Vector3f cur_tobefiltered_distance_m ( distance_m[0],distance_m[1],distance_m[2] );
static matrix::Vector3f pre_tobefiltered_distance_m ( distance_m[0],distance_m[1],distance_m[2] );
// 记录原始的三次值
static matrix::Vector3f cur_distance_m ( distance_m[0],distance_m[1],distance_m[2] );
static matrix::Vector3f pre_distance_m ( distance_m[0],distance_m[1],distance_m[2] );
static matrix::Vector3f pre_pre_distance_m ( distance_m[0],distance_m[1],distance_m[2] );
cur_distance_m(0) = distance_m[0];
cur_distance_m(1) = distance_m[1];
cur_distance_m(2) = distance_m[2];
// 放弃差别较大的数字，野值，阈值
if( fabsf(distance_m[0] - output_distance_m(0) ) > 10.0f ) cur_tobefiltered_distance_m(0) = pre_tobefiltered_distance_m(0);
if( fabsf(distance_m[1] - output_distance_m(1) ) > 10.0f ) cur_tobefiltered_distance_m(1) = pre_tobefiltered_distance_m(1);
if( fabsf(distance_m[2] - output_distance_m(2) ) > 10.0f ) cur_tobefiltered_distance_m(2) = pre_tobefiltered_distance_m(2);
pre_distance_m = cur_distance_m;
pre_pre_distance_m = pre_distance_m;
//但如果连续3次都出现差别较小的大数，就使用它 // 10 10 25 24 23（启用） 22 20
if( fabsf(cur_distance_m(0) - pre_distance_m(0) ) < 5.0f && fabsf(pre_pre_distance_m(0) - pre_distance_m(0) ) < 5.0f ) cur_tobefiltered_distance_m(0) = cur_distance_m(0);
if( fabsf(cur_distance_m(1) - pre_distance_m(1) ) < 5.0f && fabsf(pre_pre_distance_m(1) - pre_distance_m(1) ) < 5.0f ) cur_tobefiltered_distance_m(0) = cur_distance_m(0);
if( fabsf(cur_distance_m(2) - pre_distance_m(2) ) < 5.0f && fabsf(pre_pre_distance_m(2) - pre_distance_m(2) ) < 5.0f ) cur_tobefiltered_distance_m(0) = cur_distance_m(0);
if ( PX4_ISFINITE(param_cutoff_freq) || ( (fabsf(_lp_filter.get_cutoff_freq() - param_cutoff_freq) > 0.1f) && param_cutoff_freq > 1e-6f ) ) {
_lp_filter.set_cutoff_frequency(_filter_sample_rate, param_cutoff_freq);
_lp_filter.reset(cur_distance_m);
}
if( param_cutoff_freq > 1e-6f ){
output_distance_m = _lp_filter.apply(cur_tobefiltered_distance_m);
}
pre_tobefiltered_distance_m = cur_tobefiltered_distance_m;
//
// _px4_rangefinder[0].update(timestamp_sample, output_distance_m(0), 100 );
// _px4_rangefinder[1].update(timestamp_sample, output_distance_m(1), 100 );
// _px4_rangefinder[2].update(timestamp_sample, output_distance_m(2), 100 );
_px4_rangefinder[0].update(timestamp_sample, distance_m[0], 100 );
_px4_rangefinder[1].update(timestamp_sample, distance_m[1], 100 );
_px4_rangefinder[2].update(timestamp_sample, distance_m[2], 100 );
perf_end(_sample_perf);

35
src/drivers/distance_sensor/nanoradar_mr72/nanoradar_mr72.hpp
View File

@@ -53,6 +53,12 @@
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/perf/perf_counter.h>
#include <uORB/topics/obstacle_distance.h>
#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
#include <px4_platform_common/module_params.h>
using namespace time_literals;
#define MR72_MEASURE_INTERVAL 10_ms
@@ -60,19 +66,20 @@ using namespace time_literals;
#define MR72_MIN_DISTANCE 1.0f
#define MR72_VERSION 1
#define MR72_CAN 0
#define MR72_UART_SECTION 0
#define MR72_UART_POINTS 1
// #if MR72_VERSION == 1
#define MR72_PACKET_HDR 170
#define MR72_PACKET_END 85
#define MR72_BUFFER_LENGTH 18
#define MR72_BUFFER_LENGTH 19 // longest
// #else
// #define MR72_PACKET_HDR 72
// #define MR72_BUFFER_LENGTH 9
// #endif
#define MR72_PACKET_UART_SEC_HDR 84
#define MR72_PACKET_UART_SEC_2HDR 72
/**
* Assume standard deviation to be equal to sensor resolution.
* Static bench tests have shown that the sensor ouput does
@@ -88,7 +95,7 @@ public:
* @param port The serial port to open for communicating with the sensor.
* @param rotation The sensor rotation relative to the vehicle body.
*/
nanoradar_mr72(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
nanoradar_mr72(const char *port, uint16_t rotation = 0);
~nanoradar_mr72() override;
int init();
@@ -111,7 +118,9 @@ private:
void start();
void stop();
PX4Rangefinder _px4_rangefinder;
PX4Rangefinder _px4_rangefinder[3];
uORB::Publication<obstacle_distance_s> _obstacle_distance_topic{ORB_ID(obstacle_distance)};
char _port[20] {};
@@ -119,7 +128,21 @@ private:
uint8_t _buffer[MR72_BUFFER_LENGTH] {};
float _filter_sample_rate{33.3};
math::LowPassFilter2p<matrix::Vector3f> _lp_filter{};
enum output_mode{
CAN_MODE,
UART_POINTS,
UART_SECTION
};
uint8_t _output_mode;
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")};
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
param_t _param_handle_freq;
};

32
src/drivers/distance_sensor/nanoradar_mr72/nanoradar_mr72_main.cpp
View File

@@ -36,12 +36,12 @@
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
namespace nanoradar_mr72
namespace mr72_radar
{
nanoradar_mr72 *g_dev{nullptr};
static int start(const char *port, uint8_t rotation)
static int start(const char *port, uint16_t rotation)
{
if (g_dev != nullptr) {
PX4_WARN("already started");
@@ -115,24 +115,30 @@ $ nanoradar_mr72 stop
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "Serial device", false);
PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true);
PRINT_MODULE_USAGE_PARAM_INT('r', 0, 0, 720, "Sensor rotation degree - forward facing by default", true);
PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
return PX4_OK;
}
} // namespace nanoradar_mr72
} // namespace mr72_radar
extern "C" __EXPORT int nanoradar_mr72_main(int argc, char *argv[])
{
uint8_t rotation = distance_sensor_s::ROTATION_FORWARD_FACING;
uint16_t rotation = distance_sensor_s::ROTATION_FORWARD_FACING;
const char *device_path = nullptr;
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) {
// while ((ch = px4_getopt(argc, argv, "R:d:r:", &myoptind, &myoptarg)) != EOF) {
while ((ch = px4_getopt(argc, argv, "d:r:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (uint8_t)atoi(myoptarg);
// case 'R':
// rotation = (uint8_t)atoi(myoptarg);
// break;
case 'r':
rotation = (uint16_t)atoi(myoptarg);// 0-360,按照原来的方式，25朝下等等。360-720，按照减去360的角度计算。0度朝前，角度顺时针增加
break;
case 'd':
@@ -140,23 +146,23 @@ extern "C" __EXPORT int nanoradar_mr72_main(int argc, char *argv[])
break;
default:
return nanoradar_mr72::usage();
return mr72_radar::usage();
}
}
if (myoptind >= argc) {
return nanoradar_mr72::usage();
return mr72_radar::usage();
}
if (!strcmp(argv[myoptind], "start")) {
return nanoradar_mr72::start(device_path, rotation);
return mr72_radar::start(device_path, rotation);
} else if (!strcmp(argv[myoptind], "stop")) {
return nanoradar_mr72::stop();
return mr72_radar::stop();
} else if (!strcmp(argv[myoptind], "status")) {
return nanoradar_mr72::status();
return mr72_radar::status();
}
return nanoradar_mr72::usage();
return mr72_radar::usage();
}

53
src/drivers/distance_sensor/nanoradar_mr72/nanoradar_mr72_params.c Normal file
View File

@@ -0,0 +1,53 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file nanoradar_mr72_params.c
*
* Parameters defined by the nanoradar_mr72 lib.
*
* @author wp <wp_uav@163.com>
*/
/**
* nanoradar mr72 filter frequency
*
*
*
* @min -1
* @max 100
* @unit m
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(NANO_MR_FILT, -1.0f);

32
src/lib/collision_prevention/CollisionPrevention.cpp
View File

@@ -518,7 +518,9 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max
{
//calculate movement constraints based on range data
Vector2f new_setpoint = original_setpoint;
Vector2f old_setpoint = original_setpoint;
_calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);
const float cp_work_height = _param_cp_work_height.get();
//warn user if collision prevention starts to interfere
bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed
@@ -528,14 +530,42 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max
_interfering = currently_interfering;
if(_sub_vehicle_local_pos.update() ){
_local_pos = _sub_vehicle_local_pos.get();
}
// publish constraints
collision_constraints_s constraints{};
constraints.timestamp = getTime();
original_setpoint.copyTo(constraints.original_setpoint);
new_setpoint.copyTo(constraints.adapted_setpoint);
_constraints_pub.publish(constraints);
constraints.interfering = _interfering;
constraints.interfering_fail = false;
original_setpoint = new_setpoint;
if( (_local_pos.z_valid || _local_pos.dist_bottom_valid ) && cp_work_height > -1e-5f ){
if( _local_pos.z_valid && _local_pos.z > -cp_work_height ){
// if( _local_pos.z_valid && _local_pos.z > -2 ){
original_setpoint = old_setpoint;
constraints.interfering_fail = true;
}else{
constraints.interfering_fail = false;
}
if( _local_pos.dist_bottom_valid && _local_pos.dist_bottom < cp_work_height ){
// if( _local_pos.dist_bottom_valid && _local_pos.dist_bottom < 2 ){
original_setpoint = old_setpoint;
constraints.interfering_fail = true;
}else{
constraints.interfering_fail = false;
}
}
_constraints_pub.publish(constraints);
}
void CollisionPrevention::_publishVehicleCmdDoLoiter()

12
src/lib/collision_prevention/CollisionPrevention.hpp
View File

@@ -59,6 +59,11 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/obstacle_distance.h>
#include <uORB/topics/vehicle_local_position.h>
using namespace time_literals;
class CollisionPrevention : public ModuleParams
@@ -133,6 +138,7 @@ private:
uORB::SubscriptionData<obstacle_distance_s> _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */
uORB::SubscriptionData<vehicle_attitude_s> _sub_vehicle_attitude{ORB_ID(vehicle_attitude)};
uORB::SubscriptionData<vehicle_local_position_s> _sub_vehicle_local_pos{ORB_ID(vehicle_local_position)};
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500_ms};
@@ -141,6 +147,9 @@ private:
hrt_abstime _last_timeout_warning{0};
hrt_abstime _time_activated{0};
vehicle_local_position_s _local_pos;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::CP_DIST>) _param_cp_dist, /**< collision prevention keep minimum distance */
(ParamFloat<px4::params::CP_DELAY>) _param_cp_delay, /**< delay of the range measurement data*/
@@ -148,7 +157,8 @@ private:
(ParamBool<px4::params::CP_GO_NO_DATA>) _param_cp_go_nodata, /**< movement allowed where no data*/
(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p, /**< p gain from position controller*/
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max, /**< vehicle maximum jerk*/
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, /**< vehicle maximum horizontal acceleration*/
(ParamFloat<px4::params::CP_WORK_HGT>) _param_cp_work_height /**< vehicle maximum horizontal acceleration*/
)
/**

14
src/lib/collision_prevention/collisionprevention_params.c
View File

@@ -75,6 +75,19 @@ PARAM_DEFINE_FLOAT(CP_DELAY, 0.4f);
*/
PARAM_DEFINE_FLOAT(CP_GUIDE_ANG, 30.f);
/**
* above this height the collision prevention work, otherwise this function wo't work incase distance sensor data incorrect
*
*
* Only used in Position mode. set to -1 will make this function work all time
*
* @min -1
* @max 90
* @unit m
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(CP_WORK_HGT, 2.f);
/**
* Boolean to allow moving into directions where there is no sensor data (outside FOV)
*
@@ -84,3 +97,4 @@ PARAM_DEFINE_FLOAT(CP_GUIDE_ANG, 30.f);
* @group Multicopter Position Control
*/
PARAM_DEFINE_INT32(CP_GO_NO_DATA, 0);

8
src/lib/drivers/rangefinder/PX4Rangefinder.cpp
View File

@@ -64,6 +64,14 @@ void PX4Rangefinder::set_orientation(const uint8_t device_orientation)
{
_distance_sensor_pub.get().orientation = device_orientation;
}
void PX4Rangefinder::set_orientation(const float quant[4]){
_distance_sensor_pub.get().orientation = distance_sensor_s::ROTATION_CUSTOM;
_distance_sensor_pub.get().q[0] = quant[0];
_distance_sensor_pub.get().q[1] = quant[1];
_distance_sensor_pub.get().q[2] = quant[2];
_distance_sensor_pub.get().q[3] = quant[3];
}
void PX4Rangefinder::update(const hrt_abstime &timestamp_sample, const float distance, const int8_t quality)
{

1
src/lib/drivers/rangefinder/PX4Rangefinder.hpp
View File

@@ -59,6 +59,7 @@ public:
void set_min_distance(const float distance) { _distance_sensor_pub.get().min_distance = distance; }
void set_orientation(const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
void set_orientation(const float quant[4]);
void update(const hrt_abstime &timestamp_sample, const float distance, const int8_t quality = -1);

4
src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
View File

@@ -1,3 +1,4 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
@@ -40,7 +41,8 @@
using namespace matrix;
FlightTaskManualAcceleration::FlightTaskManualAcceleration() :
_stick_acceleration_xy(this),
_stick_acceleration_xy(this)
// ,
// _collision_prevention(this)
{};

3
src/modules/logger/logged_topics.cpp
View File

@@ -162,7 +162,8 @@ void LoggedTopics::add_default_topics()
// log all raw sensors at minimal rate (at least 1 Hz)
add_topic_multi("battery_status", 200, 2);
add_topic_multi("differential_pressure", 1000, 2);
add_topic_multi("distance_sensor", 1000);
// add_topic_multi("distance_sensor", 1000);
add_topic_multi("distance_sensor");
add_topic_multi("optical_flow", 1000, 1);
add_topic_multi("sensor_accel", 1000, 4);
add_topic_multi("sensor_baro", 1000, 4);