Ardupilot使用Nra24毫米波雷达仿地飞行

引子

仿地飞行能缓解植保多旋翼在农业植保过程中产生的雾滴漂移问题，增加雾滴有效沉积率，提升施药效果。传统植保无人机使用超声波作为高度计，实际应用过程中由于超声波传感器的频段处在40khz-45khz之间，容易穿透植被，不容易获得植被真实高度信息。此外，超声波传感器还容易受到雾滴及风场影响。

毫米波雷达不容易穿透植被，对雾滴及粉尘穿透性强，仿地高度信息稳定可靠。这里选用Nra24型号毫米波雷达接入Ardupilot飞控系统，实现仿地飞行。

注意：所使用的Ardupilot版本是2019年12月1日前后的4.0.0-dev的master版本。不同master版本在编写传感器驱动的细节上有一定区别。

毫米波雷达接入ardupilot

NRA24通信协议较为简单，这里不再赘述，如有需求，查阅官方手册即可。
在${ardupilot project}/libraries/AP_RangeFinder目录下新建AP_RangeFinder_NRA24Serial.cpp及AP_RangeFinder_NRA24Serial.h两个文件。

AP_RangeFinder_NRA24Serial.cpp文件

#include <AP_HAL/AP_HAL.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <ctype.h>
#include "AP_RangeFinder_NRA24Serial.h"

extern const AP_HAL::HAL& hal;


bool AP_RangeFinder_NRA24_Serial::detect(uint8_t serial_instance)
{
    return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance) != nullptr;
}

// read - return last value measured by sensor

inline bool if_data_frame(uint8_t *buf ,uint16_t &reading_cm){

    uint8_t *payload=(buf+4);
   
    uint8_t *msg_id=(buf+2);

 
    if((msg_id[0]+(((uint16_t)msg_id[1])<<8))!=0x70c) return false;


    reading_cm=static_cast<uint16_t>(((payload[2]<<8)+payload[3]*0.01)*100);

    return true;
}

bool AP_RangeFinder_NRA24_Serial::get_reading(uint16_t & reading_cm)
{
    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_cm)) return true;
            break;
        }

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

}

AP_RangeFinder_NRA24Serial.h文件内容如下

#pragma once

#include "AP_RangeFinder.h"
#include "AP_RangeFinder_Backend_Serial.h"
#define NRA24_SERIAL_BAUD_RATE 115200

class AP_RangeFinder_NRA24_Serial : public AP_RangeFinder_Backend_Serial
{

public:
    static bool detect(uint8_t serial_instance);
    using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
    uint32_t initial_baudrate(uint8_t serial_instance) const override{
        return NRA24_SERIAL_BAUD_RATE;
    }
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(uint16_t &reading_cm) override;

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

完成上述文件内容后，在同目录下AP_RangeFinder.cpp文件中

void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)

函数下switch中添加

    case Type::NRA24:
        if (AP_RangeFinder_NRA24_Serial::detect(serial_instance)) {
            drivers[instance] = new AP_RangeFinder_NRA24_Serial(state[instance], params[instance], serial_instance++);
        }
        break;

在AP_RangeFinder.h文件中

 class RangeFinder

类中 enum class Type 中添加

NRA24 = 28

至此，源代码修改部分完成，编译并将程序烧入飞控。

地面站设置

进入地面站对参数进行修改

参数名	参数	备注
RNGFNDX_TYPE	28	代表刚刚新建的nra24
RNGFNDX_MIN_CM	10	最小检测距离
RNFNDX_MAX_CM	3000	最大检测距离
SERIALX_PROTOCOL	Rangefinder	波特率不需要单独设置，已经固化进源代码

经过设置，重新启动飞控，就能在地面站中看到传感器数值了。

值得注意的是，这款毫米波雷达在静态的时候有时候是没有数值的，需要在程序中作出一定的处理。以上代码暂时没有考虑这一点，但先能用起来再说。

定高效果

待完善

仿地飞行效果

待完善