摄像头是小蚁的（拆开来看到Xbee,紫蜂通讯。直接买的小米VR，不准备做FPV。两块arduino nano,两块蓝牙主从一体模块，GY-85九轴传感器，舵机云台。硬件差不多布线完成了，软件方面参考XXXXXXXXXXXXXXXXXXXXXX/p/4445673292
发现他arduino语句有问题
因为煎饼还处于单片机跑马灯阶段，蓝牙更不懂，所以哪位程序猿帮我看看




头追程序
// Written by Dennis Frie - 2012
// Contact: XXXXXXXXXie@XXXXXXXXX

// Version 0.08

// Discussion:
// XXXXXXXXXXXXXXXXXXXXXXX/forums/XXXXXXXXXXXXXp?t=1677559


/*
Channel mapping/config for PPM out:

1 = PPM CHANNEL 1
2 = PPM CHANNEL 2
3 = PPM CHANNEL 3
4 = PPM CHANNEL 4
5 = PPM CHANNEL 5
6 = PPM CHANNEL 6
7 = PPM CHANNEL 7
8 = PPM CHANNEL 8
9 = PPM CHANNEL 9
10 = PPM CHANNEL 10
11 = PPM CHANNEL 11
12 = PPM CHANNEL 12

20 = ANALOG INPUT 0
21 = ANALOG INPUT 1
22 = ANALOG INPUT 2
23 = ANALOG INPUT 3
24 = ANALOG INPUT 4
25 = ANALOG INPUT 5
26 = ANALOG INPUT 6
27 = ANALOG INPUT 7

20 = DIGITAL INPUT 0
21 = DIGITAL INPUT 1
22 = DIGITAL INPUT 2
23 = DIGITAL INPUT 3
24 = DIGITAL INPUT 4
25 = DIGITAL INPUT 5
26 = DIGITAL INPUT 6
27 = DIGITAL INPUT 7

30 = HT pan
31 = HT tilt
32 = HT roll

Mapping example:
$123456789111CH

*/
#include <Wire.h>

#include "config.h"
#include "functions.h"
#include "sensors.h"
#include "ServoSupport.h"

#include <EEPROM.h>

extern unsigned char channel_mapping[];
extern unsigned char PpmIn_PpmOut[13];
extern char read_sensors;
extern long channel_value[];

extern float gyroOff[];


int counter =0;      // Delay for serial-output.
char printPlot = 0;  // print plot for GUI?

// External variables (from sensors.h):
extern float tiltBeta;
extern float panBeta;
extern float gyroWeight;
extern float GyroWeightPan;
extern int maxPulse;
extern int servoPanCenter;
extern int servoTiltCenter;
extern int servoRollCenter;

extern int panMaxPulse;
extern int panMinPulse;
extern int tiltMaxPulse;
extern int tiltMinPulse;
extern int rollMaxPulse;
extern int rollMinPulse;

extern float panFactor;
extern float tiltFactor;  
extern float rollFactor;  

extern char tiltInverse;
extern char rollInverse;
extern char panInverse;

extern float magOffset[];

extern int accOffset[];

extern unsigned char htChannels[];

extern char resetValues;          

void setup() {
  
  XXXXXXXXXgin(SERIAL_BAUD);

  
  pinMode(7,OUTPUT);
  pinMode(9,OUTPUT);
  digitalWrite(2,HIGH);
  digitalWrite(3,HIGH);  
  
  pinMode(0,INPUT);
  pinMode(1,INPUT);
  pinMode(2,INPUT);
  pinMode(3,INPUT);
  pinMode(6,INPUT);
  pinMode(7,INPUT);  
  pinMode(8,INPUT);    

  //Set button pin to input:
  pinMode(BUTTON_INPUT,INPUT);
  
  //Set internal pull-up resistor.
  digitalWrite(BUTTON_INPUT,HIGH);
  
  digitalWrite(0,LOW); // pull-down resistor
  digitalWrite(1,LOW);
  digitalWrite(2,HIGH);
  digitalWrite(3,HIGH);  
  
  
  XXXXXXXgin();                  // Start I2C


// If the device have just been programmed, write initial config/values to EEPROM:
  if (XXXXXXXXXad(0) != 8)  {
    
//  #if (DEBUG)
     XXXXXXXXXintln("New board - saving default values!");
//  #endif    
    
      init_sensors();
   #if (ALWAYS_CAL_GYRO ==0)    
     setSensorOffset();
   #endif    

    save Settings();
    saveMagData();
    saveAccData();
    EEPROM.write(0,8);
  }
  
  

  getSettings();                 // Get settings saved in EEPROM
  
  init_sensors();                // Initialise I2C sensors
  calMag();
  resetCenter();
  init_timer_interrupt();        // Start timer interrupt (for sensors)  
  
  ServoSupport::Init();
  ServoSupport::SetAng(1,9,90);
  ServoSupport::SetAng(2,10,90);
  ServoSupport::SetAng(3,8,90);
  ServoSupport::Start();
  
}


