参赛作品《大疆精灵2开源头追与osd抬头显示系统》

查看： 15807
回复： 12

主题

发表于2017-06-17 10:03:24 | 显示全部楼层

1# 电梯直达

【报名阶段需要填写的内容】

1. 参赛者姓名（必填项）：

黄何

2. 单位或学校名称（必填项）：

广西梧州蒙山电信公司

3. 当前职务或职称（必填项）：

工程师

4. 参赛作品的名字（必填项）：

大疆精灵2开源头追与osd抬头显示系统

5. 简要陈述您的idea和作品（必填项）：

大疆精灵2是大疆出的精灵系列第二代四轴飞行器，相比一代，二代采用了全新智能电池设计，电池信息和GPS信息等通过CAN接口通讯。此制作分为两大部分。
第一是针对需要对大疆精灵2系列四轴飞行器的电池信息，GPS信息等通过stm32加can芯片，进行CAN接口解码，叠加到摄像头图像里，方便了解飞行器飞行时的状态，并解码出一个遥控器通道进行云吞俯仰的操作，配合地面遥控器头追，实时控制云吞的俯仰状态，并通过模拟图传实时发送到地面飞手的VR眼镜里，让飞手有一种身临其境的飞行快感。
第二是头追的制作，头追传感器采用ardunio与GY-85九轴模块设计，放置在VR眼镜上部，采集飞手的头部动作，通过遥控器发送信号给四轴飞行器，从而控制飞行器的俯仰，让飞手犹如坐在飞行器机仓般的感觉。

6. 拟用到的立创商城在售物料（必填项）：

stm32f103c8t6 tja1050 at7456 atmega328

7. 拟用到的非立创商城物料或其它补充（必填项）：

GY-85模块一些接插件

【作品正式发表（报名成功后进入设计阶段）需要填写的内容】

一、作品简介

大疆精灵2是大疆出的精灵系列第二代四轴飞行器，发布于2015年，现在大疆公司不断推陈出新，大疆精灵系列已经出到了第4代，大疆精灵2集成度没有后边的几代集成度高，但相对的，留给DIYER们改造的空间也相对较大，本制作就是针对大疆精灵2的can信息进行解码，来扩展改造。此制作分为两大部分。
第一是针对需要对大疆精灵2系列四轴飞行器的电池信息，GPS信息等通过stm32加can芯片，进行CAN接口解码，叠加到摄像头图像里，方便了解飞行器飞行时的状态，并解码出两个遥控器通道进行云吞俯仰的操作，配合地面遥控器头追，实时控制云吞的俯仰状态，并通过模拟图传实时发送到地面飞手的VR眼镜里，让飞手有一种身临其境的飞行快感。
第二是头追的制作，头追传感器采用ardunio与GY-85九轴模块设计，放置在VR眼镜上部，采集飞手的头部动作，通过遥控器发送信号给四轴飞行器，从而控制飞行器摄像头的俯仰与航向，让飞手犹如坐在飞行器机仓般的感觉。

二、系统构架图

OSD系统框图

头追系统框图

三、硬件部分的描述

OSD原理图与成品图

头追系统成品图：

主要原理：

1.osd 抬头显示系统。

大疆精灵2飞控系统，会不断的向CAN总线传送各种飞控状态信息和控制信息，改OSD设计就是解码精灵2的can数据，解析出四轴飞行器的电池信息，GPS信息，飞行高度，和遥控器摇杆数值，各种飞控信息通过stm32的can模块接受并解码，送osd芯片结合摄像头传来的图像信息，进行字符叠加，再通过5.8G视频无线图传模块传送到地面接收。地面通过5.8g视频接收模块接收并显示图像。解码出的遥控器的两个通道的数据，由stm32转换成pwm输出给舵机，从而控制舵机的转向和俯仰，实现挂在舵机上的摄像头随地面遥控信号动作。

2.头追系统。

头追系统主要由一块ardunio 板和GY-85 9轴传感器和两个I2C接口的DAC芯片组成，这里采用了现成的模块搭接。ardinuo 读取9轴传感器的状态和磁力计的数据，进行融合计算，转换成俯仰，横滚，朝向的变化数值，并通过DAC芯片转换成电压给遥控器的两个通道读取，模拟遥控器的电阻器动作，从而实现头部的动作模拟成摇杆的动作，控制飞行器上的舵机转动。这样飞行器上摄像头就随着地面控制人员的头部运动而运动，模拟飞机上上下左右看的感觉。

