And here’s the 2’nd half of the V1_7 code.
// ****servo yaw with a 50Hz refresh rate****
// prescaler is set by default to 64 on Timer0
// Duemilanove : 16MHz / 64 => 4 us
// 256 steps = 1 counter cycle = 1024 us
// algorithm strategy:
// pulse high servo 0 -> do nothing for 1000 us -> do nothing for [0 to 1000] us -> pulse down servo 0
// pulse high servo 1 -> do nothing for 1000 us -> do nothing for [0 to 1000] us -> pulse down servo 1
// pulse high servo 2 -> do nothing for 1000 us -> do nothing for [0 to 1000] us -> pulse down servo 2
// pulse high servo 3 -> do nothing for 1000 us -> do nothing for [0 to 1000] us -> pulse down servo 3
// do nothing for 14 x 1000 us
ISR(TIMER0_COMPA_vect) {
static uint8_t state = 0;
static uint8_t count;
if (state == 0) {
//http://billgrundmann.wordpress.com/2009/03/03/to-use-or-not-use-writedigital/
#if defined(TRI) || defined (BI)
DIGITAL_SERVO_TRI_HIGH
#endif
OCR0A += 250; // 1000 us
state++ ;
} else if (state == 1) {
OCR0A += atomicServo[0]; // 1000 + [0-1020] us
state++;
} else if (state == 2) {
#if defined(TRI) || defined (BI)
DIGITAL_SERVO_TRI_LOW
#endif
#if defined(BI)
DIGITAL_BI_LEFT_HIGH
#endif
#if defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING)
DIGITAL_TILT_PITCH_HIGH
#endif
OCR0A += 250; // 1000 us
state++;
} else if (state == 3) {
OCR0A += atomicServo[1]; // 1000 + [0-1020] us
state++;
} else if (state == 4) {
#if defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING)
DIGITAL_TILT_PITCH_LOW
DIGITAL_TILT_ROLL_HIGH
#endif
#if defined(BI)
DIGITAL_BI_LEFT_LOW
#endif
state++;
OCR0A += 250; // 1000 us
} else if (state == 5) {
OCR0A += atomicServo[2]; // 1000 + [0-1020] us
state++;
} else if (state == 6) {
#if defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING)
DIGITAL_TILT_ROLL_LOW
#endif
#if defined(CAMTRIG)
DIGITAL_CAM_HIGH
#endif
state++;
OCR0A += 250; // 1000 us
} else if (state == 7) {
OCR0A += atomicServo[3]; // 1000 + [0-1020] us
state++;
} else if (state == 8) {
#if defined(CAMTRIG)
DIGITAL_CAM_LOW
#endif
count = 10; // 12 x 1000 us
state++;
OCR0A += 250; // 1000 us
} else if (state == 9) {
if (count > 0) count--;
else state = 0;
OCR0A += 250;
}
}
#endif
#if (NUMBER_MOTOR == 6) && defined(PROMINI)
void initializeSoftPWM() {
TCCR0A = 0; // normal counting mode
TIMSK0 |= (1 << OCIE0A); // Enable CTC interrupt
TIMSK0 |= (1 << OCIE0B);
}
ISR(TIMER0_COMPA_vect) {
static uint8_t state = 0;
if (state == 0) {
PORTD |= 1 << 5; //digital PIN 5 high
OCR0A += atomicPWM_PIN5_highState; //250 x 4 microsecons = 1ms
state = 1;
} else if (state == 1) {
OCR0A += atomicPWM_PIN5_highState;
state = 2;
} else if (state == 2) {
PORTD &= ~(1 << 5); //digital PIN 5 low
OCR0A += atomicPWM_PIN5_lowState;
state = 0;
}
}
ISR(TIMER0_COMPB_vect) { //the same with digital PIN 6 and OCR0B counter
static uint8_t state = 0;
if (state == 0) {
PORTD |= 1 << 6; OCR0B += atomicPWM_PIN6_highState; state = 1;
} else if (state == 1) {
OCR0B += atomicPWM_PIN6_highState; state = 2;
} else if (state == 2) {
PORTD &= ~(1 << 6); OCR0B += atomicPWM_PIN6_lowState; state = 0;
}
}
#endif
// ******************
// rc functions
// ******************
#define MINCHECK 1100
#define MAXCHECK 1900
volatile int16_t failsafeCnt = 0;
static uint8_t pinRcChannel[8] = {ROLLPIN, PITCHPIN, YAWPIN, THROTTLEPIN, AUX1PIN, AUX2PIN, CAM1PIN, CAM2PIN};
volatile uint16_t rcPinValue[8] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500}; // interval [1000;2000]
static int16_t rcData[8] ; // interval [1000;2000]
static int16_t rcCommand[4] ; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
static int16_t rcHysteresis[8] ;
static int16_t rcData4Values[8][4];
static uint8_t rcRate8;
static uint8_t rcExpo8;
static float rcFactor1;
static float rcFactor2;
// ***PPM SUM SIGNAL***
#ifdef SERIAL_SUM_PPM
static uint8_t rcChannel[8] = {SERIAL_SUM_PPM};
#endif
volatile uint16_t rcValue[8] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500}; // interval [1000;2000]
// Configure each rc pin for PCINT
void configureReceiver() {
#ifndef SERIAL_SUM_PPM
for (uint8_t chan = 0; chan < 8; chan++)
for (uint8_t a = 0; a < 4; a++)
rcData4Values[chan][a] = 1500; //we initiate the default value of each channel. If there is no RC receiver connected, we will see those values
#if defined(PROMINI)
// PCINT activated only for specific pin inside [D0-D7] , [D2 D4 D5 D6 D7] for this multicopter
PORTD = (1 << 2) | (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7); //enable internal pull ups on the PINs of PORTD (no high impedence PINs)
PCMSK2 |= (1 << 2) | (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7);
PCICR = 1 << 2; // PCINT activated only for the port dealing with [D0-D7] PINs
#endif
#if defined(MEGA)
// PCINT activated only for specific pin inside [A8-A15]
DDRK = 0; // defined PORTK as a digital port ([A8-A15] are consired as digital PINs and not analogical)
PORTK = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7); //enable internal pull ups on the PINs of PORTK
PCMSK2 |= (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7);
PCICR = 1 << 2; // PCINT activated only for PORTK dealing with [A8-A15] PINs
#endif
#else
PPM_PIN_INTERRUPT
#endif
}
#ifndef SERIAL_SUM_PPM
ISR(PCINT2_vect) { //this ISR is common to every receiver channel, it is call everytime a change state occurs on a digital pin [D2-D7]
uint8_t mask;
uint8_t pin;
uint16_t cTime, dTime;
static uint16_t edgeTime[8];
static uint8_t PCintLast;
#if defined(PROMINI)
pin = PIND; // PIND indicates the state of each PIN for the arduino port dealing with [D0-D7] digital pins (8 bits variable)
#endif
#if defined(MEGA)
pin = PINK; // PINK indicates the state of each PIN for the arduino port dealing with [A8-A15] digital pins (8 bits variable)
#endif
mask = pin ^ PCintLast; // doing a ^ between the current interruption and the last one indicates wich pin changed
sei(); // re enable other interrupts at this point, the rest of this interrupt is not so time critical and can be interrupted safely
PCintLast = pin; // we memorize the current state of all PINs [D0-D7]
cTime = micros(); // micros() return a uint32_t, but it is not usefull to keep the whole bits => we keep only 16 bits
// mask is pins [D0-D7] that have changed // the principle is the same on the MEGA for PORTK and [A8-A15] PINs
// chan = pin sequence of the port. chan begins at D2 and ends at D7
if (mask & 1 << 2) //indicates the bit 2 of the arduino port [D0-D7], that is to say digital pin 2, if 1 => this pin has just changed
if (!(pin & 1 << 2)) { //indicates if the bit 2 of the arduino port [D0-D7] is not at a high state (so that we match here only descending PPM pulse)
dTime = cTime - edgeTime[2]; if (900 < dTime && dTime < 2200) rcPinValue[2] = dTime; // just a verification: the value must be in the range [1000;2000] + some margin
} else edgeTime[2] = cTime; // if the bit 2 of the arduino port [D0-D7] is at a high state (ascending PPM pulse), we memorize the time
if (mask & 1 << 4) //same principle for other channels // avoiding a for() is more than twice faster, and it's important to minimize execution time in ISR
if (!(pin & 1 << 4)) {
dTime = cTime - edgeTime[4]; if (900 < dTime && dTime < 2200) rcPinValue[4] = dTime;
} else edgeTime[4] = cTime;
if (mask & 1 << 5)
if (!(pin & 1 << 5)) {
dTime = cTime - edgeTime[5]; if (900 < dTime && dTime < 2200) rcPinValue[5] = dTime;
} else edgeTime[5] = cTime;
if (mask & 1 << 6)
if (!(pin & 1 << 6)) {
dTime = cTime - edgeTime[6]; if (900 < dTime && dTime < 2200) rcPinValue[6] = dTime;
} else edgeTime[6] = cTime;
if (mask & 1 << 7)
if (!(pin & 1 << 7)) {
dTime = cTime - edgeTime[7]; if (900 < dTime && dTime < 2200) rcPinValue[7] = dTime;
} else edgeTime[7] = cTime;
#if defined(MEGA)
if (mask & 1 << 0)
if (!(pin & 1 << 0)) {
dTime = cTime - edgeTime[0]; if (900 < dTime && dTime < 2200) rcPinValue[0] = dTime;
} else edgeTime[0] = cTime;
if (mask & 1 << 1)
if (!(pin & 1 << 1)) {
dTime = cTime - edgeTime[1]; if (900 < dTime && dTime < 2200) rcPinValue[1] = dTime;
} else edgeTime[1] = cTime;
if (mask & 1 << 3)
if (!(pin & 1 << 3)) {
dTime = cTime - edgeTime[3]; if (900 < dTime && dTime < 2200) rcPinValue[3] = dTime;
} else edgeTime[3] = cTime;
#endif
#if defined(FAILSAFE)
if (mask & 1 << THROTTLEPIN) { // If pulse present on THROTTLE pin (independent from ardu version), clear FailSafe counter - added by MIS
if (failsafeCnt > 20) failsafeCnt -= 20; else failsafeCnt = 0;
}
#endif
}
#else
void rxInt() {
uint16_t now, diff;
static uint16_t last = 0;
static uint8_t chan = 0;
now = micros();
diff = now - last;
last = now;
if (diff > 3000) chan = 0;
else {
if (900 < diff && diff < 2200 && chan < 8 ) rcValue[chan] = diff;
chan++;
#if defined(FAILSAFE)
if (failsafeCnt > 20) failsafeCnt -= 20; else failsafeCnt = 0; // clear FailSafe counter - added by MIS
#endif
}
}
#endif
uint16_t readRawRC(uint8_t chan) {
uint16_t data;
uint8_t oldSREG;
oldSREG = SREG;
cli(); // Let's disable interrupts
#ifndef SERIAL_SUM_PPM
data = rcPinValue[pinRcChannel[chan]]; // Let's copy the data Atomically
#else
data = rcValue[rcChannel[chan]];
#endif
SREG = oldSREG;
sei();// Let's enable the interrupts
return data; // We return the value correctly copied when the IRQ's where disabled
}
void computeRC() {
static uint8_t rc4ValuesIndex = 0;
uint8_t chan, a;
rc4ValuesIndex++;
for (chan = 0; chan < 8; chan++) {
rcData4Values[chan][rc4ValuesIndex % 4] = readRawRC(chan);
rcData[chan] = 0;
for (a = 0; a < 4; a++)
rcData[chan] += rcData4Values[chan][a];
rcData[chan] = (rcData[chan] + 2) / 4;
if ( rcData[chan] < rcHysteresis[chan] - 3) rcHysteresis[chan] = rcData[chan] + 2;
if ( rcData[chan] > rcHysteresis[chan] + 3) rcHysteresis[chan] = rcData[chan] - 2;
}
}
// ****************
// EEPROM functions
// ****************
static uint8_t P8[3], I8[3], D8[3]; //8 bits is much faster and the code is much shorter
static uint8_t dynP8[3], dynI8[3], dynD8[3];
static uint8_t PLEVEL8, ILEVEL8;
static uint8_t rollPitchRate;
static uint8_t yawRate;
static uint8_t dynThrPID;
static uint8_t checkNewConf = 134;
static uint8_t activateAcc8, activateBaro8, activateMag8;
static uint8_t activateCamStab8, activateCamTrig8;
void readEEPROM() {
uint8_t i, p = 1;
for (i = 0; i < 3; i++) {
P8[i] = EEPROM.read(p++);
I8[i] = EEPROM.read(p++);
D8[i] = EEPROM.read(p++);
}
PLEVEL8 = EEPROM.read(p++); ILEVEL8 = EEPROM.read(p++);
rcRate8 = EEPROM.read(p++); rcExpo8 = EEPROM.read(p++);
rollPitchRate = EEPROM.read(p++);
yawRate = EEPROM.read(p++);
dynThrPID = EEPROM.read(p++);
activateAcc8 = EEPROM.read(p++); activateBaro8 = EEPROM.read(p++); activateMag8 = EEPROM.read(p++);
activateCamStab8 = EEPROM.read(p++); activateCamTrig8 = EEPROM.read(p++);
for (i = 0; i < 3; i++) accZero[i] = (EEPROM.read(p++) & 0xff) + (EEPROM.read(p++) << 8);
//note on the following lines: we do this calcul here because it's a static and redundant result and we don't want to load the critical loop whith it
rcFactor1 = rcRate8 / 50.0 * rcExpo8 / 100.0 / 250000.0;
rcFactor2 = (100 - rcExpo8) * rcRate8 / 5000.0;
}
void writeParams() {
uint8_t i, p = 1;
EEPROM.write(0, checkNewConf);
for (i = 0; i < 3; i++) {
EEPROM.write(p++, P8[i]);
EEPROM.write(p++, I8[i]);
EEPROM.write(p++, D8[i]);
}
EEPROM.write(p++, PLEVEL8); EEPROM.write(p++, ILEVEL8);
EEPROM.write(p++, rcRate8); EEPROM.write(p++, rcExpo8);
EEPROM.write(p++, rollPitchRate);
EEPROM.write(p++, yawRate);
EEPROM.write(p++, dynThrPID);
EEPROM.write(p++, activateAcc8); EEPROM.write(p++, activateBaro8); EEPROM.write(p++, activateMag8);
EEPROM.write(p++, activateCamStab8); EEPROM.write(p++, activateCamTrig8);
for (i = 0; i < 3; i++) {
EEPROM.write(p++, accZero[i]);
EEPROM.write(p++, accZero[i] >> 8 & 0xff);
}
readEEPROM();
blinkLED(15, 20, 1);
}
void checkFirstTime() {
if ( EEPROM.read(0) != checkNewConf ) {
P8[ROLL] = 40; I8[ROLL] = 30; D8[ROLL] = 15;
P8[PITCH] = 40; I8[PITCH] = 30; D8[PITCH] = 15;
P8[YAW] = 80; I8[YAW] = 0; D8[YAW] = 0;
PLEVEL8 = 140; ILEVEL8 = 45;
rcRate8 = 45;
rcExpo8 = 65;
rollPitchRate = 0;
yawRate = 0;
dynThrPID = 0;
activateAcc8 = 0; activateBaro8 = 0; activateMag8 = 0;
activateCamStab8 = 0; activateCamTrig8 = 0;
writeParams();
}
}
// *****************************
// LCD & display & monitoring
// *****************************
// 1000000 / 9600 = 104 microseconds at 9600 baud.
// we set it below to take some margin with the running interrupts
#define BITDELAY 102
void LCDprint(uint8_t i) {
#if defined(LCD_TEXTSTAR)
Serial.print( i , BYTE);
#else
LCDPIN_OFF
delayMicroseconds(BITDELAY);
for (uint8_t mask = 0x01; mask; mask <<= 1) {
if (i & mask) LCDPIN_ON else LCDPIN_OFF // choose bit
delayMicroseconds(BITDELAY);
}
LCDPIN_ON //switch ON digital PIN 0
delayMicroseconds(BITDELAY);
#endif
}
void LCDprintChar(const char *s) {
while (*s) LCDprint(*s++);
}
void initLCD() {
blinkLED(20, 30, 1);
#if defined(LCD_TEXTSTAR)
// Cat's Whisker Technologies 'TextStar' Module CW-LCD-02
// http://cats-whisker.com/resources/documents/cw-lcd-02_datasheet.pdf
// Modified by Luca Brizzi aka gtrick90 @ RCG
LCDprint(0xFE); LCDprint(0x43); LCDprint(0x02); //cursor blink mode
LCDprint(0x0c); //clear screen
LCDprintChar("MultiWii");
LCDprint(0x0d); // carriage return
LCDprintChar("CONFIG PARAMS");
delay(2500);
LCDprint(0x0c); //clear screen
#else
Serial.end();
//init LCD
PINMODE_LCD //TX PIN for LCD = Arduino RX PIN (more convenient to connect a servo plug on arduino pro mini)
#endif
}
void configurationLoop() {
uint8_t chan, i;
uint8_t param, paramActive;
uint8_t val, valActive;
static char line1[17], line2[17];
uint8_t LCD = 1;
uint8_t refreshLCD = 1;
typedef struct {
char* paramText;
uint8_t* var;
uint8_t decimal;
uint8_t increment;
} paramStruct;
static paramStruct p[] = {
{"PITCH&ROLL P", &P8[ROLL], 1, 1},
{"ROLL P", &P8[ROLL], 1, 1}, {"ROLL I", &I8[ROLL], 3, 5}, {"ROLL D", &D8[ROLL], 0, 1},
{"PITCH P", &P8[PITCH], 1, 1}, {"PITCH I", &I8[PITCH], 3, 5}, {"PITCH D", &D8[PITCH], 0, 1},
{"YAW P", &P8[YAW], 1, 1}, {"YAW I", &I8[YAW], 3, 5}, {"YAW D", &D8[YAW], 0, 1},
{"LEVEL P", &PLEVEL8, 1, 1}, {"LEVEL I", &ILEVEL8, 3, 5},
{"RC RATE", &rcRate8, 2, 2}, {"RC EXPO", &rcExpo8, 2, 2},
{"PITCH&ROLL RATE", &rollPitchRate, 2, 2}, {"YAW RATE", &yawRate, 2, 2},
{"THROTTLE PID", &dynThrPID, 2, 2},
};
initLCD();
param = 0;
while (LCD == 1) {
if (refreshLCD == 1) {
strcpy(line2, " ");
strcpy(line1, " ");
i = 0; char* point = p[param].paramText; while (*point) line1[i++] = *point++;
uint16_t unit = *p[param].var;
if (param == 12) {
unit *= 2; // RC RATE can go up to 500
}
char c1 = '0' + unit / 100; char c2 = '0' + unit / 10 - (unit / 100) * 10; char c3 = '0' + unit - (unit / 10) * 10;
if (p[param].decimal == 0) {
line2[6] = c1;
line2[7] = c2;
line2[8] = c3;
}
if (p[param].decimal == 1) {
line2[5] = c1;
line2[6] = c2;
line2[7] = '.';
line2[8] = c3;
}
if (p[param].decimal == 2) {
line2[5] = c1;
line2[6] = '.';
line2[7] = c2;
line2[8] = c3;
}
if (p[param].decimal == 3) {
line2[4] = '0';
line2[5] = '.';
line2[6] = c1;
line2[7] = c2;
line2[8] = c3;
}
#if defined(LCD_TEXTSTAR)
LCDprint(0xFE); LCDprint('L'); LCDprint(1); LCDprintChar(line1); //refresh line 1 of LCD
LCDprint(0xFE); LCDprint('L'); LCDprint(2); LCDprintChar(line2); //refresh line 2 of LCD
#else
LCDprint(0xFE); LCDprint(128); LCDprintChar(line1);
LCDprint(0xFE); LCDprint(192); LCDprintChar(line2);
#endif
refreshLCD = 0;
}
for (chan = ROLL; chan < 4; chan++) rcData[chan] = readRawRC(chan);
//switch config param with pitch
if (rcData[PITCH] < MINCHECK && paramActive == 0 && param < 16) {
paramActive = 1; refreshLCD = 1; blinkLED(10, 20, 1);
param++;
}
if (rcData[PITCH] > MAXCHECK && paramActive == 0 && param > 0) {
paramActive = 1; refreshLCD = 1; blinkLED(10, 20, 1);
param--;
}
if (rcData[PITCH] < MAXCHECK && rcData[PITCH] > MINCHECK) paramActive = 0;
//+ or - param with low and high roll
if (rcData[ROLL] < MINCHECK && valActive == 0 && *p[param].var > p[param].increment - 1) {
valActive = 1; refreshLCD = 1; blinkLED(10, 20, 1);
*p[param].var -= p[param].increment; //set val -
if (param == 0) *p[4].var = *p[0].var; //PITCH P
}
if (rcData[ROLL] > MAXCHECK && valActive == 0) {
valActive = 1; refreshLCD = 1; blinkLED(10, 20, 1);
*p[param].var += p[param].increment; //set val +
if (param == 0) *p[4].var = *p[0].var; //PITCH P
}
if (rcData[ROLL] < MAXCHECK && rcData[ROLL] > MINCHECK) valActive = 0;
if (rcData[YAW] < MINCHECK && rcData[PITCH] > MAXCHECK) LCD = 0;
}
#if defined(LCD_TEXTSTAR)
blinkLED(20, 30, 1);
LCDprint(0x0c); //clear screen
LCDprintChar("Saving Settings...");
#endif
writeParams();
#if defined(LCD_TEXTSTAR)
LCDprintChar("..done! Exit.");
#else
Serial.begin(SERIAL_COM_SPEED);
#endif
}
void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat) {
uint8_t i, r;
for (r = 0; r < repeat; r++) {
for (i = 0; i < num; i++) {
LEDPIN_SWITCH //switch LEDPIN state
BUZZERPIN_ON
delay(wait);
BUZZERPIN_OFF
}
delay(60);
}
}
void annexCode() { //this code is excetuted at each loop and won't interfere with control loop if it lasts less than 650 microseconds
static uint32_t serialTime;
static uint32_t buzzerTime;
static uint32_t calibrateTime;
static uint32_t calibratedAccTime;
static uint8_t buzzerState = 0;
static uint32_t vbatRaw = 0; //used for smoothing voltage reading
static uint8_t buzzerFreq; //delay between buzzer ring
uint8_t axis;
uint8_t prop1, prop2;
for (axis = 0; axis < 2; axis++) {
//PITCH & ROLL dynamic PID adjustemnt, depending on stick deviation
prop1 = 100 - min(abs(rcData[axis] - 1500) / 5, 100) * rollPitchRate / 100;
//PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
if (rcData[THROTTLE] < 1500) prop2 = 100;
else if (rcData[THROTTLE] > 1499 && rcData[THROTTLE] < 2000) prop2 = 100 - (rcData[THROTTLE] - 1500) * dynThrPID / 500;
else prop2 = 100 - dynThrPID;
dynP8[axis] = P8[axis] * prop1 / 100 * prop2 / 100;
dynD8[axis] = D8[axis] * prop1 / 100 * prop2 / 100;
}
//YAW dynamic PID adjustemnt
prop1 = 100 - min(abs(rcData[YAW] - 1500) / 5, 100) * yawRate / 100;
dynP8[YAW] = P8[YAW] * prop1 / 100;
dynD8[YAW] = D8[YAW] * prop1 / 100;
#if defined(VBAT)
vbatRaw = (vbatRaw * 15 + analogRead(V_BATPIN) * 16) >> 4; // smoothing of vbat readings
vbat = vbatRaw / VBATSCALE; // result is Vbatt in 0.1V steps
if (vbat > VBATLEVEL1_3S) {
buzzerFreq = 0; buzzerState = 0; BUZZERPIN_OFF;
} else if (vbat > VBATLEVEL2_3S)
buzzerFreq = 1;
else if (vbat > VBATLEVEL3_3S)
buzzerFreq = 2;
else
buzzerFreq = 4;
if (buzzerFreq) {
if (buzzerState && (currentTime > buzzerTime + 250000) ) {
buzzerState = 0; BUZZERPIN_OFF; buzzerTime = currentTime;
} else if ( !buzzerState && (currentTime > (buzzerTime + (2000000 >> buzzerFreq))) ) {
buzzerState = 1; BUZZERPIN_ON; buzzerTime = currentTime;
}
}
#endif
if ( (currentTime > calibrateTime + 100000) && ( (calibratingA > 0 && (nunchukPresent == 1 || accPresent == 1)) || (calibratingG > 0) ) ) { // Calibration phasis
LEDPIN_SWITCH
calibrateTime = currentTime;
} else if ( (calibratingA == 0) || (calibratingG == 0 && !(nunchukPresent == 1 || accPresent == 1)) ) {
if (armed) LEDPIN_ON
else if (calibratedACC == 1) LEDPIN_OFF
}
if ( currentTime > calibratedAccTime + 500000 ) {
if ( (nunchukPresent == 1 || accPresent == 1) && (abs(accADC[ROLL]) > 50 || abs(accADC[PITCH]) > 50 || abs(accADC[YAW]) > 400) ) {
calibratedACC = 0; //the multi uses ACC and is not calibrated or is too much inclinated
LEDPIN_SWITCH
calibratedAccTime = currentTime;
} else
calibratedACC = 1;
}
if (currentTime > serialTime + 20000) { // 50Hz
serialCom();
serialTime = currentTime;
}
for (axis = 0; axis < 2; axis++)
rcCommand[axis] = (rcHysteresis[axis] - MIDRC) * (rcFactor2 + rcFactor1 * square((rcHysteresis[axis] - MIDRC)));
rcCommand[THROTTLE] = (MAXTHROTTLE - MINTHROTTLE) / (2000.0 - MINCHECK) * (rcHysteresis[THROTTLE] - MINCHECK) + MINTHROTTLE;
rcCommand[YAW] = rcHysteresis[YAW] - MIDRC;
}
void setup() {
Serial.begin(SERIAL_COM_SPEED);
LEDPIN_PINMODE
POWERPIN_PINMODE
BUZZERPIN_PINMODE
POWERPIN_OFF
initializeMotors();
readEEPROM();
checkFirstTime();
configureReceiver();
delay(200);
POWERPIN_ON
delay(100);
i2c_init();
delay(100);
#if defined(ITG3200) || defined(L3G4200D)
i2c_Gyro_init();
#else
i2c_WMP_init(250);
#endif
#if defined(BMP085)
i2c_Baro_init();
#endif
#if defined(I2C_ACC)
i2c_ACC_init();
#endif
#if defined(ADCACC)
adc_ACC_init();
#endif
#if defined(HMC5843) || defined (HMC5883)
i2c_Mag_init();
#endif
#if defined(SERVO)
initializeServo();
#elif (NUMBER_MOTOR == 6) && defined(PROMINI)
initializeSoftPWM();
#endif
previousTime = micros();
#if defined(GIMBAL) || defined(FLYING_WING)
calibratingA = 400;
#else
calibratingA = 0;
#endif
calibratingG = 400;
}
// ******** Main Loop *********
void loop () {
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
uint8_t axis, i;
int16_t error;
int32_t errorAngle;
int16_t delta;
int16_t PTerm, ITerm, DTerm;
static int16_t lastGyro[3] = {0, 0, 0};
static int16_t delta1[3];
static int16_t delta2[3];
int16_t maxMotor;
static int32_t errorGyroI[3] = {0, 0, 0};
static int32_t errorAngleI[2] = {0, 0};
static int16_t altitudeHold = 0;
static uint8_t altitudeLock;
static uint8_t camCycle = 0;
static uint8_t camState = 0;
static uint32_t camTime, magTime;
static uint8_t rcOptions;
if (currentTime > (rcTime + 20000) ) { // 50Hz
computeRC();
// Failsafe routine - added by MIS
#if defined(FAILSAFE)
if ( failsafeCnt > (5 * FAILSAVE_DELAY) && armed == 1) { // Stabilize, and set Throttle to specified level
for (i = 0; i < 3; i++) { // after specified guard time after RC signal is lost (in 0.1sec)
rcHysteresis[i] = MIDRC;
rcData[i] = MIDRC;
}
rcHysteresis[THROTTLE] = FAILSAVE_THR0TTLE;
rcData[THROTTLE] = FAILSAVE_THR0TTLE;
if (failsafeCnt > 5 * (FAILSAVE_DELAY + FAILSAVE_OFF_DELAY)) { // Turn OFF motors after specified Time (in 0.1sec)
armed = 0;
writeAllMotors(MINCOMMAND);
}
}
failsafeCnt++;
#endif
// end of failsave routine - next change is made with RcOptions setting
if (rcData[THROTTLE] < MINCHECK) {
errorGyroI[ROLL] = 0;
errorGyroI[PITCH] = 0;
errorGyroI[YAW] = 0;
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
rcDelayCommand++;
if ( (rcData[YAW] < MINCHECK || rcData[ROLL] < MINCHECK) && armed == 1) {
if (rcDelayCommand == 20) { // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
armed = 0;
writeAllMotors(MINCOMMAND);
}
} else if (rcData[YAW] < MINCHECK && rcData[PITCH] < MINCHECK && armed == 0) {
if (rcDelayCommand == 20) calibratingG = 400;
} else if ( (rcData[YAW] > MAXCHECK || rcData[ROLL] > MAXCHECK) && rcData[PITCH] < MAXCHECK && armed == 0 && calibratingG == 0 && calibratedACC == 1) {
if (rcDelayCommand == 20) {
armed = 1;
writeAllMotors(MINTHROTTLE);
}
} else if (rcData[YAW] > MAXCHECK && rcData[PITCH] > MAXCHECK && armed == 0) {
if (rcDelayCommand == 20) {
atomicServo[0] = 125; //we center the yaw gyro in conf mode
#if defined(LCD_CONF)
configurationLoop(); //beginning LCD configuration
#endif
previousTime = micros();
}
} else {
rcDelayCommand = 0;
}
} else if (rcData[THROTTLE] > MAXCHECK && armed == 0) {
if (rcData[YAW] < MINCHECK && rcData[PITCH] < MINCHECK) {
if (rcDelayCommand == 20) calibratingA = 400;
rcDelayCommand++;
} else if (rcData[PITCH] > MAXCHECK) {
accZero[PITCH]++; writeParams();
} else if (rcData[PITCH] < MINCHECK) {
accZero[PITCH]--; writeParams();
} else if (rcData[ROLL] > MAXCHECK) {
accZero[ROLL]++; writeParams();
} else if (rcData[ROLL] < MINCHECK) {
accZero[ROLL]--; writeParams();
} else {
rcDelayCommand = 0;
}
}
rcOptions = (rcData[AUX1] < 1300) + (1300 < rcData[AUX1] && rcData[AUX1] < 1700) * 2 + (rcData[AUX1] > 1700) * 4
+ (rcData[AUX2] < 1300) * 8 + (1300 < rcData[AUX2] && rcData[AUX2] < 1700) * 16 + (rcData[AUX2] > 1700) * 32;
//note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
if (((rcOptions & activateAcc8) || (failsafeCnt > 5 * FAILSAVE_DELAY) ) && (nunchukPresent == 1 || accPresent == 1)) accMode = 1; else accMode = 0; // modified by MIS for failsave support
#if defined(BMP085)
if (rcOptions & activateBaro8 && baroPresent == 1) {
if (baroMode == 0) {
baroMode = 1;
altitudeHold = altitudeSmooth;
}
} else baroMode = 0;
#endif
#if defined(HMC5843) || defined(HMC5883)
if (rcOptions & activateMag8 && magPresent == 1) {
if (magMode == 0) {
magMode = 1;
magHold = heading;
}
} else magMode = 0;
#endif
rcTime = currentTime;
}
#if defined(HMC5843) || defined(HMC5883)
i2c_Mag_getADC();
#endif
#if defined(BMP085)
i2c_Baro_update();
#endif
computeIMU();
// Measure loop rate just afer reading the sensors
currentTime = micros();
cycleTime = currentTime - previousTime;
previousTime = currentTime;
#if defined(BMP085)
if (baroMode) rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE] - 3 * (altitudeSmooth - altitudeHold), max(rcCommand[THROTTLE] - 200, MINTHROTTLE), min(rcCommand[THROTTLE] + 200, MAXTHROTTLE));
#endif
#if defined(HMC5843) || defined(HMC5883)
if (-50 < rcCommand[YAW] && rcCommand[YAW] < 50 && magMode == 1) {
int16_t dif = heading - magHold;
if (dif <= - 180) dif += 360;
if (dif >= + 180) dif -= 360;
rcCommand[YAW] -= dif;
magTime = micros();
} else {
if (micros() - magTime > 1000 ) { //we add a small timing (1s) to reselect the new compass hold angle
magHold = heading;
}
}
#endif
#ifdef BI
static int16_t gyroPitchSmooth = 0;
gyroData[PITCH] = ((int32_t)gyroPitchSmooth * 10 + gyroData[PITCH] + 5) / 11;
gyroPitchSmooth = gyroData[PITCH];
#endif
//**** PITCH & ROLL & YAW PID ****
for (axis = 0; axis < 3; axis++) {
if (accMode == 1 && axis < 2 ) { //LEVEL MODE
errorAngle = rcCommand[axis] / 2 - angle[axis] / 2;
PTerm = (errorAngle) * PLEVEL8 / 50 - gyroData[axis] * dynP8[axis] / 10;
errorAngleI[axis] += errorAngle;
errorAngleI[axis] = constrain(errorAngleI[axis], -5000, +5000); //WindUp
ITerm = errorAngleI[axis] * ILEVEL8 / 2000;
} else { //ACRO MODE or YAW axis
error = rcCommand[axis] * 10 / P8[axis] - gyroData[axis];
PTerm = rcCommand[axis] - gyroData[axis] * dynP8[axis] / 10;
errorGyroI[axis] += error;
errorGyroI[axis] = constrain(errorGyroI[axis], -2000, +2000); //WindUp
if (abs(gyroData[axis]) > 80) errorGyroI[axis] = 0;
ITerm = errorGyroI[axis] * I8[axis] / 1000;
}
delta = gyroData[axis] - lastGyro[axis];
DTerm = (delta1[axis] + delta2[axis] + delta + 1) * dynD8[axis] / 3;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
lastGyro[axis] = gyroData[axis];
axisPID[axis] = PTerm + ITerm - DTerm;
}
#if NUMBER_MOTOR > 3
//prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW]));
#endif
#ifdef BI
motor[0] = rcCommand[THROTTLE] + axisPID[ROLL]; //LEFT
motor[1] = rcCommand[THROTTLE] - axisPID[ROLL]; //RIGHT
servo[0] = constrain(1500 + YAW_DIRECTION * (axisPID[YAW] + axisPID[PITCH]), 1020, 2000); //LEFT
servo[1] = constrain(1500 + YAW_DIRECTION * (axisPID[YAW] - axisPID[PITCH]), 1020, 2000); //RIGHT
#endif
#ifdef TRI
motor[0] = rcCommand[THROTTLE] + axisPID[PITCH] * 4 / 3 ; //REAR
motor[1] = rcCommand[THROTTLE] - axisPID[ROLL] - axisPID[PITCH] * 2 / 3 ; //RIGHT
motor[2] = rcCommand[THROTTLE] + axisPID[ROLL] - axisPID[PITCH] * 2 / 3 ; //LEFT
servo[0] = constrain(TRI_YAW_MIDDLE + YAW_DIRECTION * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
#endif
#ifdef QUADP
motor[0] = rcCommand[THROTTLE] + axisPID[PITCH] - YAW_DIRECTION * axisPID[YAW]; //REAR
motor[1] = rcCommand[THROTTLE] - axisPID[ROLL] + YAW_DIRECTION * axisPID[YAW]; //RIGHT
motor[2] = rcCommand[THROTTLE] + axisPID[ROLL] + YAW_DIRECTION * axisPID[YAW]; //LEFT
motor[3] = rcCommand[THROTTLE] - axisPID[PITCH] - YAW_DIRECTION * axisPID[YAW]; //FRONT
#endif
#ifdef QUADX
motor[0] = rcCommand[THROTTLE] - axisPID[ROLL] + axisPID[PITCH] - YAW_DIRECTION * axisPID[YAW]; //REAR_R
motor[1] = rcCommand[THROTTLE] - axisPID[ROLL] - axisPID[PITCH] + YAW_DIRECTION * axisPID[YAW]; //FRONT_R
motor[2] = rcCommand[THROTTLE] + axisPID[ROLL] + axisPID[PITCH] + YAW_DIRECTION * axisPID[YAW]; //REAR_L
motor[3] = rcCommand[THROTTLE] + axisPID[ROLL] - axisPID[PITCH] - YAW_DIRECTION * axisPID[YAW]; //FRONT_L
#endif
#ifdef Y4
motor[0] = rcCommand[THROTTLE] + axisPID[PITCH] - YAW_DIRECTION * axisPID[YAW]; //REAR_1
motor[1] = rcCommand[THROTTLE] - axisPID[ROLL] - axisPID[PITCH]; //FRONT_R
motor[2] = rcCommand[THROTTLE] + axisPID[PITCH] + YAW_DIRECTION * axisPID[YAW]; //REAR_2
motor[3] = rcCommand[THROTTLE] + axisPID[ROLL] - axisPID[PITCH]; //FRONT_L
#endif
#ifdef Y6
motor[0] = rcCommand[THROTTLE] + axisPID[PITCH] * 4 / 3 + YAW_DIRECTION * axisPID[YAW]; //REAR
motor[1] = rcCommand[THROTTLE] - axisPID[ROLL] - axisPID[PITCH] * 2 / 3 - YAW_DIRECTION * axisPID[YAW]; //RIGHT
motor[2] = rcCommand[THROTTLE] + axisPID[ROLL] - axisPID[PITCH] * 2 / 3 - YAW_DIRECTION * axisPID[YAW]; //LEFT
motor[3] = rcCommand[THROTTLE] + axisPID[PITCH] * 4 / 3 - YAW_DIRECTION * axisPID[YAW]; //UNDER_REAR
motor[4] = rcCommand[THROTTLE] - axisPID[ROLL] - axisPID[PITCH] * 2 / 3 + YAW_DIRECTION * axisPID[YAW]; //UNDER_RIGHT
motor[5] = rcCommand[THROTTLE] + axisPID[ROLL] - axisPID[PITCH] * 2 / 3 + YAW_DIRECTION * axisPID[YAW]; //UNDER_LEFT
#endif
#ifdef HEX6
motor[0] = rcCommand[THROTTLE] - axisPID[ROLL] / 2 + axisPID[PITCH] / 2 + YAW_DIRECTION * axisPID[YAW]; //REAR_R
motor[1] = rcCommand[THROTTLE] - axisPID[ROLL] / 2 - axisPID[PITCH] / 2 - YAW_DIRECTION * axisPID[YAW]; //FRONT_R
motor[2] = rcCommand[THROTTLE] + axisPID[ROLL] / 2 + axisPID[PITCH] / 2 + YAW_DIRECTION * axisPID[YAW]; //REAR_L
motor[3] = rcCommand[THROTTLE] + axisPID[ROLL] / 2 - axisPID[PITCH] / 2 - YAW_DIRECTION * axisPID[YAW]; //FRONT_L
motor[4] = rcCommand[THROTTLE] - axisPID[PITCH] + YAW_DIRECTION * axisPID[YAW]; //FRONT
motor[5] = rcCommand[THROTTLE] + axisPID[PITCH] - YAW_DIRECTION * axisPID[YAW]; //REAR
#endif
#ifdef HEX6X
motor[0] = rcCommand[THROTTLE] - axisPID[ROLL] / 2 + axisPID[PITCH] / 2 + YAW_DIRECTION * axisPID[YAW]; //REAR_R
motor[1] = rcCommand[THROTTLE] - axisPID[ROLL] / 2 - axisPID[PITCH] / 2 + YAW_DIRECTION * axisPID[YAW]; //FRONT_R
motor[2] = rcCommand[THROTTLE] + axisPID[ROLL] / 2 + axisPID[PITCH] / 2 - YAW_DIRECTION * axisPID[YAW]; //REAR_L
motor[3] = rcCommand[THROTTLE] + axisPID[ROLL] / 2 - axisPID[PITCH] / 2 - YAW_DIRECTION * axisPID[YAW]; //FRONT_L
motor[4] = rcCommand[THROTTLE] - axisPID[ROLL] - YAW_DIRECTION * axisPID[YAW]; //RIGHT
motor[5] = rcCommand[THROTTLE] + axisPID[ROLL] + YAW_DIRECTION * axisPID[YAW]; //LEFT
#endif
#ifdef SERVO_TILT
if (rcOptions & activateCamStab8 ) {
servo[1] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] / 16 , TILT_PITCH_MIN, TILT_PITCH_MAX);
servo[2] = constrain(TILT_ROLL_MIDDLE + TILT_ROLL_PROP * angle[ROLL] / 16 , TILT_ROLL_MIN, TILT_ROLL_MAX);
} else {
servo[1] = TILT_PITCH_MIDDLE;
servo[2] = TILT_ROLL_MIDDLE;
}
#endif
#ifdef GIMBAL
servo[1] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] / 16 + rcCommand[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX);
servo[2] = constrain(TILT_ROLL_MIDDLE + TILT_ROLL_PROP * angle[ROLL] / 16 + rcCommand[ROLL], TILT_ROLL_MIN, TILT_ROLL_MAX);
#endif
#ifdef FLYING_WING
servo[1] = constrain(1500 + axisPID[PITCH] - axisPID[ROLL], 1020, 2000); //LEFT the direction of the 2 servo can be changed here: invert the sign before axisPID
servo[2] = constrain(1500 + axisPID[PITCH] + axisPID[ROLL], 1020, 2000); //RIGHT
#endif
maxMotor = motor[0];
for (i = 1; i < NUMBER_MOTOR; i++)
if (motor[i] > maxMotor) maxMotor = motor[i];
for (i = 0; i < NUMBER_MOTOR; i++) {
if (maxMotor > MAXTHROTTLE) // this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxMotor - MAXTHROTTLE;
motor[i] = constrain(motor[i], MINTHROTTLE, MAXTHROTTLE);
if ((rcData[THROTTLE]) < MINCHECK)
#ifndef MOTOR_STOP
motor[i] = MINTHROTTLE;
#else
motor[i] = MINCOMMAND;
#endif
if (armed == 0)
motor[i] = MINCOMMAND;
}
#if defined(CAMTRIG)
if (camCycle == 1) {
if (camState == 0) {
servo[3] = CAM_SERVO_HIGH;
camState = 1;
camTime = millis();
} else if (camState == 1) {
if ( (millis() - camTime) > CAM_TIME_HIGH ) {
servo[3] = CAM_SERVO_LOW;
camState = 2;
camTime = millis();
}
} else { //camState ==2
if ( (millis() - camTime) > CAM_TIME_LOW ) {
camState = 0;
camCycle = 0;
}
}
}
if (rcOptions & activateCamTrig8) camCycle = 1;
#endif
#if defined(SERVO)
atomicServo[0] = (servo[0] - 1000) / 4;
atomicServo[1] = (servo[1] - 1000) / 4;
atomicServo[2] = (servo[2] - 1000) / 4;
atomicServo[3] = (servo[3] - 1000) / 4;
#endif
writeMotors();
}
static uint8_t point;
static uint8_t s[128];
void serialize16(int16_t a) {
s[point++] = a;
s[point++] = a >> 8 & 0xff;
}
void serialize8(uint8_t a) {
s[point++] = a;
}
// ***********************************
// Interrupt driven UART transmitter for MIS_OSD
// ***********************************
static uint8_t tx_ptr;
ISR_UART {
UDR0 = s[tx_ptr++]; /* Transmit next byte */
if ( tx_ptr == point ) /* Check if all data is transmitted */
UCSR0B &= ~(1 << UDRIE0); /* Disable transmitter UDRE interrupt */
}
void UartSendData() { // start of the data block transmission
tx_ptr = 0;
UCSR0A |= (1 << UDRE0); /* Clear UDRE interrupt flag */
UCSR0B |= (1 << UDRIE0); /* Enable transmitter UDRE interrupt */
UDR0 = s[tx_ptr++]; /* Start transmission */
}
void serialCom() {
int16_t a;
uint8_t i;
if (Serial.available()) {
switch (Serial.read()) {
case 'A': //arduino to GUI all data
point = 0;
serialize8('A');
for (i = 0; i < 3; i++) serialize16(accSmooth[i]);
for (i = 0; i < 3; i++) serialize16(gyroData[i]); //12
serialize16(altitudeSmooth);
serialize16(heading); // compass
for (i = 0; i < 4; i++) serialize16(servo[i]); //24
for (i = 0; i < 6; i++) serialize16(motor[i]); //36
for (i = 0; i < 8; i++) serialize16(rcHysteresis[i]); //52
serialize8(nunchukPresent | accPresent << 1 | baroPresent << 2 | magPresent << 3);
serialize8(accMode | baroMode << 1 | magMode << 2);
serialize16(cycleTime);
for (i = 0; i < 2; i++) serialize16(angle[i] / 10); //60
#if defined(TRI)
serialize8(1);
#elif defined(QUADP)
serialize8(2);
#elif defined(QUADX)
serialize8(3);
#elif defined(BI)
serialize8(4);
#elif defined(GIMBAL)
serialize8(5);
#elif defined(Y6)
serialize8(6);
#elif defined(HEX6)
serialize8(7);
#elif defined(FLYING_WING)
serialize8(8);
#elif defined(Y4)
serialize8(9);
#elif defined(HEX6X)
serialize8(10);
#endif
for (i = 0; i < 3; i++) {
serialize8(P8[i]); //70
serialize8(I8[i]);
serialize8(D8[i]);
}
serialize8(PLEVEL8); serialize8(ILEVEL8);
serialize8(rcRate8); serialize8(rcExpo8);
serialize8(rollPitchRate); serialize8(yawRate);
serialize8(dynThrPID);
serialize8(activateAcc8); serialize8(activateBaro8); serialize8(activateMag8); //80
serialize8(activateCamStab8); serialize8(activateCamTrig8); //82
serialize8('A');
Serial.write(s, point);
break;
case 'O': // arduino to OSD data - contribution from MIS
point = 0;
serialize8('O');
for (i = 0; i < 3; i++) serialize16(accSmooth[i]);
for (i = 0; i < 3; i++) serialize16(gyroData[i]);
serialize16(altitudeSmooth);
serialize16(heading); // compass - 16 bytes
for (i = 0; i < 2; i++) serialize16(angle[i]); //20
for (i = 0; i < 6; i++) serialize16(motor[i]); //32
for (i = 0; i < 6; i++) {
serialize16(rcHysteresis[i]); //44
}
serialize8(nunchukPresent | accPresent << 1 | baroPresent << 2 | magPresent << 3);
serialize8(accMode | baroMode << 1 | magMode << 2);
serialize8(vbat); // Vbatt 47
serialize8(17); // MultiWii Firmware version
serialize8('O'); //49
UartSendData();
break;
case 'C': //GUI to arduino param
while (Serial.available() < 21) {}
for (i = 0; i < 3; i++) {
P8[i] = Serial.read();
I8[i] = Serial.read();
D8[i] = Serial.read();
}
PLEVEL8 = Serial.read(); ILEVEL8 = Serial.read();
rcRate8 = Serial.read(); rcExpo8 = Serial.read();
rollPitchRate = Serial.read(); yawRate = Serial.read();
dynThrPID = Serial.read();
activateAcc8 = Serial.read(); activateBaro8 = Serial.read(); activateMag8 = Serial.read();
activateCamStab8 = Serial.read(); activateCamTrig8 = Serial.read();
writeParams();
break;
case 'D': //GUI to arduino calibration request
calibratingA = 400;
break;
}
}
}
EDIT : remove the //I had added on line 2216