雷达采用的是MR72,其BMS默认为0x21,最远探测距离是80米,通讯过程中会依次传回8个扇区测到的距离。
其CAN通讯和电池CAN通讯一致,最终在bool Frame::parse函数当中进行数据的解析。
bool Frame::parse(const CanFrame& can_frame)
{
/// 雷 达 //
if(src_node_id_ == 0x20){
hal.util->mr72_time = AP_HAL::millis();
MR72PI ++;
switch(transfer_id_.get())
{
case 0:
gMR72code = ((int)can_frame.data[3]<<8) + can_frame.data[2];
gMR72Dist[0] = ((int)can_frame.data[5]<<8) + can_frame.data[4];
gMR72Dist[1] = can_frame.data[6];
break;
case 1:
gMR72Dist[1] += ((int)can_frame.data[0]<<8);
gMR72Dist[2] = ((int)can_frame.data[2]<<8) + can_frame.data[1];
gMR72Dist[3] = ((int)can_frame.data[4]<<8) + can_frame.data[3];
gMR72Dist[4] = ((int)can_frame.data[6]<<8) + can_frame.data[5];
break;
case 2:
gMR72Dist[5] = ((int)can_frame.data[1]<<8) + can_frame.data[0];
gMR72Dist[6] = ((int)can_frame.data[3]<<8) + can_frame.data[2];
gMR72Dist[7] = ((int)can_frame.data[5]<<8) + can_frame.data[4];
break;
default:
break;
}
if(end_of_transfer_ ==1 && MR72PI >=4)
{
for (int i=0; i<8; i++)
{
if (gMR72Dist[i] >= 8000)//最远距离为80m,这里的单位为cm
{
gMR72Dist[i] = 0;
}
hal.util->mr72_dist[i] = gMR72Dist[i];
}
}
}
}
解析到的8个扇区的距离数据都存储到hal.util->mr72_dist[i]数组当中,这些数据将到Proximity库当中运用。
1、初始化
void AP_Proximity::init(void)
{
if (num_instances != 0) {
// c
return;
}
for (uint8_t i=0; i<PROXIMITY_MAX_INSTANCES; i++) {
detect_instance(i);//识别Proximity类型
if (drivers[i] != nullptr) {
// we loaded a driver for this instance, so it must be
// present (although it may not be healthy)
num_instances = i+1;
}
// initialise status
state[i].status = Status::NotConnected;
}
}
2、实例识别detect_instance(i);//识别Proximity类型
// detect if an instance of a proximity sensor is connected.检测是否连接了接近传感器的实例。
void AP_Proximity::detect_instance(uint8_t instance)
{
switch (get_type(instance)) {
case Type::None:
return;
case Type::RPLidarA2:
if (AP_Proximity_RPLidarA2::detect()) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_RPLidarA2(*this, state[instance]);
return;
}
break;
case Type::MAV:
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_MAV(*this, state[instance]);
return;
case Type::TRTOWER:
if (AP_Proximity_TeraRangerTower::detect()) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance]);
return;
}
break;
case Type::TRTOWEREVO:
if (AP_Proximity_TeraRangerTowerEvo::detect()) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_TeraRangerTowerEvo(*this, state[instance]);
return;
}
break;
case Type::RangeFinder:
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance]);
return;
case Type::SF40C:
if (AP_Proximity_LightWareSF40C::detect()) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance]);
return;
}
break;
case Type::SF45B:
if (AP_Proximity_LightWareSF45B::detect()) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_LightWareSF45B(*this, state[instance]);
return;
}
break;
case Type::CYGBOT_D1:
#if AP_PROXIMITY_CYGBOT_ENABLED
if (AP_Proximity_Cygbot_D1::detect()) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_Cygbot_D1(*this, state[instance]);
return;
}
# endif
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case Type::SITL:
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_SITL(*this, state[instance]);
return;
case Type::AirSimSITL:
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance]);
return;
#endif
}
}
在实例识别当中可以发现所有的类型都是采用了串口通讯,而我们的雷达采用的是CAN通讯,因此我们要在实例识别程序中添加CAN通讯。
本次以TRTOWEREVO类型为例进行操作,即参数 PRX_TYPE=6;
进入AP_Proximity_TeraRangerTowerEvo.cpp当中,它的更新程序为:
void AP_Proximity_TeraRangerTowerEvo::update(void)
{
if (_uart == nullptr) {
return;
}
if (_last_request_sent_ms == 0) {
_last_request_sent_ms = AP_HAL::millis();
}
//initialize the sensor
if(_current_init_state != InitState::InitState_Finished)
{
initialise_modes();
}
// process incoming messages
read_sensor_data();//读取数据
// 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);
} else {
set_status(AP_Proximity::Status::Good);
}
}
其中最主要的是read_sensor_data();//读取数据函数
// check for replies from sensor, returns true if at least one message was processed
bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data()
{
if (_uart == nullptr) {
return false;
}
uint16_t message_count = 0;
int16_t nbytes = _uart->available();
if(copter.get_avoid_port() == 0){ //串口雷达
if(_current_init_state != InitState_Finished && nbytes == 4) {
//Increment _current_init_state only when we receive 4 ack bytes
switch (_current_init_state) {
case InitState_Printout:
_current_init_state = InitState_Sequence;
break;
case InitState_Sequence:
_current_init_state = InitState_Rate;
break;
case InitState_Rate:
_current_init_state = InitState_StreamStart;
break;
case InitState_StreamStart:
_current_init_state = InitState_Finished;
break;
case InitState_Finished:
break;
}
}
while (nbytes-- > 0) {
char c = _uart->read();
if (c == 'T' ) {
buffer_count = 0;
}
buffer[buffer_count++] = c;
// we should always read 19 bytes THxxxxxxxxxxxxxxxxMC
if (buffer_count >= 20){
buffer_count = 0;
//check if message has right CRC
if (crc_crc8(buffer, 19) == buffer[19]){
update_sector_data(0, UINT16_VALUE(buffer[2], buffer[3])); // d1
update_sector_data(45, UINT16_VALUE(buffer[4], buffer[5])); // d2
update_sector_data(90, UINT16_VALUE(buffer[6], buffer[7])); // d3
update_sector_data(135, UINT16_VALUE(buffer[8], buffer[9])); // d4
update_sector_data(180, UINT16_VALUE(buffer[10], buffer[11])); // d5
update_sector_data(225, UINT16_VALUE(buffer[12], buffer[13])); // d6
update_sector_data(270, UINT16_VALUE(buffer[14], buffer[15])); // d7
update_sector_data(315, UINT16_VALUE(buffer[16], buffer[17])); // d8
message_count++;
}
}
}
else if(copter.get_avoid_port() == 1){//CAN口雷达
update_sector_data(0, hal.util->mr72_dist[0]); // d1 hal.util->mr72_dist[i],
update_sector_data(45, hal.util->mr72_dist[1]); // d2
update_sector_data(90, hal.util->mr72_dist[2]); // d3
update_sector_data(135, hal.util->mr72_dist[3]); // d4
update_sector_data(180, hal.util->mr72_dist[4]); // d5
update_sector_data(225, hal.util->mr72_dist[5]); // d6
update_sector_data(270, hal.util->mr72_dist[6]); // d7
update_sector_data(315, hal.util->mr72_dist[7]); // d8
}
return (message_count > 0);
}
在此函数当中添加CAN雷达通讯,并将我们的读取到的扇区数据传递给实例当中即可。
3、更新避障数据
void AP_Proximity::update(void)
{
for (uint8_t i=0; i<num_instances; i++) {
if (!valid_instance(i)) {
continue;
}
drivers[i]->update();//驱动数据更新
drivers[i]->boundary_3D_checks();
}
// work out primary instance - first sensor returning good data
for (int8_t i=num_instances-1; i>=0; i--) {
// gcs().send_text(MAV_SEVERITY_CRITICAL, "update 3");
if (drivers[i] != nullptr && (state[i].status == Status::Good)) {
primary_instance = i;
// gcs().send_text(MAV_SEVERITY_CRITICAL, "update 3");
}
}
}
其中的drivers[i]->update();//驱动数据更新即为AP_Proximity_TeraRangerTowerEvo::update(void)函数
// update the state of the sensor
void AP_Proximity_TeraRangerTowerEvo::update(void)
{
if (_uart == nullptr) {
return;
}
if (_last_request_sent_ms == 0) {
_last_request_sent_ms = AP_HAL::millis();
}
//initialize the sensor
if(_current_init_state != InitState::InitState_Finished)
{
initialise_modes();
}
// process incoming messages
read_sensor_data();//读取数据
// 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);
} else {
set_status(AP_Proximity::Status::Good);
}
}
在read_sensor_data();函数中读取到8个扇区的雷达数据。
// check for replies from sensor, returns true if at least one message was processed
bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data()
{
if (_uart == nullptr) {
return false;
}
uint16_t message_count = 0;
int16_t nbytes = _uart->available();
if(copter.get_avoid_port() == 0){ //串口雷达
if(_current_init_state != InitState_Finished && nbytes == 4) {
//Increment _current_init_state only when we receive 4 ack bytes
switch (_current_init_state) {
case InitState_Printout:
_current_init_state = InitState_Sequence;
break;
case InitState_Sequence:
_current_init_state = InitState_Rate;
break;
case InitState_Rate:
_current_init_state = InitState_StreamStart;
break;
case InitState_StreamStart:
_current_init_state = InitState_Finished;
break;
case InitState_Finished:
break;
}
}
while (nbytes-- > 0) {
char c = _uart->read();
if (c == 'T' ) {
buffer_count = 0;
}
buffer[buffer_count++] = c;
// we should always read 19 bytes THxxxxxxxxxxxxxxxxMC
if (buffer_count >= 20){
buffer_count = 0;
//check if message has right CRC
if (crc_crc8(buffer, 19) == buffer[19]){
update_sector_data(0, UINT16_VALUE(buffer[2], buffer[3])); // d1
update_sector_data(45, UINT16_VALUE(buffer[4], buffer[5])); // d2
update_sector_data(90, UINT16_VALUE(buffer[6], buffer[7])); // d3
update_sector_data(135, UINT16_VALUE(buffer[8], buffer[9])); // d4
update_sector_data(180, UINT16_VALUE(buffer[10], buffer[11])); // d5
update_sector_data(225, UINT16_VALUE(buffer[12], buffer[13])); // d6
update_sector_data(270, UINT16_VALUE(buffer[14], buffer[15])); // d7
update_sector_data(315, UINT16_VALUE(buffer[16], buffer[17])); // d8
message_count++;
}
}
}
else if(copter.get_avoid_port() == 1){//CAN口雷达
update_sector_data(0, hal.util->mr72_dist[0]); // d1 hal.util->mr72_dist[i],
update_sector_data(45, hal.util->mr72_dist[1]); // d2
update_sector_data(90, hal.util->mr72_dist[2]); // d3
update_sector_data(135, hal.util->mr72_dist[3]); // d4
update_sector_data(180, hal.util->mr72_dist[4]); // d5
update_sector_data(225, hal.util->mr72_dist[5]); // d6
update_sector_data(270, hal.util->mr72_dist[6]); // d7
update_sector_data(315, hal.util->mr72_dist[7]); // d8
}
return (message_count > 0);
}
而update_sector_data(0, hal.util->mr72_dist[0]); 函数即是数据传递。
// process reply
void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
{
// Get location on 3-D boundary based on angle to the object
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg);
//check for target too far, target too close and sensor not connected
const bool valid = (distance_cm != 0xffff) && (distance_cm > 0x0001);
if (valid && !ignore_reading(angle_deg, distance_cm * 0.001f, false)) {
boundary.set_face_attributes(face, angle_deg, ((float) distance_cm) / 1000);
// update OA database
database_push(angle_deg, ((float) distance_cm) / 1000);
} else {
boundary.reset_face(face);
}
_last_distance_received_ms = AP_HAL::millis();
}