四轴飞行器程序下载

来自Microduino Wikipedia
跳转至： 导航、 搜索 
#include "mQuad.h"
#include <Microduino_Protocol.h>
#include <stdlib.h>
#include <EEPROM.h>
#define EEPROM_ADDR 0
byte eeprom_data = 0;

#include <RF24Network.h>
RF24 radio(D9, D10);
RF24Network network(radio);

const uint16_t this_node = 00;
const uint16_t other_node = 01;


#define CHANNEL_NUM 8


struct receive_a {
  uint32_t ms;
  uint16_t rf_CH[CHANNEL_NUM];
};
struct send_a { //鍙戦��
  uint32_t node_ms;   //鑺傜偣杩愯鏃堕棿
};


//ProtocolSer protocol(&Serial, 16);  //閲囩敤ProSerial锛屾暟鎹暱搴︿负16涓瓧鑺�

uint8_t recCmd;
uint16_t Data[8];
uint16_t dataBuf[4] = {1000, 1000, 1000, 1000};
uint32_t spiTimer;
uint8_t spiCmd;
uint8_t spiCmdData;
uint8_t spiFlag = 0;
bool ITflag = 0;


//新添加的变量
enum state_t {
  state_init = 0, state_check_matched,  state_public_channel_init,  state_public_channel,  state_priviate_channel_init, state_priviate_channel, state_rotate
};

#define PRIVIATE_CHN_MIN   10         //随机生成通道范围下限
#define PRIVIATE_CHN_MAX   50       //随机生成通道范围上限
#define LOST_TIM_MAX      1500
#define PUBLIC_CHANNEL     0

struct payload_public_t {                  // 数据的结构体
  int priviate_chn;
};

int _state = state_init;
int priviate_channel = 0;
int rotate_flag = 0;
int unlock_cnt = 0; 
unsigned long now_1, now_2, now_3, now_4, tim_1, tim_2 = 0;
;
void setup() {
  // initialize both serial ports:
  digitalWrite(QUAD_CS, HIGH);
  QuadBegin();
  delay(100);
  Serial.begin(9600);  //9600/19200/38400
  Serial.println("RF24Network/examples/helloworld_rx/");
  spiTimer = millis();
  _state = state_init;
}