char serial_data[101];           // Array for serial-data
unsigned char serial_index =0;   // How many bytes have been received?
char string_started = 0;         // Only saves data if string starts with right byte

char outputMag = 0;
char outputAcc = 0;


void loop() {
  
  // Check button
  if (digitalRead(BUTTON_INPUT)==0) {
   resetValues = 1;
  }
  
  
// All this is used for communication with GUI  
  if (Serial.available()) {
    
    if (string_started == 1) {
      
         // Read incoming byte
           serial_data[serial_index++] = XXXXXXXXXad();
          
           // If string ends with 'CH" it's channel configuration, that have been received.
           // String must always be 12 chars/bytes and ending with CH to be valid.
           if (serial_data[serial_index-2] == 'C' && serial_data[serial_index-1] == 'H' && serial_index == 14) {
  
              // To keep it simple, we will not let the channels be 0-initialised, but start from 1 to match actual channels.
               for (unsigned char i=0; i<13; i++) {
                  channel_mapping[i+1] = serial_data[i]-48;
                  
                  // Update the dedicated PPM-in -> PPM-out array for faster performance.
                  if (serial_data[i]-48 < 14) {
//                    PpmIn_PpmOut[i+1] =  serial_data[i]-48;
                    PpmIn_PpmOut[serial_data[i]-48] =i+1;
                  }
                  
               }
              
               XXXXXXXXXintln("Channel mapping received");
              
               // Reset serial_index and serial_started
               serial_index=0;
               string_started = 0;
           }
          
          
          
          if (serial_data[serial_index-2] == 'H' && serial_data[serial_index-1] == 'E') {

          
           // We need 8 parameters:
          
           // LP_tilt
           // LP_pan
           // Gyro_weight_tilt
           // Gyro_weight_pan
           // servo_max
           // servo_min
           // tilt_servo_gain
           // pan_servo_gain
          
           XXXXXXXXXintln("HT config received:");
          
           /*
           for (unsigned char k = 0; k< 10; k++) {
            valuesReceived[k] =0;
           }
           */
           int valuesReceived[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
           int comma_index =0;



           for (unsigned char k = 0; k < serial_index-2; k++) {
            
               // Looking for comma
               if (serial_data[k] == 44) {
                 comma_index++;
               }
            
             else {
             valuesReceived[comma_index] = valuesReceived[comma_index]*10 + (serial_data[k]-48);
             }
            
            
             // Mainly for debug:
             #if (DEBUG)
             XXXXXXXXXint(serial_data[k]);
             #endif
            
          }



          
          #if (DEBUG)
           XXXXXXXXXintln();
            for (unsigned char k = 0; k < comma_index+1; k++) {
               XXXXXXXXXint(valuesReceived[k]);
               XXXXXXXXXint(",");          
            }
           XXXXXXXXXintln();
          #endif

         tiltBeta        = (float)valuesReceived[0] / 100;  
         panBeta         = (float)valuesReceived[1] / 100;
         gyroWeight      = (float)valuesReceived[2] / 100;
         GyroWeightPan   = (float)valuesReceived[3] / 100;

         tiltFactor     = (float)valuesReceived[4] / 10;        
         panFactor     = (float)valuesReceived[5] / 10;          
         rollFactor     = (float)valuesReceived[6] / 10;  
  

          
        tiltInverse = 1;
        rollInverse = 1;
        panInverse = 1;          
          
         if (valuesReceived[7] / 100 == 1) {
             panInverse = -1;
             valuesReceived[7]-=100;
         }  
        
         if (valuesReceived[7] / 10 == 1) {
          rollInverse = -1;
          valuesReceived[7]-=10;
         }

         if (valuesReceived[7] / 1 == 1) {
           tiltInverse = -1;
         }
               serial_index=0;
               string_started = 0;


        
         servoPanCenter = valuesReceived[8];
         panMinPulse = valuesReceived[9];
         panMaxPulse = valuesReceived[10];        
        
         servoTiltCenter = valuesReceived[11];
         tiltMinPulse = valuesReceived[12];
         tiltMaxPulse = valuesReceived[13];        

         servoRollCenter = valuesReceived[14];
         rollMinPulse = valuesReceived[15];
         rollMaxPulse = valuesReceived[16];              
    
         htChannels[0] = valuesReceived[17];                  
         htChannels[1] = valuesReceived[18];              
         htChannels[2] = valuesReceived[19];                      
        
         XXXXXXXXXintln(htChannels[0]);
         XXXXXXXXXintln(htChannels[1]);
         XXXXXXXXXintln(htChannels[2]);                
        
         save Settings();
            
          }
          
          // Debug info
          else if (serial_data[serial_index-5] == 'D' && serial_data[serial_index-4] == 'E' && serial_data[serial_index-3] == 'B' && serial_data[serial_index-2] == 'U' && serial_data[serial_index-1] == 'G') {  
               debugOutput();
              
               serial_index=0;
               string_started = 0;
          }                        
          

          // Start magnetometer cal
          else if (serial_data[serial_index-4] == 'C' && serial_data[serial_index-3] == 'A' && serial_data[serial_index-2] == 'S' && serial_data[serial_index-1] == 'T') {  
               outputMag = 1;
              
               serial_index=0;
               string_started = 0;
          }        
          
          
          // Start accelerometer cal
          else if (serial_data[serial_index-4] == 'G' && serial_data[serial_index-3] == 'R' && serial_data[serial_index-2] == 'A' && serial_data[serial_index-1] == 'V') {  
               outputAcc = 1;
              
               serial_index=0;
               string_started = 0;
          }              

          // Stop magnetometer cal
          else if (serial_data[serial_index-4] == 'C' && serial_data[serial_index-3] == 'A' && serial_data[serial_index-2] == 'E' && serial_data[serial_index-1] == 'N') {  
               outputMag = 0;
              
               serial_index=0;
               string_started = 0;
          }
          
          // Stop accelerometer cal
          else if (serial_data[serial_index-4] == 'G' && serial_data[serial_index-3] == 'R' && serial_data[serial_index-2] == 'E' && serial_data[serial_index-1] == 'N') {  
               outputAcc = 0;
              
               serial_index=0;
               string_started = 0;
          }

          

          
          // Start plotting if PLST received:
          else if (serial_data[serial_index-4] == 'P' && serial_data[serial_index-3] == 'L' && serial_data[serial_index-2] == 'S' && serial_data[serial_index-1] == 'T') {  
               printPlot = 1;
              
               serial_index=0;
               string_started = 0;
          }        

          // Stop plotting if PLEN received:          
          else if (serial_data[serial_index-4] == 'P' && serial_data[serial_index-3] == 'L' && serial_data[serial_index-2] == 'E' && serial_data[serial_index-1] == 'N') {  
               printPlot = 0;
              
               serial_index=0;
               string_started = 0;
          }
          
          // Save settings
          else if (serial_data[serial_index-4] == 'S' && serial_data[serial_index-3] == 'A' && serial_data[serial_index-2] == 'V' && serial_data[serial_index-1] == 'E') {  
              save Settings();
              
               serial_index=0;
               string_started = 0;
          }          
          
          //Calibrate gyro
          else if (serial_data[serial_index-4] == 'C' && serial_data[serial_index-3] == 'A' && serial_data[serial_index-2] == 'L' && serial_data[serial_index-1] == 'I') {  
               setSensorOffset();
               saveSsttings();
              
                XXXXXXXXXint("Gyro offset measured:");
                XXXXXXXXXint(gyroOff[0]);
                XXXXXXXXXint(",");  
                XXXXXXXXXint(gyroOff[1]);
                XXXXXXXXXint(",");      
                XXXXXXXXXintln(gyroOff[2]);    
              
               serial_index=0;
               string_started = 0;
          }


            else if (serial_data[serial_index-2] == 'M' && serial_data[serial_index-1] == 'A') {
              XXXXXXXXXintln(serial_data);
              int valuesReceived[5] = {0,0,0,0,0};
              int comma_index =0;
              
           for (unsigned char k = 0; k < serial_index-2; k++) {
                
                   // Looking for comma
                   if (serial_data[k] == 44) {
                     comma_index++;
                   }
                
                 else {
                 valuesReceived[comma_index] = valuesReceived[comma_index]*10 + (serial_data[k]-48);
                 }              
                  
                  
                  
                }
                

                  magOffset[0] = valuesReceived[0]-2000;
                  magOffset[1] = valuesReceived[2]-2000;
                  magOffset[2] = valuesReceived[1]-2000;
                  
                  saveMagData();                
            }
            
            
            else if (serial_data[serial_index-3] == 'A' && serial_data[serial_index-2] == 'C' && serial_data[serial_index-1] == 'C') {
              XXXXXXXXXintln(serial_data);
              int valuesReceived[5] = {0,0,0,0,0};
              int comma_index =0;
              
           for (unsigned char k = 0; k < serial_index-3; k++) {
                
                   // Looking for comma
                   if (serial_data[k] == 44) {
                     comma_index++;
                   }
                
                 else {
                 valuesReceived[comma_index] = valuesReceived[comma_index]*10 + (serial_data[k]-48);
                 }              
                  
                  
                  
                }

                  accOffset[0] = valuesReceived[0]-2000;
                  accOffset[1] = valuesReceived[1]-2000;
                  accOffset[2] = valuesReceived[2]-2000;
                  
                  saveAccData();                
            }            
            
          
  
         // If more than 100 bytes have been received, the string is not valid. Reset and "try again" (wait for $ to indicate start of new string).
           else if (serial_index > 100) {
               serial_index = 0;
               string_started = 0;
           }      
          
     }
    
    else if (XXXXXXXXXad() == '$') {
        string_started = 1;
    }
    

    
  }
  
  // if "read_sensors" flag is set high, read sensors and update
  if (read_sensors == 1) {
    
        updateSensors();
    
        gyroCalc();
        accCalc();
        magCalc();
        filter();    
              
        if (counter++ >7) {
          
//          testGyroOutput();
       //   testMagOutput();
//           quickTest();
          
          if (printPlot == 1) {
             headtrackerOutput();
                  
          }
        
          else if (outputMag) {
             calMagOutput();
          }
          
          else if (outputAcc) {
            calAccOutput();
          }
        
            counter=0;
        }
    
    
    // Will first update read_sensors when everything's done.  
        read_sensors = 0;
        
    }

  
}

void saveSettings() {
  
// This is the necessary head-tracler settings:
/*
         tiltBeta        = (float)valuesReceived[0] / 100;  
         panBeta         = (float)valuesReceived[1] / 100;
         gyroWeight      = (float)valuesReceived[2] / 100;
         GyroWeightPan   = (float)valuesReceived[3] / 100;  
         maxPulse        = valuesReceived[4];
         servoCenter     = valuesReceived[5];        
         tiltFactor     = (float)valuesReceived[6] / 10;        
         panFactor     = (float)valuesReceived[7] / 10;  
*/        
  
  // Chars
  EEPROM.write(1,(unsigned char) (tiltBeta*100));
  EEPROM.write(2,(unsigned char) (panBeta*100));
  EEPROM.write(3,(unsigned char) (gyroWeight*100));
  EEPROM.write(4,(unsigned char) (GyroWeightPan*100));
  
  // And some integers
  EEPROM.write(5,(unsigned char)maxPulse);
  EEPROM.write(6,(unsigned char)(maxPulse>>8));
  
  EEPROM.write(7,(unsigned char)servoPanCenter);
  EEPROM.write(8,(unsigned char)(servoPanCenter>>8));  
  
  EEPROM.write(9,(unsigned char) (tiltFactor*10));
  EEPROM.write(10,(int)((tiltFactor*10))>>8);  

  EEPROM.write(11,(unsigned char) (panFactor*10));
  EEPROM.write(12,(int)((panFactor*10))>>8);  

  // Channel inverse settings:
  EEPROM.write(13,(unsigned char)(tiltInverse+1));
  EEPROM.write(14,(unsigned char)(rollInverse+1));    
  EEPROM.write(15,(unsigned char)(panInverse+1));  

  EEPROM.write(16,(unsigned char)servoTiltCenter);
  EEPROM.write(17,(unsigned char)(servoTiltCenter>>8));  

  EEPROM.write(18,(unsigned char)servoRollCenter);
  EEPROM.write(19,(unsigned char)(servoRollCenter>>8));  


  EEPROM.write(20,(unsigned char)panMaxPulse);
  EEPROM.write(21,(unsigned char)(panMaxPulse>>8));  
  
  EEPROM.write(22,(unsigned char)panMinPulse);
  EEPROM.write(23,(unsigned char)(panMinPulse>>8));    

  EEPROM.write(24,(unsigned char)tiltMaxPulse);
  EEPROM.write(25,(unsigned char)(tiltMaxPulse>>8));    

  EEPROM.write(26,(unsigned char)tiltMinPulse);
  EEPROM.write(27,(unsigned char)(tiltMinPulse>>8));    


  EEPROM.write(28,(unsigned char)rollMaxPulse);
  EEPROM.write(29,(unsigned char)(rollMaxPulse>>8));    

  EEPROM.write(30,(unsigned char)rollMinPulse);
  EEPROM.write(31,(unsigned char)(rollMinPulse>>8));
  
  
  EEPROM.write(32,(unsigned char)htChannels[0]);
  EEPROM.write(33,(unsigned char)htChannels[1]);
  EEPROM.write(34,(unsigned char)htChannels[2]);
  
  // Saving gyro calibration values
  int temp = (int)(gyroOff[0]+500.5);
  EEPROM.write(35,(unsigned char)temp);
  EEPROM.write(36,(unsigned char)(temp>>8));  
  
  temp = (int)(gyroOff[1]+500.5);
  EEPROM.write(37,(unsigned char)temp);
  EEPROM.write(38,(unsigned char)(temp>>8));    

  temp = (int)(gyroOff[2]+500.5);
  EEPROM.write(39,(unsigned char)temp);
  EEPROM.write(40,(unsigned char)(temp>>8));    
  
  XXXXXXXXXintln("Settings saved!");
}


void getSettings() {
  
  tiltBeta = (float)XXXXXXXXXad(1)/100;
  panBeta = (float)XXXXXXXXXad(2)/100;
  gyroWeight = (float)XXXXXXXXXad(3)/100;
  GyroWeightPan = (float)XXXXXXXXXad(4)/100;  
  
  maxPulse = XXXXXXXXXad(5)+(XXXXXXXXXad(6)<<8);
  
  servoPanCenter = XXXXXXXXXad(7)+(XXXXXXXXXad(8)<<8);
  
  tiltFactor = (float)(XXXXXXXXXad(9)+(XXXXXXXXXad(10)<<8)) / 10;
  
  panFactor = (float)(XXXXXXXXXad(11)+(XXXXXXXXXad(12)<<8)) / 10;  


  servoTiltCenter = XXXXXXXXXad(16)+(XXXXXXXXXad(17)<<8);
  servoRollCenter = XXXXXXXXXad(18)+(XXXXXXXXXad(19)<<8);  
  
  panMaxPulse = XXXXXXXXXad(20)+(XXXXXXXXXad(21)<<8);  
  panMinPulse = XXXXXXXXXad(22)+(XXXXXXXXXad(23)<<8);    
  
  tiltMaxPulse = XXXXXXXXXad(24)+(XXXXXXXXXad(25)<<8);  
  tiltMinPulse = XXXXXXXXXad(26)+(XXXXXXXXXad(27)<<8);      
  
  rollMaxPulse = XXXXXXXXXad(28)+(XXXXXXXXXad(29)<<8);  
  rollMinPulse = XXXXXXXXXad(30)+(XXXXXXXXXad(31)<<8);        
  
  htChannels[0] = XXXXXXXXXad(32);  
  htChannels[1] = XXXXXXXXXad(33);  
  htChannels[2] = XXXXXXXXXad(34);    
  
  gyroOff[0] = XXXXXXXXXad(35)+(XXXXXXXXXad(36)<<8)-500;
  gyroOff[1] = XXXXXXXXXad(37)+(XXXXXXXXXad(38)<<8)-500;
  gyroOff[2] = XXXXXXXXXad(39)+(XXXXXXXXXad(40)<<8)-500;  
  
  
  tiltInverse = -1;
  rollInverse = -1;
  panInverse = -1;
  
  if (XXXXXXXXXad(13) == 2) {
      tiltInverse = 1;
  }  

  if (XXXXXXXXXad(14) == 2) {
      rollInverse = 1;
  }  
  
  if (XXXXXXXXXad(15) == 2) {
      panInverse = 1;
  }


  magOffset[0] = XXXXXXXXXad(200)+(XXXXXXXXXad(201)<<8)-2000;    
  magOffset[1] = XXXXXXXXXad(202)+(XXXXXXXXXad(203)<<8)-2000;    
  magOffset[2] = XXXXXXXXXad(204)+(XXXXXXXXXad(205)<<8)-2000;      
  
  accOffset[0] = XXXXXXXXXad(206)+(XXXXXXXXXad(207)<<8)-2000;    
  accOffset[1] = XXXXXXXXXad(208)+(XXXXXXXXXad(209)<<8)-2000;    
  accOffset[2] = XXXXXXXXXad(210)+(XXXXXXXXXad(211)<<8)-2000;      
  

#if (DEBUG)    

debugOutput();

#endif

}


void saveMagData() {
  int temp;
    temp = (int)(magOffset[0]+2000);
  EEPROM.write(200,(unsigned char)temp);
  EEPROM.write(201,(unsigned char)(temp>>8));  
  
    temp = (int)(magOffset[1]+2000);
  EEPROM.write(202,(unsigned char)temp);
  EEPROM.write(203,(unsigned char)(temp>>8));  
  
    temp = (int)(magOffset[2]+2000);
  EEPROM.write(204,(unsigned char)temp);
  EEPROM.write(205,(unsigned char)(temp>>8));  
  
  XXXXXXXXXintln("Mag-offset saved!");
  
}



void saveAccData() {
  int temp;
    temp = (int)(accOffset[0]+2000);
  EEPROM.write(206,(unsigned char)temp);
  EEPROM.write(207,(unsigned char)(temp>>8));  
  
    temp = (int)(accOffset[1]+2000);
  EEPROM.write(208,(unsigned char)temp);
  EEPROM.write(209,(unsigned char)(temp>>8));  
  
    temp = (int)(accOffset[2]+2000);
  EEPROM.write(210,(unsigned char)temp);
  EEPROM.write(211,(unsigned char)(temp>>8));  
  
  XXXXXXXXXintln("Acc-offset saved!");
  XXXXXXXXXint(accOffset[0]);
  XXXXXXXXXint(",");
  XXXXXXXXXint(accOffset[1]);
  XXXXXXXXXint(",");  
  XXXXXXXXXintln(accOffset[2]);
  
}



void debugOutput() {
  
XXXXXXXXXintln();  
XXXXXXXXXintln();
XXXXXXXXXintln();
XXXXXXXXXintln("------ Debug info------");
  
XXXXXXXXXint("tiltBeta: ");
XXXXXXXXXintln(tiltBeta);

XXXXXXXXXint("panBeta: ");
XXXXXXXXXintln(panBeta);

XXXXXXXXXint("gyroWeight: ");
XXXXXXXXXintln(gyroWeight);

XXXXXXXXXint("GyroWeightPan: ");
XXXXXXXXXintln(GyroWeightPan);

XXXXXXXXXint("servoPanCenter: ");
XXXXXXXXXintln(servoPanCenter);

XXXXXXXXXint("servoTiltCenter: ");
XXXXXXXXXintln(servoTiltCenter);

XXXXXXXXXint("servoRollCenter: ");
XXXXXXXXXintln(servoRollCenter);

XXXXXXXXXint("tiltFactor: ");
XXXXXXXXXintln(tiltFactor);

XXXXXXXXXint("panFactor: ");
XXXXXXXXXintln(panFactor);  

XXXXXXXXXint("Gyro offset saved ");
XXXXXXXXXint(gyroOff[0]);
XXXXXXXXXint(",");  
XXXXXXXXXint(gyroOff[1]);
XXXXXXXXXint(",");      
XXXXXXXXXintln(gyroOff[2]);    

XXXXXXXXXint("Mag offset saved ");
XXXXXXXXXint(magOffset[0]);
XXXXXXXXXint(",");  
XXXXXXXXXint(magOffset[1]);
XXXXXXXXXint(",");      
XXXXXXXXXintln(magOffset[2]);

XXXXXXXXXint("Acc offset saved ");
XXXXXXXXXint(accOffset[0]);
XXXXXXXXXint(",");  
XXXXXXXXXint(accOffset[1]);
XXXXXXXXXint(",");      
XXXXXXXXXintln(accOffset[2]);


SensorInfoPrint();    


}
接收机程序
//this programm will put out a PPM signal

//////////////////////CONFIGURATION///////////////////////////////
#define chanel_number 9  //set the number of chanels
#define default_servo_value 1500  //set the default servo value
#define PPM_FrLen 22500  //set the PPM frame length in microseconds (1ms = 1000μs)
#define PPM_PulseLen 300  //set the pulse length
#define onState 1  //set polarity of the pulses: 1 is positive, 0 is negative
#define sigPin 10  //set PPM signal output pin on the arduino
//////////////////////////////////////////////////////////////////


/*this array holds the servo values for the ppm signal
change theese values in your code (usually servo values move between 1000 and 2000)*/
int ppm[chanel_number];
unsigned int angs,ang1,ang2;
char xx;


void setup(){  
  
  XXXXXXXXXgin(9600);
  //initiallize default ppm values
  for(int i=0; i<chanel_number; i++){
    ppm[i]= default_servo_value;
  }

  pinMode(sigPin, OUTPUT);
  digitalWrite(sigPin, !onState);  //set the PPM signal pin to the default state (off)
  
  cli();
  TCCR1A = 0; // set entire TCCR1 register to 0
  TCCR1B = 0;
  
  OCR1A = 100;  // compare match register, change this
  TCCR1B |= (1 << WGM12);  // turn on CTC mode
  TCCR1B |= (1 << CS11);  // 8 prescaler: 0,5 microseconds at 16mhz
  TIMSK1 |= (1 << OCIE1A); // enable timer compare interrupt
  sei();
}

void loop(){
  //put main code here

  if (Serial.available()) {
    
    angs=XXXXXXXXXrseInt();  //angs=ang1*180+ang2
  ppm[1] = angs/180;
  ppm[1] = map(ppm[1],1,179,1000,2000);
  ppm[2] = angs%180;
  ppm[2] = map(ppm[2],1,179,1000,2000);
  }
  //ang1=angs/180; ang2=angs%180;
xx++;
if(50<=xx)
{
  ppm[0] = analogRead(A0);
  ppm[0] = map(ppm[0],0,1023,1000,2000);
  xx=0;
}
  delayMicroseconds(200);
}

ISR(TIMER1_COMPA_vect){  //leave this alone
  static boolean state = true;
  
  TCNT1 = 0;
  
  if(state) {  //start pulse
    digitalWrite(sigPin, onState);
    OCR1A = PPM_PulseLen * 2;
    state = false;
  }
  else{  //end pulse and calculate when to start the next pulse
    static byte cur_chan_numb;
    static unsigned int calc_rest;
  
    digitalWrite(sigPin, !onState);
    state = true;

    if(cur_chan_numb >= chanel_number){
      cur_chan_numb = 0;
      calc_rest = calc_rest + PPM_PulseLen;//
      OCR1A = (PPM_FrLen - calc_rest) * 2;
      calc_rest = 0;
    }
    else{
      OCR1A = (ppm[cur_chan_numb] - PPM_PulseLen) * 2;
      calc_rest = calc_rest + ppm[cur_chan_numb];
      cur_chan_numb++;
    }    
  }
}
原概念是位老外Dennis Fire提出的，他的程序只写了头追的传感部分，输出的信号通过无人机的遥控器教练功能发射，不知道如何转蓝牙