ardupilot4.2.2-雷达CAN通讯

        雷达采用的是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();
}

  • 1
    点赞
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值