void loop()
{
 /*
  if(!(radio.isPVariant()))
{
    // initialize both serial ports:
  QuadBegin();
  delay(100);
  Serial.begin(9600);  //9600/19200/38400
  //spiTimer = millis();
  //_state = state_init;   
  }
  */
  //Get_SoftDeviceVersion();
  //先从遥控器接收
  pinMode(QUAD_CS, OUTPUT);
  digitalWrite(QUAD_CS, HIGH);

  switch (_state) {
    case state_init: {
        _state = state_check_matched;
        break;
      }
      
    case state_check_matched: {
        eeprom_data = EEPROM.read(EEPROM_ADDR);
        if (eeprom_data == 0)
        {
          _state = state_public_channel_init;
          rotate_flag = 0;
        }
        else
          _state = state_rotate;
        break;
      }

    case state_public_channel_init: {
        radio.begin();
        radio.setDataRate(RF24_250KBPS);
        network.begin(/*channel*/ PUBLIC_CHANNEL, /*node address*/ this_node);
        Serial.println("state_public_channel_init success");
        now_1 = millis();
        _state = state_public_channel;
        break;
      }

    //公共通道配对，并接收priviate_chn信息
    case state_public_channel: {
        network.update();
        while (network.available()) {
          now_1 = millis();
          RF24NetworkHeader header;
          payload_public_t payload_public;
          network.read(header, &payload_public, sizeof(payload_public)); //读取数据
          priviate_channel = payload_public.priviate_chn;
          EEPROM.write(EEPROM_ADDR, priviate_channel); //收到了新的 就写进去
          Serial.println("state_public_channel match success");
          Serial.print("The new channel is ");
          Serial.println(priviate_channel);
          _state = state_priviate_channel_init;
        }
        now_2 = millis();
        if (now_2 - now_1 > LOST_TIM_MAX && rotate_flag == 1)
          _state = state_priviate_channel_init;
        else if (now_2 - now_1 > LOST_TIM_MAX)
          _state = state_public_channel_init;
        break;
      }

    //设置新的通道
    case state_priviate_channel_init: {
        priviate_channel = EEPROM.read(EEPROM_ADDR);
        radio.begin();
        radio.setDataRate(RF24_250KBPS);
        network.begin(/*channel*/ priviate_channel, /*node address*/ this_node);
        Serial.println("state_priviate_channel_init success");
        Serial.print("The priviate_channel is : ");
        Serial.print(priviate_channel);
        _state = state_priviate_channel;
        now_3 = millis();
        break;
      }

    //新通道配对，并接收新通道的信息
    case state_priviate_channel: {
        network.update();
        if (network.available())
        {
          now_3 = millis();
          RF24NetworkHeader header;
          receive_a rec;
          network.read(header, &rec, sizeof(rec));
          for (int a = 0; a < CHANNEL_NUM; a++) {
            Data[a] = rec.rf_CH[a];
          }
          Data[0] = Data[0] ;
          Data[1] = Data[1] ;

          //protocol.readWords(&recCmd, Data, 8);    
          spiFlag = 1;                               //从joypad收到了数据
          Data[0] = map(Data[0], 1000, 2000, 2000, 1000);
          Data[1] = map(Data[1], 1000, 2000, 2000, 1000);
          RF24NetworkHeader talker(other_node);
            
          network.write(talker,  Data_to_Rc, sizeof(Data_to_Rc));
        } 
          //后向四轴发送
          if (millis() - spiTimer > 10)
          {
            pinMode(QUAD_CS, OUTPUT);
            digitalWrite(QUAD_CS, LOW);
            pinMode(NRF_CS, OUTPUT);
            digitalWrite(NRF_CS, HIGH);
            spiTimer = millis();
            tim_1 = millis();
            if (spiFlag == 0) {
              spiCmd = CMD_NONE;
            }
            else if (Data[4] != dataBuf[0]) {
              dataBuf[0] = Data[4];
              spiCmd = CMD_AUTO_FLY_UP;
            }
            else if (Data[5] != dataBuf[1]) {
              dataBuf[1] = Data[5];
              spiCmd = CMD_AUTO_LANDING;
            }
            else if (Data[6] != dataBuf[2]) {
              dataBuf[2] = Data[6];
              spiCmd = CMD_ACC_CLI;
            }
            //解锁
            
            else if (Data[0] < 1300 && Data[1] > 1600 && Data[2] < 1300 && Data[3] < 1300 && Data[7] == 2000 && unlock_cnt++ > 25 && (tim_1 - tim_2) > 4500 ) {
              tim_2 = millis();              
              spiCmd = CMD_ARM_IT_STICK;
              ITflag = 1;
              spiCmdData = ITflag;
              unlock_cnt = 0;
            }
             //上锁
            else if (Data[0] > 1600 && Data[1] > 1600 && Data[2] > 1600 && Data[3] < 1300 ) {
              spiCmd = CMD_ARM_IT_STICK;
              ITflag = 0;
              spiCmdData = ITflag;
            }
            
            else if ( Data[7] == 1000 ) {
              spiCmd = CMD_ARM_IT_SWITCH;
              ITflag = 0;
              spiCmdData = ITflag;
            }

            else {
              spiCmd = CMD_SET_CH4;
            }

            spiFlag = 0;
            QuadWrite(spiCmd, spiCmdData, Data);
                                        
          }
        
        now_4 = millis();
        if (now_4 - now_3 > LOST_TIM_MAX)
          _state = state_rotate;
        break;
      }

    case state_rotate: {
        rotate_flag = 1;
        _state = state_public_channel_init;
        
      }
    default:
      return 0;

  }
}

头文件"mQuad.h" 

//arduino 与 stm32 通过spi通信   arduino 作为主机  stm32作为从机     arduino 与 蓝牙 通过串口通信  arduino 与 nrf 通过spi通信
#include <SPI.h>

#define QUAD_CS   D8                       // 给digitalWrite()函数使用，所以是D8引脚  片选信号 可以随便指定
#define NRF_CS   D10

#define CMD_NONE                  0x00
#define CMD_SET_CH4               0x01           //对应 RAW PITCH YAW THROTTLE四个通道
#define CMD_ARM_IT_SWITCH         0x02           //使用总开关控制加锁、解锁 
#define CMD_HEAD_IT               0x03           //有头模式 无头模式   
#define CMD_ACC_CLI               0x04           //校准 
#define CMD_AUTO_FLY_UP           0x05           //自动起飞
#define CMD_AUTO_LANDING          0x07           //自动降落
#define CMD_PID_SET               0x08
#define CMD_ARM_IT_STICK          0x09           //使用摇杆控制加锁、解锁


