pragma once
2
include
4
include
5
include
6
define AP_SERIALMANAGER_OPEN_MV_BAUD 115200 //串口波特率定义
6
define AP_SERIALMANAGER_OPENMV_BUFSIZE_RX 64//接受缓存的容量
7
define AP_SERIALMANAGER_OPENMV_BUFSIZE_TX 64//发送缓存容量
8
include “AP_OpenMV.h”
10
if LOGGING_ENABLED == ENABLED
2
endif
4

通过前面的课程大家已经对飞控源码有了更深刻的认识，明白了飞控源码级是如何和地面站互动的，这一节课我们要明白飞控是如何和外设互动的，掌握这个可以让大家给自己的无人机添加实现特别功能的外部设备。

一概念理解
外设：故名思意，即外部设备。外部设备大致可分为三类：
1．人机交互设备，如打印机，显示器，绘图仪，语言合成器。
2．计算机信息的存储设备，如磁盘，光盘，磁带。
3机－机通信设备，如两台计算机之间可利用电话线进行通信，它们可以通过调制解调器完成。
外设可以简单的理解为输入设备和输出设备。如显示器只是用来显示电脑信息的输出设备，鼠标键盘是用来输入信息的输入设备，都属于外设。计算机系统中输入、输出设备和外存储器的统称。对数据和信息起着传输、转送和存储的作用。是计算机系统中的重要组成部分。
外围设备涉及到主机以外的任何设备。外围设备是附属的或辅助的与计算机连接起来的设备。外围设备能扩充计算机系统。
对于飞控系统来说，除去飞控系统内部完成自己控制飞行的主要任务的那部分必要设备外，人们用于完成特定任务的那部分设备称为外设。无人机离开了外设也就是离开了任务，也就失去了无人机的特色，与航模无异。航拍无人机的机载航拍设备就是外设。
OpenMV:OpenMV是一个开源，低成本，功能强大的机器视觉模块。以STM32F767CPU为核心，集成了OV7725摄像头芯片，在小巧的硬件模块上，用C语言高效地实现了核心机器视觉算法，提供Python编程接口。使用者们（包括发明家、爱好者以及智能设备开发商）可以用python语言使用OpenMV提供的机器视觉功能，为自己的产品和发明增加有特色的竞争力。
OpenMV上的机器视觉算法包括寻找色块、人脸检测、眼球跟踪、边缘检测、标志跟踪等。可以用来实现非法入侵检测、产品的残次品筛选、跟踪固定的标记物等。使用者仅需要写一些简单的Python代码，即可轻松的完成各种机器视觉相关的任务。小巧的设计，使得OpenMV可以用到很多创意的产品上。比如，可以给自己的机器人提供周边环境感知能力；给智能车增加视觉巡线功能；给智能玩具增加识别人脸功能，提高产品趣味性等；甚至，可以给工厂产品线增加残次品筛选功能等。
OpenMV采用的STM32F427拥有丰富的硬件资源，引出UART，I2C，SPI，PWM，ADC，DAC以及GPIO等接口方便扩展外围功能。USB接口用于连接电脑上的集成开发环境OpenMVIDE，协助完成编程、调试和更新固件等工作。TF卡槽支持大容量的TF卡，可以用于存放程序和保存照片等。 (链接：https://www.jianshu.com/p/8ff6a7223ebb 来源：简书)
OpenMV是当下智能机器人方面比较火热的一个机器视觉模块，用小巧精悍来说明不为过。（具体可以看看星瞳科技的官网：https://singtown.com/openmv/）这一课我们先看看如何将openmv这个好东西添加到我们的飞控当中去。

二添加外设驱动OpenMV

步骤一:添加驱动文件
利用前面的知识拷贝一份源码（版本稍微高一点）
进入ardupilot/libraries目录，新建OpenMV的底层驱动目录文件，命名为AP_OpenMV。进入该文件中新建一个AP_OpenMV.cpp文件和AP_OpenMV.h文件。
首先，在AP_OpenMV.h文件中添加如下代码。

C++复制代码

pragma once
2

include
4

include
5

include
6

class AP_OpenMV {
8

public:
9

AP_OpenMV();<br />10
AP_OpenMV(const AP_OpenMV &other) = delete;<br />11
AP_OpenMV &operator=(const AP_OpenMV&) = delete;<br />12

// 初始化外设<br />14
void init(const AP_SerialManager& serial_manager);<br />15

// 定时更新外设数据<br />17
bool update(void);<br />18

uint8_t cx;//定义x坐标变量

其次，在AP_OpenMV.cpp文件中添加如下代码。

C++复制代码

/*
2

OpenMV library
3

*/
4

define AP_SERIALMANAGER_OPEN_MV_BAUD 115200 //串口波特率定义
6

define AP_SERIALMANAGER_OPENMV_BUFSIZE_RX 64//接受缓存的容量
7

define AP_SERIALMANAGER_OPENMV_BUFSIZE_TX 64//发送缓存容量
8

include “AP_OpenMV.h”
10

extern const AP_HAL::HAL& hal;
12

//constructor
14

AP_OpenMV::AP_OpenMV(void)
15

{
16

_port = NULL;//串口默认为空<br />17
_step = 0;//解析贞步骤默认为0<br />18

}
19

void AP_OpenMV::init(const AP_SerialManager& serial_manager)
22

{
23

// 查找串口为_port附值，并判断openmv是否接入<br />24
if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_OPEN_MV, 0))) {  //注意，这里出现了SerialProtocol_OPEN_MV这个值，代码段结束后将设置这个值<br />25
    _port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);<br />26
    // 初始化与openmv链接的串口设置