skylink_osd_p2_V1.0_PCB.zip 原理图和PCB文件（采用kicad软件)

四、材料清单（BOM列表)

stm32f103c8t6 商品编号：C8734

tja1050 商品编号：C112947

at7456 商品编号：C82351

atmega328 商品编号：C14877

mcp4725 商品编号：C61423

GY-85模块 https://item.taobao.com/item.htm?spm=a230r.1.14.36.76bf5236XgcH1&id=35070081530&ns=1&abbucket=5#detail

五、软件部分的描述（选填）

OSD软件流程

头追软件流程

osd 大疆can信息解码代码

uint16_t NazaCanDecoderLib::decode()
{
  uint16_t msgId = NAZA_MESSAGE_NONE;
  if(can.available())
  {
    can.read(&RxMessage);
    if(RxMessage.StdId == 0x090) canMsgIdIdx = 0;
    else if(RxMessage.StdId == 0x108) canMsgIdIdx = 1;
    else if(RxMessage.StdId == 0x7F8) canMsgIdIdx = 2;
    else return msgId; // we don't care about other CAN messages


    for(uint8_t i = 0; i < RxMessage.DLC; i++)
    {
      canMsgByte = RxMessage.Data[i];
      if(collectData[canMsgIdIdx] == 1)
      {
        msgBuf[canMsgIdIdx].bytes[msgIdx[canMsgIdIdx]] = canMsgByte;
        if(msgIdx[canMsgIdIdx] == 3)
        {
          msgLen[canMsgIdIdx] = msgBuf[canMsgIdIdx].header.len;
        }
        msgIdx[canMsgIdIdx] += 1;
        if((msgIdx[canMsgIdIdx] > (msgLen[canMsgIdIdx] + 8)) || (msgIdx[canMsgIdIdx] > 256)) collectData[canMsgIdIdx] = 0;
      }
       
      // Look fo header
      if(canMsgByte == 0x55) { if(header[canMsgIdIdx] == 0) header[canMsgIdIdx] = 1; else if(header[canMsgIdIdx] == 2) header[canMsgIdIdx] = 3; else header[canMsgIdIdx] = 0;}
      else if(canMsgByte == 0xAA) { if(header[canMsgIdIdx] == 1) header[canMsgIdIdx] = 2; else if(header[canMsgIdIdx] == 3) { header[canMsgIdIdx] = 0; collectData[canMsgIdIdx] = 1; msgIdx[canMsgIdIdx] = 0; } else header[canMsgIdIdx] = 0;}
      else header[canMsgIdIdx] = 0;
      
      // Look fo footer
      if(canMsgByte == 0x66) { if(footer[canMsgIdIdx] == 0) footer[canMsgIdIdx] = 1; else if(footer[canMsgIdIdx] == 2) footer[canMsgIdIdx] = 3; else footer[canMsgIdIdx] = 0;}
      else if(canMsgByte == 0xCC) { if(footer[canMsgIdIdx] == 1) footer[canMsgIdIdx] = 2; else if(footer[canMsgIdIdx] == 3) { footer[canMsgIdIdx] = 0; if(collectData[canMsgIdIdx] != 0) collectData[canMsgIdIdx] = 2; } else footer[canMsgIdIdx] = 0;}
      else footer[canMsgIdIdx] = 0;
      
      if(collectData[canMsgIdIdx] == 2)
      {
        if(msgIdx[canMsgIdIdx] == (msgLen[canMsgIdIdx] + 8))
        {
          if(msgBuf[canMsgIdIdx].header.id == NAZA_MESSAGE_MSG1002)
          {
            float magCalX = msgBuf[canMsgIdIdx].msg1002.magCalX;
            float magCalY = msgBuf[canMsgIdIdx].msg1002.magCalY;
            headingNc = -atan2(magCalY, magCalX) / M_PI * 180.0;
            if(headingNc < 0) headingNc += 360.0;
            float headCompX = msgBuf[canMsgIdIdx].msg1002.headCompX;
            float headCompY = msgBuf[canMsgIdIdx].msg1002.headCompY;
            heading = atan2(headCompY, headCompX) / M_PI * 180.0;
            if(heading < 0) heading += 360.0;
            sat = msgBuf[canMsgIdIdx].msg1002.numSat;
            gpsAlt = msgBuf[canMsgIdIdx].msg1002.altGps;
            lat = msgBuf[canMsgIdIdx].msg1002.lat / M_PI * 180.0;
            lon = msgBuf[canMsgIdIdx].msg1002.lon / M_PI * 180.0;
            alt = msgBuf[canMsgIdIdx].msg1002.altBaro;
            float nVel = msgBuf[canMsgIdIdx].msg1002.northVelocity;
            float eVel = msgBuf[canMsgIdIdx].msg1002.eastVelocity;
            spd = sqrt(nVel * nVel + eVel * eVel);
            cog = atan2(eVel, nVel) / M_PI * 180;
            if(cog < 0) cog += 360.0;
            vsi = -msgBuf[canMsgIdIdx].msg1002.downVelocity;
            msgId = NAZA_MESSAGE_MSG1002;
          }
          else if(msgBuf[canMsgIdIdx].header.id == NAZA_MESSAGE_MSG1003)
          {
            uint32_t dateTime = msgBuf[canMsgIdIdx].msg1003.dateTime;
            second = dateTime & 0x3f; dateTime >>= 6;  //0b00111111
            minute = dateTime & 0x3f; dateTime >>= 6;  //0b00111111
            hour = dateTime & 0x0f; dateTime >>= 4;    //0b00001111
            day = dateTime & 0x3f; dateTime >>= 5; if(hour > 7) day++; //0b00011111
            month = dateTime & 0x0f; dateTime >>= 4;  //0b00001111
            year = dateTime & 0x7f;  //0b01111111
            gpsVsi = -msgBuf[canMsgIdIdx].msg1003.downVelocity;
            vdop = (double)msgBuf[canMsgIdIdx].msg1003.vdop / 100;
            double ndop = (double)msgBuf[canMsgIdIdx].msg1003.ndop / 100; 
            double edop = (double)msgBuf[canMsgIdIdx].msg1003.edop / 100;
            hdop = sqrt(ndop * ndop + edop * edop);
            uint8_t fixType = msgBuf[canMsgIdIdx].msg1003.fixType;
            uint8_t fixFlags = msgBuf[canMsgIdIdx].msg1003.fixStatus;
            switch(fixType)
            {
              case 2 : fix = FIX_2D; break;
              case 3 : fix = FIX_3D; break;
              default: fix = NO_FIX; break;
            }
            if((fix != NO_FIX) && (fixFlags & 0x02)) fix = FIX_DGPS;
            msgId = NAZA_MESSAGE_MSG1003;
          }
          else if(msgBuf[canMsgIdIdx].header.id == NAZA_MESSAGE_MSG1009)
          {
            for(uint8_t j = 0; j < 10; j++)
            {
              rcIn[j] = msgBuf[canMsgIdIdx].msg1009.rcIn[j];
            }
#ifndef GET_SMART_BATTERY_DATA
            battery = msgBuf[canMsgIdIdx].msg1009.batVolt;
#endif
           // rollRad = msgBuf[canMsgIdIdx].msg1009.roll;
           // pitchRad = msgBuf[canMsgIdIdx].msg1009.pitch;
						
					 	rollRad = msgBuf[canMsgIdIdx].msg1009.stabRollIn;
						pitchRad = msgBuf[canMsgIdIdx].msg1009.stabPitchIn;
						
						roll = (int8_t)(rollRad*0.1);
					  pitch = (int8_t)(pitchRad*0.1);
												
           // roll = (int8_t)(rollRad * 180.0 / M_PI);
           // pitch = (int8_t)(pitchRad * 180.0 / M_PI);
            mode = (NazaCanDecoderLib::mode_t)msgBuf[canMsgIdIdx].msg1009.flightMode;
            msgId = NAZA_MESSAGE_MSG1009;
          }
#ifdef GET_SMART_BATTERY_DATA
          else if(msgBuf[canMsgIdIdx].header.id == NAZA_MESSAGE_MSG0926)
          {
            battery = msgBuf[canMsgIdIdx].msg0926.voltage;
            batteryPercent = msgBuf[canMsgIdIdx].msg0926.chargePercent;
            for(uint8_t j = 0; j < 3; j++)
            {
              batteryCell[j] = msgBuf[canMsgIdIdx].msg0926.cellVoltage[j];
            }
            msgId = NAZA_MESSAGE_MSG0926;
          }
#endif
        }
        collectData[canMsgIdIdx] = 0;
      }
    }
  }

  return msgId;
}

