基于Arduino的四轴飞行器.docx

1、基于Arduino的四轴飞行器第2章 基于Arduino的四轴飞行器2.3.3 模块介绍1、电机输出模块（2）相关代码/主要函数定义extern uint8_t PWM_PIN8;void initOutput();/初始化函数void mixTable();/PID计算void writeMotors();/信号输出到电机/输出程序uint8_t PWM_PIN8 = 9,10,11,3,6,5,A2,12; /定义输出接口void initOutput() for(uint8_t i=0;i4;i+) pinMode(PWM_PINi,OUTPUT); /初始化，使PWM引脚作为输出引脚v

2、oid mixTable() int16_t maxMotor;/定义最大转速电机编号 uint8_t i; #define PIDMIX(X,Y,Z) rcCommandTHROTTLE + axisPIDROLL*X + axisPIDPITCH*Y + YAW_DIRECTION * axisPIDYAW*Z /PID算法motor0 = PIDMIX(-1,+1,-1);motor1 = PIDMIX(-1,-1,+1);motor2 = PIDMIX(+1,+1,+1);motor3 = PIDMIX(+1,-1,-1);/4个电机输出计算（PID）maxMotor=motor0;/

3、以下代码限制最大输出油门，防止异常for(i=1; imaxMotor) maxMotor=motori;for(i=0; i MAXTHROTTLE) /保证当某一个油门达到最大时，陀螺仪仍有良好的信号连接motori -= maxMotor - MAXTHROTTLE;motori = constrain(motori, conf.minthrottle,MAXTHROTTLE);if (rcDataTHROTTLE 3; / pin 9输出电机1号OCR1B = motor13; / pin 10输出电机2号OCR2A = motor23; / pin 11输出电机3号OCR2B = m

4、otor33; / pin 3输出电机4号void writeAllMotors(int16_t mc) /所有电机输出设定为mc for (uint8_t i =0;i4;i+) motori=mc; writeMotors();/*电调初始化函数，电调初始化完成后注释掉defined重新上传#if defined(ESC_CALIB_CANNOT_FLY) writeAllMotors(ESC_CALIB_HIGH);blinkLED(2,20, 2);delay(4000); writeAllMotors(ESC_CALIB_LOW);blinkLED(3,20, 2); while (

5、1) delay(5000);blinkLED(4,20, 2); #endif exit; #endif*/2、遥控器发送/接收模块（2）相关代码/RX.h主要函数定义#include Arduino.h#define RC_CHANS 8#define PCINT_PIN_COUNT5#define PCINT_RX_PORTPORTB#define PCINT_RX_MASKPCMSK0#define PCIR_PORT_BIT(10)#define RX_PC_INTERRUPTPCINT0_vect#define RX_PCINT_PIN_PORTPINB#define ROLLPI

6、N4/预定义各信道的名称#define THROTTLEPIN 3#define PITCHPIN5#define YAWPIN2#define AUX1PIN6#define AUX2PIN 7 #define AUX3PIN 1 /保留#define AUX4PIN 0 /保留#define PCINT_RX_BITS (11),(12),(13),(14),(10)void configureReceiver();void computeRC();uint16_t readRawRC(uint8_t chan);/初始信号读取函数/接收代码volatile uint16_t rcValu

7、eRC_CHANS = 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502; static uint8_t rcChannelRC_CHANS = ROLLPIN, PITCHPIN, YAWPIN, THROTTLEPIN, AUX1PIN,AUX2PIN,AUX3PIN,AUX4PIN;static uint8_t PCInt_RX_PinsPCINT_PIN_COUNT = PCINT_RX_BITS;void configureReceiver() for(uint8_t i = 0; i PCINT_PIN_COUNT; i+) PCINT_

8、RX_PORT |= PCInt_RX_Pinsi; PCINT_RX_MASK |= PCInt_RX_Pinsi;PCICR = PCIR_PORT_BIT;PCICR |= (1 0) ;#define RX_PIN_CHECK(pin_pos, rc_value_pos) if (mask & PCInt_RX_Pinspin_pos) if (!(pin & PCInt_RX_Pinspin_pos) dTime = cTime-edgeTimepin_pos; if (900dTime & dTime 0) RX_PIN_CHECK(0,2);#endif#if (PCINT_PI

9、N_COUNT 1)RX_PIN_CHECK(1,4);#endif#if (PCINT_PIN_COUNT 2)RX_PIN_CHECK(2,5);#endif#if (PCINT_PIN_COUNT 3)RX_PIN_CHECK(3,6);#endif#if (PCINT_PIN_COUNT 4)RX_PIN_CHECK(4,7);#endif#if (PCINT_PIN_COUNT 5)RX_PIN_CHECK(5,0);#endif#if (PCINT_PIN_COUNT 6)RX_PIN_CHECK(6,1);#endif#if (PCINT_PIN_COUNT 7)RX_PIN_C

10、HECK(7,3);#endifISR(PCINT0_vect) uint8_t pin;uint16_t cTime,dTime;static uint16_t edgeTime;pin = PINB;cTime = micros();sei();dTime = cTime-edgeTime; if (900dTime & dTime2200) rcValue0 = dTime; else edgeTime = cTime; /如果bit2在高电平(PPM pulse上升),存储时间uint16_t readRawRC(uint8_t chan) /读取原始信号uint16_t data;u

11、int8_t oldSREG;oldSREG = SREG; cli(); / 禁止中断data = rcValuercChannelchan;SREG = oldSREG;return data;void computeRC()/提取遥控器信号，进行数据处理，主要取平均值static uint16_t rcData4ValuesRC_CHANS4, rcDataMeanRC_CHANS;static uint8_t rc4ValuesIndex = 0;uint8_t chan,a;rc4ValuesIndex+;if (rc4ValuesIndex = 4) rc4ValuesIndex

12、= 0;for (chan = 0; chan RC_CHANS; chan+) rcData4Valueschanrc4ValuesIndex = readRawRC(chan);rcDataMeanchan = 0;for (a=0;a2;if ( rcDataMeanchan (uint16_t)rcDatachan +3) rcDatachan = rcDataMeanchan-2;if (chan 0) rcSerialCount -;if (rcSerialchan 900) rcDatachan = rcSerialchan;3、传感器读取模块（2）相关代码void ACC_getADC();void Gyro_getADC();void initSensors();void i2c_rep_start(uint8_t address);void i