保存上述文件，在ardupilot/libraries/AP_SerialManager/AP_SerialManager.h文件中第80行附近的AP_SerialManager类里的SerialProtocol枚举类型末尾添加一个openmv成员设为19。意思是在MP地面站的全部参数表里面串口类型的地方设置为19就表示链接openmv

C++复制代码

enum SerialProtocol {
2

    SerialProtocol_None = -1,<br />3
    SerialProtocol_Console = 0, // unused<br />4
    SerialProtocol_MAVLink = 1,<br />5
    SerialProtocol_MAVLink2 = 2,                 // do not use - use MAVLink and provide instance of 1<br />6
    SerialProtocol_FrSky_D = 3,                  // FrSky D protocol (D-receivers)<br />7
    SerialProtocol_FrSky_SPort = 4,              // FrSky SPort protocol (X-receivers)<br />8
    SerialProtocol_GPS = 5,<br />9
    SerialProtocol_GPS2 = 6,                     // do not use - use GPS and provide instance of 1<br />10
    SerialProtocol_AlexMos = 7,<br />11
    SerialProtocol_SToRM32 = 8,<br />12
    SerialProtocol_Rangefinder = 9,<br />13
    SerialProtocol_FrSky_SPort_Passthrough = 10, // FrSky SPort Passthrough (OpenTX) protocol (X-receivers)<br />14
    SerialProtocol_Lidar360 = 11,                // Lightware SF40C, TeraRanger Tower or RPLidarA2<br />15
    SerialProtocol_Aerotenna_uLanding      = 12, // Ulanding support - deprecated, users should use Rangefinder<br />16
    SerialProtocol_Beacon = 13,<br />17
    SerialProtocol_Volz = 14,                    // Volz servo protocol<br />18
    SerialProtocol_Sbus1 = 15,<br />19
    SerialProtocol_ESCTelemetry = 16,<br />20
    SerialProtocol_Devo_Telem = 17,<br />21
    SerialProtocol_OpticalFlow = 18,<br />22
    SerialProtocol_OPEN_MV = 19,  //！！！！这里！！！！<br />23
};

到这里只是表示修改代码的人知道了有openmv的加入，但是对于编译器来说并不知道，所以我们需要在ardupilot/ArduCopter/wscript中第38行附近加入

Python复制代码

def build(bld):
2