头追系统融合代码

//--------------------------------------------------------------------------------------
// Func: Filter
// Desc: Filters / merges sensor data. 
//--------------------------------------------------------------------------------------
void FilterSensorData()
{
    int temp = 0;

    // Used to set initial values. 
    if (resetValues == 1)
    {
#if FATSHARK_HT_MODULE
        digitalWrite(BUZZER, HIGH);
#endif
        resetValues = 0; 
      
        tiltStart = 0;
        panStart = 0;
        rollStart = 0;
  
        UpdateSensors();    
        GyroCalc();
        AccelCalc();
        MagCalc();
        
        panAngle = 0;
        tiltStart = accAngle[0];
        panStart = magAngle[2];
        rollStart = accAngle[1];

#if FATSHARK_HT_MODULE
        digitalWrite(BUZZER, LOW);
#endif
    }

    // Simple FilterSensorData, uses mainly gyro-data, but uses accelerometer to compensate for drift
    rollAngle = (rollAngle + ((gyroRaw[0] - gyroOff[0]) * cos((tiltAngle - 90) / 57.3) + (gyroRaw[2] - gyroOff[2]) * sin((tiltAngle - 90) / 57.3)) / (SAMPLERATE * SCALING_FACTOR)) * gyroWeightTiltRoll + accAngle[1] * (1 - gyroWeightTiltRoll);
    tiltAngle = (tiltAngle + ((gyroRaw[1] - gyroOff[1]) * cos((rollAngle - 90) / 57.3) + (gyroRaw[2] - gyroOff[2]) * sin((rollAngle - 90) / 57.3) * -1) / (SAMPLERATE * SCALING_FACTOR)) * gyroWeightTiltRoll + accAngle[0] * (1 - gyroWeightTiltRoll);
    panAngle  = (panAngle + ((gyroRaw[2] - gyroOff[2]) * cos((tiltAngle - 90) / 57.3) + (((gyroRaw[0] - gyroOff[0]) * -1) * (sin((tiltAngle - 90) / 57.3)) ) + ( ((gyroRaw[1] - gyroOff[1]) * 1) * (sin((rollAngle - 90) / 57.3)))) / (SAMPLERATE * SCALING_FACTOR)) * GyroWeightPan + magAngle[2] * (1 - GyroWeightPan);

    if (TrackerStarted)
    {
        // All low-pass filters
        tiltAngleLP = tiltAngle * tiltRollBeta + (1 - tiltRollBeta) * lastTiltAngle;
        lastTiltAngle = tiltAngleLP;
  
        rollAngleLP = rollAngle * tiltRollBeta + (1 - tiltRollBeta) * lastRollAngle;
        lastRollAngle = rollAngleLP;

        panAngleLP = panAngle * panBeta + (1 - panBeta) * lastPanAngle;
        lastPanAngle = panAngleLP;

        float panAngleTemp = panAngleLP * panInverse * panFactor;
        if ( (panAngleTemp > -panMinPulse) && (panAngleTemp < panMaxPulse) )
        {
            temp = servoPanCenter + panAngleTemp;
            channel_value[htChannels[0]] = (int)temp;
        }    

        float tiltAngleTemp = (tiltAngleLP - tiltStart) * tiltInverse * tiltFactor;
        if ( (tiltAngleTemp > -tiltMinPulse) && (tiltAngleTemp < tiltMaxPulse) )
        {
            temp = servoTiltCenter + tiltAngleTemp;
            channel_value[htChannels[1]] = temp;
        }   

        float rollAngleTemp = (rollAngleLP - rollStart) * rollInverse * rollFactor;
        if ( (rollAngleTemp > -rollMinPulse) && (rollAngleTemp < rollMaxPulse) )
        {
            temp = servoRollCenter + rollAngleTemp;
            channel_value[htChannels[2]] = temp;
        }
    }
}

ebox_skylink_osd_p2_fw_v1.0.zip osd源代码

skylink_osd_p2_PC_v1.0.zip osd上位机代码

DIY_Headtracker.zip 头追代码与设计

六、作品演示

七、总结

这个方案是模拟遥控器的摇杆动作的，所以要接线到遥控器的两个通道，这样其实很别扭，下一步是要用433m无线模块替代模拟摇杆，做到真正的免连线头追。

主题

发表于2017-08-22 11:00:16 | 显示全部楼层

11#

发表于2017-08-21 18:21:15 10# 看这效果不错啊，没有太大的晃动，能支持飞行多久呀

15分钟吧

* 姓名：
* 手机号码：
* 报名人数：	报名人数超过一个请在备注中填写其他人手机号码及身份证号码。
备注：

禁言/删除

举报