#SecondLichuangcompetition# DJI Phantom 2 open source head tracking and osd head-up display system

introduction:

DJI Phantom 2 is the second generation quadcopter of the Phantom series produced by DJI. Compared with the first generation, the second generation adopts a new smart battery design. Battery information and GPS information are communicated through the CAN interface. This production is divided into two parts. The first is to decode the battery information, GPS information, etc. of the DJI Phantom 2 series quadcopter through stm32 plus can chip, decode the CAN interface, and superimpose it into the camera image, so as to facilitate understanding of the status of the aircraft during flight, and decode a The remote control channel performs the pitching operation of the wonton, and cooperates with the ground remote control head tracking to control the pitching state of the wonton in real time, and sends it to the ground pilot's VR glasses in real time through simulated image transmission, giving the pilot an immersive experience. The thrill of flying. The second is the production of head tracking. The head tracking sensor is designed using ardunio and GY-85 nine-axis module. It is placed on the top of the VR glasses, collects the head movements of the pilot, and sends signals to the quadcopter through the remote controller to control the aircraft. Pitching makes the pilot feel like he is sitting in the cabin of an aircraft.      

2. System architecture diagram

OSD system block diagram   Head chasing system block diagram  

3. Description of the hardware part

OSD schematic diagram and finished product diagram     Head tracking system finished picture:   Main principles: 1.osd head-up display system. The DJI Phantom 2 flight control system will continuously transmit various flight control status information and control information to the CAN bus. The OSD design is to decode the CAN data of the Phantom 2 and parse out the battery information, GPS information, and flight altitude of the quadcopter. , and the remote control joystick value, various flight control information is received and decoded through the STM32 can module, and sent to the OSD chip to combine the image information from the camera for character superposition, and then transmit it to the ground for reception through the 5.8G video wireless image transmission module . The ground receives and displays images through a 5.8g video receiving module. The decoded data of the two channels of the remote control is converted into pwm by stm32 and output to the steering gear, thereby controlling the steering and pitch of the steering gear, and realizing that the camera hanging on the steering gear moves according to the ground remote control signal. 2. Head chasing system. The head tracking system mainly consists of an ardunio board, a GY-85 9-axis sensor and two I2C interface DAC chips. Ready-made modules are used here. ardinuo reads the status of the 9-axis sensor and the data of the magnetometer, performs fusion calculations, converts them into changes in pitch, roll, and orientation, and converts them into voltages through the DAC chip to be read by the two channels of the remote control, simulating the remote control. The action of the resistor allows the movement of the head to be simulated as the movement of a rocker to control the rotation of the servo on the aircraft. In this way, the camera on the aircraft moves with the head movement of the ground controller, simulating the feeling of looking up, down, left, and right on the aircraft.

4. Material list (BOM list)

stm32f103c8t6 Product number: C8734 tja1050 Product number: C112947 at7456 Product number: C82351 atmega328 Product number: C14877 mcp4725 Product number: C61423 GY-85 module  https://item.taobao.com/item.htm?spm=a230r.1.14 .36.76 bf5236XgcH1&id=35070081530&ns=1&abbucket=5#detail

skylink_osd_p2_V1.0_PCB.zip   schematic diagram and PCB file (using kicad software)

5. Description of the software part

OSD software process   head chasing software process   osd DJI can information decoding code

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;
}

Head pursuit system fusion code

//--------------------------------------------------------------------------------------
// 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 source code skylink_osd_p2_PC_v1.0.zip   osd host computer code DIY_Headtracker.zip   head tracking code and design

6. Work demonstration

Video 1: https://v.qq.com/x/page/w05395c58zm.html?start=3 Video 2:  https://v.qq.com/x/page/o0540rby11f.html

7. Summary

This solution simulates the joystick action of the remote control, so it needs to be wired to the two channels of the remote control. This is actually very awkward. The next step is to use a 433m wireless module to replace the analog joystick to achieve true wire-free head tracking. .

For more project details, please see the link: http://club.szlcsc.com/article/details_7034_1.html

This project is owned by Lichuang Community " diylink "