bool debugit = 0;
#include <avr/eeprom.h>
#include <util/atomic.h>
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps21.h"
#include "PinChangeInt.h"
#define F_8MHZ 8000000UL
#define F_16MHZ 16000000UL
// <VR>
//#define AIN_PORTC {&ail_vr, &ele_vr, &rud_vr, NULL, NULL, NULL}
#define PWM_OUT_VAR {&sv_nick_out, &sv_roll_out, &sv_yaw_out, NULL}
#define PWM_OUT_PIN {NICK_OUT_PIN, ROLL_OUT_PIN, YAW_OUT_PIN, -1}
// <SERVO>
#define NICK_OUT_PIN 5
#define ROLL_OUT_PIN 6
#define YAW_OUT_PIN 7
#define F_XTAL F_16MHZ // external crystal oscillator frequency
#define F_CPU F_16MHZ
// adc
//volatile uint8_t ail_vr = 128;
//volatile uint8_t ele_vr = 128;
//volatile uint8_t rud_vr = 128;
uint16_t PWM_MIN_N = 800;
uint16_t PWM_MIN_R = 800;
uint16_t PWM_MIN_Y = 800;
uint16_t PWM_MAX_N = 2200;
uint16_t PWM_MAX_R = 2200;
uint16_t PWM_MAX_Y = 2200;
float DEG_MIN_N = -67.5;
float DEG_MIN_R = -67.5;
float DEG_MIN_Y = -67.5;
float DEG_MAX_N = 68.5;
float DEG_MAX_R = 68.5;
float DEG_MAX_Y = 68.5;
// servo
volatile int16_t sv_roll_out;
volatile int16_t sv_nick_out;
volatile int16_t sv_yaw_out;
int i ;
int old_setPWM1 = 0;
int old_setPWM2 = 0;
int16_t sv_roll_out2, sv_nick_out2, sv_yaw_out2;
uint32_t t;
volatile int8_t servo_sync = true;
bool initServos = true;
#define AD0 high = 0x69
float isDeg, setDeg, sollDeg,offset,offset2;
int16_t setPWM1,setPWM2;
// RC Channels
#define RCP1 10
#define RCP2 11
#define RCP3 12
// Neutral PWM
#define NE_PPM1 1500
#define NE_PPM2 1500
#define NE_PPM3 1500
volatile int nPPMIn = NE_PPM1; // volatile, we set this in the Interrupt and read it in loop so it must be declared volatile
volatile int nPPM1In = NE_PPM1; // volatile, we set this in the Interrupt and read it in loop so it must be declared volatile
volatile int nPPM2In = NE_PPM2; // volatile, we set this in the Interrupt and read it in loop so it must be declared volatile
volatile unsigned long ulStartPeriod = 0; // set in the interrupt
volatile boolean bNewPPM1Signal = false; // set in the interrupt and read in the loop
volatile boolean bNewPPM2Signal = false; // set in the interrupt and read in the loop
int bNewPPMSig;
int count = 0;
uint8_t latest_interrupted_pin;
uint8_t interrupt_count[20]={0}; // 20 possible arduino pins
void quicfunc() {
latest_interrupted_pin=PCintPort::arduinoPin;
interrupt_count[latest_interrupted_pin]++;
// Serial.println(" Call input");
calcInputPPM1();
}
void quicfunc2() {
latest_interrupted_pin=PCintPort::arduinoPin;
interrupt_count[latest_interrupted_pin]++;
calcInputPPM2();
}
/***************************************************************************************************************
* MPU
***************************************************************************************************************/
MPU6050 mpu;
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
Quaternion q; // [w, x, y, z] quaternion container
VectorFloat gravity; // [x, y, z] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
/***************************************************************************************************************
* TIMER1 and MISC
***************************************************************************************************************/
volatile uint16_t timer1_high = 0;
volatile uint8_t timer1_ovf = 0;
ISR(TIMER1_OVF_vect)
{
if (!++timer1_high)
timer1_ovf++;
}
uint32_t micros1()
{
uint16_t th, tl;
uint8_t to;
#if 0
// interrupt-disable version
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
tl = TCNT1; // tick=0.5us if F_CPU=16M, tick=1.0us if F_CPU=8M
th = timer1_high;
to = timer1_ovf;
}
#else
// lock-free version. read twice, ensure low order counter did not overflow and
// high order bits remain unchanged
while (1) {
tl = TCNT1; // tick=0.5us if F_CPU=16M, tick=1.0us if F_CPU=8M
th = timer1_high;
to = timer1_ovf;
if (!((tl ^ TCNT1) & 0x8000) && th == timer1_high && to == timer1_ovf)
break;
}
#endif
#if F_CPU == F_16MHZ
return ((uint32_t)to << 31) | ((uint32_t)th << 15) | (tl >> 1);
#else
return ((uint32_t)th << 16) | tl;
#endif
}
void delay1(uint32_t ms)
{
uint32_t us = ms * 1000;
uint32_t t = micros1();
while ((int32_t)(micros1() - t) < us);
}
int freeRam()
{
extern int __heap_start, *__brkval;
int v;
return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval);
}
/***************************************************************************************************************
* DIGITAL OUT (SERVO)
***************************************************************************************************************/
volatile int16_t *pwm_out_var[] = PWM_OUT_VAR;
volatile int8_t pwm_out_pin[] = PWM_OUT_PIN;
volatile int8_t servo_busy = false;
ISR(TIMER1_COMPA_vect)
{
static int8_t fall_ch = 0;
static int8_t rise_ch = 0;
uint16_t wait;
uint16_t tcnt1;
digitalWrite(pwm_out_pin[fall_ch], LOW);
if (rise_ch >= 0) {
tcnt1 = TCNT1;
digitalWrite(pwm_out_pin[rise_ch], HIGH);
wait = (*pwm_out_var[rise_ch] - 2) << (F_CPU == F_16MHZ ? 1 : 0);
fall_ch = rise_ch;
if (!pwm_out_var[++rise_ch]) {
rise_ch = -1;
}
} else {
rise_ch = 0;
servo_busy = false;
TIMSK1 &= ~(1 << OCIE1A); // disable further interrupt on TCNT1 == OCR1A
}
OCR1A = tcnt1 + wait;
}
void init_digital_out()
{
int8_t i = 0;
while (pwm_out_pin[i] >= 0) {
pinMode(pwm_out_pin[i], OUTPUT);
i++;
}
}
/***************************************************************************************************************
* RX IN - MIXER - SERVO OUT
***************************************************************************************************************/
void start_servo_frame(int svout,int servo_on)
{
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
//
servo_busy = true;
switch(servo_on){
case 1: sv_nick_out = svout; break;
case 2: sv_roll_out = svout; break;
case 3: sv_yaw_out = svout; break;
}
// Serial.print(gRoll); Serial.print("\t");
// Serial.println(ele_out);
TIMSK1 |= (1 << OCIE1A); // enable interrupt on TCNT1 == OCR1A
TIFR1 |= (1 << OCF1A); // clear any pending interrupt
OCR1A = TCNT1 + 4; // force interrupt condition
} // reenable global interrupts
servo_busy = false;
}
void setup()
{
Wire.begin();
Serial.begin(115200);
Serial.println("Setup");
mpu.initialize();
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
devStatus = mpu.dmpInitialize();
if (devStatus == 0) {
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
int8_t i;
// set clock prescaler to /2 when (F_XTAL, F_CPU) = (16M, 8M)
#if F_XTAL == F_16MHZ && F_CPU == F_8MHZ
uint8_t saveSREG = SREG;
cli();
CLKPR = (1 << CLKPCE);
CLKPR = (1 << CLKPS0);
SREG = saveSREG;
#endif
// init TIMER1
TCCR1A = 0; // normal counting mode
TCCR1B = (1 << CS11); // clkio/8
TIMSK1 = (1 << TOIE1); // enable overflow interrupt
// TIMSK1 |= (1 << OCIE1A); // enable interrupt on TCNT1 == OCR1A
// disable TIMER0
TCCR0B &= ~((1 << CS00) | (1 << CS01) | (1 << CS02)); // clock stopped
// TIMSK0 &= ~(1 << TOIE0); // disable overflow interrupt
init_digital_out(); // servo
pinMode(RCP1, INPUT);digitalWrite(RCP1, HIGH);
pinMode(RCP2, INPUT);digitalWrite(RCP2, HIGH);
PCintPort::attachInterrupt(RCP1,&quicfunc, CHANGE);
PCintPort::attachInterrupt(RCP2,&quicfunc2, CHANGE);
Serial.print("Setup complete ");
}
/***************************************************************************************************************
* LOOP
***************************************************************************************************************/
void loop()
{
again:
servo_sync = false;
for (i=0; i < 20; i++) {
if (interrupt_count[i] != 0) {
count=interrupt_count[i];
interrupt_count[i]=0;
}
}
if(bNewPPM1Signal){bNewPPM1Signal = false;}
if(bNewPPM2Signal){bNewPPM2Signal = false;}
// if programming failed, don't try to do anything
if (!dmpReady) {Serial.println("not ready");return;}
// while (!mpuInterrupt && fifoCount < packetSize) {}
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
if(debugit){
Serial.print("ypr\t");
Serial.print(ypr[0] * 180/M_PI);
Serial.print("\t");
Serial.print(ypr[1] * 180/M_PI);
Serial.print("\t");
Serial.println(ypr[2] * 180/M_PI);
}
// Serial.print(nPPM1In);Serial.print("\t");Serial.print(nPPM2In);Serial.print("\t");
if(initServos)
{
Serial.println("Set init servo postition");
start_servo_frame(1500,1);
start_servo_frame(1500,2);
initServos=false;
Serial.println(initServos);
}
offset=calulate_offset(nPPM1In,offset);
offset2=calulate_offset(nPPM2In,offset2);
servo_sync = true;
setPWM1 = mapf(computeAngle(ypr[2],offset), DEG_MAX_N, DEG_MIN_N, PWM_MIN_N, PWM_MAX_N);
// Serial.print(offset);Serial.print("\t");Serial.println(setPWM1);
if (servo_sync && !servo_busy) {if(old_setPWM1!=setPWM1){ start_servo_frame(setPWM1,1);old_setPWM1=setPWM1;}}
// Serial.print(DEG_MAX_N);Serial.print("\t");Serial.print(DEG_MIN_N);Serial.print("\t");Serial.print(PWM_MIN_N);Serial.print("\t");Serial.print(PWM_MAX_N);Serial.print("\t");Serial.print(val);Serial.print("\t");
setPWM2 = mapf(computeAngle(ypr[1],offset2), DEG_MAX_R, DEG_MIN_R, PWM_MIN_R, PWM_MAX_R);
if (servo_sync && !servo_busy) {if(old_setPWM2!=setPWM2){start_servo_frame(setPWM2,2);old_setPWM2=setPWM2;}}
t = micros1();
goto again;
}
long mapf(float x, float in_min, float in_max, long out_min, long out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
float computeAngle(float angleIN, float offsetIN)
{
//angleIN = (float)((int)((angleIN * 180/M_PI)*10))/10; // round floar much load
angleIN = (angleIN * 180/M_PI)+offsetIN;
if(angleIN>=0){setDeg = sollDeg-angleIN;} else {setDeg = sollDeg + abs(angleIN); }
return setDeg;
}
void calcInputPPM1()
{
if(digitalRead(RCP1) == HIGH)
{
ulStartPeriod = micros1();
}
else
{
if(ulStartPeriod && (bNewPPM1Signal == false))
{
nPPM1In = (int)(micros1() - ulStartPeriod);
ulStartPeriod = 0;
bNewPPM1Signal = true;
}
}
}
void calcInputPPM2()
{
if(digitalRead(RCP2) == HIGH)
{
ulStartPeriod = micros1();
}
else
{
if(ulStartPeriod && (bNewPPM2Signal == false))
{
nPPM2In = (int)(micros1() - ulStartPeriod);
ulStartPeriod = 0;
bNewPPM2Signal = true;
}
}
}
float calulate_offset(float nPPMIn,float offsetIN){
if(nPPMIn>=700 && nPPMIn<=2000)
{
if(nPPMIn>=1660 && offsetIN<=45){offsetIN+=0.15;}
if(nPPMIn>=1720 && offsetIN<=45){offsetIN+=0.3;}
if(nPPMIn>=1780 && offsetIN<=45){offsetIN+=0.5;}
if(nPPMIn>=1830 && offsetIN<=45){offsetIN+=1;}
if(nPPMIn>=1870 && offsetIN<=45){offsetIN+=2;}
if(nPPMIn>=1910 && offsetIN<=45){offsetIN+=4;}
if(nPPMIn<=1400 && offsetIN>=-45){offsetIN-=0.15;}
if(nPPMIn<=1340 && offsetIN>=-45){offsetIN-=0.3;}
if(nPPMIn<=1280 && offsetIN>=-45){offsetIN-=0.5;}
if(nPPMIn<=1230 && offsetIN>=-45){offsetIN-=1;}
if(nPPMIn<=1190 && offsetIN>=-45){offsetIN-=2;}
if(nPPMIn<=1140 && offsetIN>=-45){offsetIN-=4;}
}
return offsetIN;
}
)
