【ardupilot增加自定义测距传感器-NRA15】

ardupilot增加自定义测距传感器-NRA15

环境描述

1.ardupilot软件版本：copter-4.2.2
2.编辑软件：VS code
3.编译软件：Cygwin64
4.地面站：Missionplanner

代码添加步骤

NRA15协议截图如下：

① 在目录"…\ardupilot\libraries\AP_RangeFinder"下新建AP_RangeFinder_NRA15Serial.cpp，AP_RangeFinder_NRA15Serial.h两个文件
AP_RangeFinder_NRA15Serial.cpp程序如下：

#include <AP_HAL/AP_HAL.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include "AP_RangeFinder_NRA15Serial.h"

extern const AP_HAL::HAL& hal;

// read - return last value measured by sensor
inline bool if_data_frame(uint8_t *buf ,float &reading_m){
    uint8_t *payload=(buf+4);  // 目标数据
    uint8_t *msg_id=(buf+2);   // 包ID
    if((msg_id[0]+(((uint16_t)msg_id[1])<<8))!=0x70c) return false;
    reading_m=((payload[2]*0x100+payload[3])*0.01);
    return true;
}

// frame: Start_Sequence Message_ID Data_Payload End_Sequence
bool AP_RangeFinder_NRA15_Serial::get_reading(float &reading_m)
{
    if (uart == nullptr) {
        return false;
    }
    uint32_t nbytes = uart->available();
    while(nbytes-->0){
        uint8_t c = uart->read();
        switch (_reading_state)
        {
        case Status::WAITTING:{            
            if (c == 0xAA)
            {
                buffer_count=0;
                linebuf[buffer_count]=c;
                
                _reading_state=Status::GET_HEAD_ONCE;
            }
            break;
        }
        case Status::GET_HEAD_ONCE:{
            if(c == 0xAA){
                buffer_count++;
                linebuf[buffer_count]=c;
                _reading_state=Status::WAITTING_FOR_TAIL;
            }
            else
            {
                buffer_count++;
                linebuf[buffer_count]=0xAA;
                buffer_count++;
                linebuf[buffer_count]=c;
                _reading_state=Status::WAITTING_FOR_TAIL;
            }
            
            break;
        }
        case Status::WAITTING_FOR_TAIL:{
            buffer_count++;
            linebuf[buffer_count]=c;
            if(c==0x55)
                _reading_state=Status::GET_TAIL_ONCE;
            break;
        }

        case Status::GET_TAIL_ONCE:{
            if(c ==0x55){
                buffer_count++;
                linebuf[buffer_count]=c;
                _reading_state=Status::GET_ONE_FRAME;
            }
            break;
        }
        case Status::GET_ONE_FRAME:{
            _reading_state=Status::WAITTING;
            if(if_data_frame(linebuf,reading_m)) return true;
            break;
        }

        default:
            break;
        }
        if(buffer_count>sizeof(linebuf)){
            buffer_count=0;
            _reading_state=Status::WAITTING;
        }
    }
    return true;

}

AP_RangeFinder_NRA15Serial.h程序如下：

#pragma once

#include "AP_RangeFinder.h"
#include "AP_RangeFinder_Backend_Serial.h"

class AP_RangeFinder_NRA15_Serial : public AP_RangeFinder_Backend_Serial
{

public:
    using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
protected:
    enum class Status {
        WAITTING=0,
        GET_HEAD_ONCE,
        GET_HEAD,
        GET_TAIL_ONCE,
        GET_TAIL,
        GET_ONE_FRAME,
        WAITTING_FOR_TAIL,
    }_reading_state;

    virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
        return MAV_DISTANCE_SENSOR_LASER;
    }

private:
    // get a reading
    bool get_reading(float &reading_m) override;

    uint8_t linebuf[50];
    uint8_t buffer_count=0;
};

② 在AP_RangeFinder.cpp包含
#include "AP_RangeFinder_NRA15Serial.h"
③ 在void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)函数switch语句中添加：

case Type::NRA15:
        if (AP_RangeFinder_NRA15_Serial::detect(serial_instance)) {
            _add_backend(new AP_RangeFinder_NRA15_Serial(state[instance], params[instance]), instance, serial_instance++);
        }

④ 在class RangeFinder的enum class Type中新增测距传感器NRA15：

NRA15 = 67,

⑤ 编译然后下载到飞控

测试步骤

① Missionplanner参数设置
Ⅰ 设置串口及波特率，这里我用的NRA15接口为串口，我接在飞控的telem2上
SERIAL2_PROTOCOL = 9
SERIAL2_BAUD = 115
Ⅱ 设置测距传感器类型，测距范围及安装方向
RNGFND1_TYPE = 67
RNGFND1_MAX_CM = 3000
RNGFND1_MIN_CM = 10
RNGFND1_ORIENT = 25 // 向下安装
Ⅲ 写入参数，然后重启飞控
② 调试查看
在Missionplanner最下面有一个调试，勾选上然后会出来一个曲线显示表

双击箭头所指的位置，然后再列表里找到刚刚配置的rangefinder1，勾选就会显示对应的传感器测量曲线。