vehicle = bld.path.name<br />3
bld.ap_stlib(<br />4
    name=vehicle + '_libs',<br />5
    ap_vehicle=vehicle,<br />6
    ap_libraries=bld.ap_common_vehicle_libraries() + [<br />7
        'AP_ADSB',<br />8
        'AC_AttitudeControl',<br />9
        'AC_InputManager',<br />10
        'AC_Fence',<br />11
        'AC_Avoidance',<br />12
        'AC_PID',<br />13
        'AC_PrecLand',<br />14
        'AC_Sprayer',<br />15
        'AC_WPNav',<br />16
        'AP_Camera',<br />17
        'AP_IRLock',<br />18
        'AP_InertialNav',<br />19
        'AP_LandingGear',

到此我们的驱动添加完毕，接下来是调用，就是让飞控定时调用我们的驱动。

步骤二：在源码中添加顶层调用
在ardupilot/ArduCopter/ArduCopter.cpp文件中第132行附近的任务表中加入openmv任务

C++复制代码

if LOGGING_ENABLED == ENABLED
2

SCHED_TASK(fourhundred_hz_logging,400,    50),<br />3

endif
4

SCHED_TASK_CLASS(AP_Notify,            &copter.notify,              update,          50,  90),<br />5
SCHED_TASK(one_hz_loop,            1,    100),<br />6
SCHED_TASK(update_OpenMV,        400,    100),  //这里加入openmv的调用任务，表示运行频率400hz，最大运行时间不超过100um<br />7
SCHED_TASK(ekf_check,             10,     75),<br />8
SCHED_TASK(gpsglitch_check,       10,     50),<br />9
SCHED_TASK(landinggear_update,    10,     75),<br />10
SCHED_TASK(lost_vehicle_check,    10,     50),<br />11
SCHED_TASK(gcs_check_input,      400,    180),<br />12
SCHED_TASK(gcs_send_heartbeat,     1,    110),<br />13
SCHED_TASK(gcs_send_deferred,     50,    550),<br />14
SCHED_TASK(gcs_data_stream_send,  50,    550),

在ardupilot/ArduCopter/Copter.h文件中第673行附近写出openmv任务函数的声明

C++复制代码

// ArduCopter.cpp<br />3
void fast_loop();<br />4
void rc_loop();<br />5
void throttle_loop();<br />6
void update_batt_compass(void);<br />7
void update_OpenMV(void);       //openmv任务函数声明<br />8
void fourhundred_hz_logging();<br />9
void ten_hz_logging_loop();<br />10
void twentyfive_hz_logging();<br />11
void three_hz_loop();<br />12
void one_hz_loop();<br />13
void update_GPS(void);<br />14
void init_simple_bearing();<br />15
void update_simple_mode(void);<br />16
void update_super_simple_bearing(bool force_update);<br />17
void read_AHRS(void);<br />18
void update_altitude();<br />19

函数的具体实现在ardupilot/ArduCopter/ArduCopter.cpp中第317行附近

C++复制代码

//openmv模拟输入数据函数
2

void Copter::update_OpenMV(void)
3

{
4

// simulation,在实际飞行时需要将这以下代码（到end of simulation code 处结束）<br />5
bool sim_openmv_new_data = false;<br />6
static uint32_t last_sim_new_data_time_ms = 0;<br />7
if(control_mode != GUIDED) {<br />8
    last_sim_new_data_time_ms = millis();<br />9
    openmv.cx = 80;<br />10
    openmv.cy = 60;<br />11
} else if (millis()- last_sim_new_data_time_ms < 15000) {<br />12
    sim_openmv_new_data = true;<br />13
    openmv.last_frame_ms = millis();<br />14
    openmv.cx = 1;<br />15
    openmv.cy = 1;<br />16
} else if (millis()- last_sim_new_data_time_ms < 30000) {<br />17
    sim_openmv_new_data = true;<br />18
    openmv.last_frame_ms = millis();<br />19
    openmv.cx = 160;<br />20
    openmv.cy = 120;<br />21
} else {<br />22
    sim_openmv_new_data = false;<br />23
    openmv.cx = 80;<br />24
    openmv.cy = 60;<br />25
}<br />26

// end of simulation code（这里之上的是制造openmv模拟数据的代码）<br />28

static uint32_t last_set_pos_target_time_ms = 0;<br />30
Vector3f target = Vector3f(0, 0, 0);<br />31
if(openmv.update() || sim_openmv_new_data) {  //在实际飞行时需要将 sim_openmv_new_data 变量给删掉<br />32
    Log_Write_OpenMV();<br />33

    if(control_mode != GUIDED)<br />35
        return;<br />36

    int16_t target_body_frame_y = (int16_t)openmv.cx - 80;  // QQVGA 160 * 120<br />38
    int16_t target_body_frame_z = (int16_t)openmv.cy - 60;<br />39

    float angle_y_deg = target_body_frame_y * 60.0f / 160.0f;<br />41
    float angle_z_deg = target_body_frame_z * 60.0f / 120.0f;<br />42

    Vector3f v = Vector3f(1.0f, tanf(radians(angle_y_deg)), tanf(radians(angle_z_deg)));<br />44
    v = v / v.length();<br />45

    const Matrix3f &rotMat = copter.ahrs.get_rotation_body_to_ned();<br />47
    v = rotMat * v;<br />48

    target = v * 10000.0f;  // distance 100m<br />50

    target.z = -target.z;  // ned to neu<br />52

    Vector3f current_pos = inertial_nav.get_position();<br />54
    target = target + current_pos;<br />55

    if(millis() - last_set_pos_target_time_ms > 500) {  // call in 2Hz<br />57
        // wp_nav->set_wp_destination(target, false);<br />58
        mode_guided.set_destination(target, false, 0, true, 0, false);<br />59
        last_set_pos_target_time_ms= millis();<br />60
    }<br />61
}<br />62

}
63

无人机飞控系统1

添加新的外设驱动，OpenMV登场

pragma once
2

include
4

include
5

include
6

define AP_SERIALMANAGER_OPEN_MV_BAUD 115200 //串口波特率定义
6

define AP_SERIALMANAGER_OPENMV_BUFSIZE_RX 64//接受缓存的容量
7

define AP_SERIALMANAGER_OPENMV_BUFSIZE_TX 64//发送缓存容量
8

include “AP_OpenMV.h”
10

if LOGGING_ENABLED == ENABLED
2

endif
4

添加新的外设驱动，OpenMV登场

pragma once2

include 4

include 5

include 6

define AP_SERIALMANAGER_OPEN_MV_BAUD 115200 //串口波特率定义6

define AP_SERIALMANAGER_OPENMV_BUFSIZE_RX 64//接受缓存的容量7

define AP_SERIALMANAGER_OPENMV_BUFSIZE_TX 64//发送缓存容量8

include “AP_OpenMV.h”10

if LOGGING_ENABLED == ENABLED2

endif4

pragma once
2

include
4

include
5

include
6

define AP_SERIALMANAGER_OPEN_MV_BAUD 115200 //串口波特率定义
6

define AP_SERIALMANAGER_OPENMV_BUFSIZE_RX 64//接受缓存的容量
7

define AP_SERIALMANAGER_OPENMV_BUFSIZE_TX 64//发送缓存容量
8

include “AP_OpenMV.h”
10

if LOGGING_ENABLED == ENABLED
2

endif
4