#define STEP_WAIT_AA      1
#define STEP_WAIT_BB      2
#define STEP_WAIT_LEN     3
#define STEP_WAIT_DATA    4
#define STEP_WAIT_BCC     5
#define DATA_TX_LEN       10        //发送数据的数据位长度
#define DATA_LENGTH       16        //接收数据的数据位长度
#define TOTAL_LENGTH      21        //接收数据的总长度

enum State {
  WAIT_AA = 0, WAIT_BB, WAIT_FUNC, WAIT_LEN, WAIT_DATA, WAIT_CRC
} ;
int state = 0;

int data_len_cnt = 0;  

uint8_t rxBuffer[100];                //用于存储spi收到的数据
uint8_t rxData[TOTAL_LENGTH] = {};    //用于存储从rxBuffer整理后的数据
uint8_t Data_to_Rc[TOTAL_LENGTH - 2];     //用于发送给遥控器的数据  因为添加了2字节的软件版本数据，但遥控器不需要
uint8_t SoftVersion[2] = {};              //用于存储表示软件版本的2byte数据

typedef union _byteToint {
  int16_t twobyte;
  uint8_t Twobyte[2];
} _byteToint;

void Data_Parse(uint8_t data)
{
  static int data_len_cnt = 0;
  switch (state)
  {
    case WAIT_AA:
      if (data == 0xAA)
      {
        rxData[0] = data;
        state = WAIT_BB;
      }
      else
        state = WAIT_AA;
      break;
    case WAIT_BB:
      if (data == 0xBB)
      {
        rxData[1] = data;
        state = WAIT_FUNC;
      }
      else
        state = WAIT_BB;
      break;
    case WAIT_FUNC:
      if (data == 0x00)
      {
        rxData[2] = data;
        state = WAIT_LEN;
      }
      else
        state = WAIT_AA;
      break;

    case WAIT_LEN:
      if (data == DATA_LENGTH)
      {
        rxData[3] = DATA_LENGTH;
        state = WAIT_DATA;
      }
      else
        state = WAIT_AA;
      break;
    case WAIT_DATA:
      rxData[4 + data_len_cnt++] = data;
      if (data_len_cnt > DATA_LENGTH - 1)
      {
        data_len_cnt = 0;
        state = WAIT_CRC;
      }
      else
        state = WAIT_DATA;
      break;
    case WAIT_CRC:
      rxData[TOTAL_LENGTH - 1] = data;

      //添加了2byte的软件版本，但不需要发给遥控器
      memcpy(Data_to_Rc, rxData, TOTAL_LENGTH-3);
      Data_to_Rc[TOTAL_LENGTH - 3] = rxData[TOTAL_LENGTH - 1];
      SoftVersion[1] = rxData[TOTAL_LENGTH - 3];
      SoftVersion[0] = rxData[TOTAL_LENGTH - 2];       
      state = WAIT_AA;
      break;
  }
}


void QuadBegin() {
  SPI.begin();
  pinMode(QUAD_CS, OUTPUT);
  digitalWrite(QUAD_CS, HIGH);  
}

uint8_t QuadWrite(uint8_t cmd1, uint8_t cmd2, uint16_t *data) {  
  uint8_t rxResult = 0;
  uint8_t txData[14];
  txData[0] = 0xAA;
  txData[1] = 0XBB;
  txData[2] = DATA_TX_LEN;
  txData[13] = DATA_TX_LEN;
  txData[3] = cmd1;
  txData[4] = cmd2;
 
  memcpy(txData + 5, (uint8_t *)data, 8);
    
  for (uint8_t i = 3; i < 13; i++)
    txData[13] ^= txData[i];
    
  digitalWrite(QUAD_CS, LOW);
  for (uint8_t i = 0; i < 14; i++) 
  {    
   rxBuffer[i] = SPI.transfer(txData[i]);
   //Serial.print(txData[i]);
   Data_Parse(rxBuffer[i]);
  }
  digitalWrite(QUAD_CS, HIGH);  
}

void Get_SoftDeviceVersion(void)
{
  _byteToint Version;
  Version.Twobyte[1] = SoftVersion[1];
  Version.Twobyte[0] = SoftVersion[0];

  Serial.print("四轴软件版本号为： ");
  Serial.println(Version.twobyte/10.0);   
}
取自“http:///https//wiki.microduino.cn/index.php?title=四轴飞行器程序下载&oldid=27387”