785 lines
30 KiB
C++
Executable File
785 lines
30 KiB
C++
Executable File
/*
|
|
MultiWiiCopter by Alexandre Dubus
|
|
www.multiwii.com
|
|
December 2011 V1.dev
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
any later version. see <http://www.gnu.org/licenses/>
|
|
*/
|
|
|
|
#include "config.h"
|
|
#include "def.h"
|
|
#include <avr/pgmspace.h>
|
|
#define VERSION 19
|
|
|
|
/*********** RC alias *****************/
|
|
#define ROLL 0
|
|
#define PITCH 1
|
|
#define YAW 2
|
|
#define THROTTLE 3
|
|
#define AUX1 4
|
|
#define AUX2 5
|
|
#define AUX3 6
|
|
#define AUX4 7
|
|
|
|
#define PIDALT 3
|
|
#define PIDVEL 4
|
|
#define PIDGPS 5
|
|
#define PIDLEVEL 6
|
|
#define PIDMAG 7
|
|
|
|
#define BOXACC 0
|
|
#define BOXBARO 1
|
|
#define BOXMAG 2
|
|
#define BOXCAMSTAB 3
|
|
#define BOXCAMTRIG 4
|
|
#define BOXARM 5
|
|
#define BOXGPSHOME 6
|
|
#define BOXGPSHOLD 7
|
|
#define BOXPASSTHRU 8
|
|
#define BOXHEADFREE 9
|
|
#define BOXBEEPERON 10
|
|
|
|
#define CHECKBOXITEMS 11
|
|
#define PIDITEMS 8
|
|
|
|
static uint32_t currentTime = 0;
|
|
static uint16_t previousTime = 0;
|
|
static uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
|
|
static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
|
|
static uint8_t calibratingM = 0;
|
|
static uint16_t calibratingG;
|
|
static uint8_t armed = 0;
|
|
static uint16_t acc_1G; // this is the 1G measured acceleration
|
|
static int16_t acc_25deg;
|
|
static uint8_t nunchuk = 0;
|
|
static uint8_t accMode = 0; // if level mode is a activated
|
|
static uint8_t magMode = 0; // if compass heading hold is a activated
|
|
static uint8_t baroMode = 0; // if altitude hold is activated
|
|
static uint8_t GPSModeHome = 0; // if GPS RTH is activated
|
|
static uint8_t GPSModeHold = 0; // if GPS PH is activated
|
|
static uint8_t headFreeMode = 0; // if head free mode is a activated
|
|
static uint8_t passThruMode = 0; // if passthrough mode is activated
|
|
static int16_t headFreeModeHold;
|
|
static int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
|
|
static int16_t accTrim[2] = { 0, 0 };
|
|
static int16_t heading, magHold;
|
|
static uint8_t calibratedACC = 0;
|
|
static uint8_t vbat; // battery voltage in 0.1V steps
|
|
static uint8_t okToArm = 0;
|
|
static uint8_t rcOptions1, rcOptions2;
|
|
static int32_t pressure;
|
|
static int16_t BaroAlt;
|
|
static int16_t EstAlt; // in cm
|
|
static int16_t zVelocity;
|
|
static uint8_t buzzerState = 0;
|
|
static int16_t debug1, debug2, debug3, debug4;
|
|
|
|
//for log
|
|
static uint16_t cycleTimeMax = 0; // highest ever cycle timen
|
|
static uint16_t cycleTimeMin = 65535; // lowest ever cycle timen
|
|
static uint16_t powerMax = 0; // highest ever current
|
|
|
|
static int16_t i2c_errors_count = 0;
|
|
static int16_t annex650_overrun_count = 0;
|
|
|
|
// **********************
|
|
//Automatic ACC Offset Calibration
|
|
// **********************
|
|
static uint16_t InflightcalibratingA = 0;
|
|
static int16_t AccInflightCalibrationArmed;
|
|
static uint16_t AccInflightCalibrationMeasurementDone = 0;
|
|
static uint16_t AccInflightCalibrationSavetoEEProm = 0;
|
|
|
|
// **********************
|
|
// power meter
|
|
// **********************
|
|
#define PMOTOR_SUM 8 // index into pMeter[] for sum
|
|
static uint32_t pMeter[PMOTOR_SUM + 1]; //we use [0:7] for eight motors,one extra for sum
|
|
static uint8_t pMeterV; // dummy to satisfy the paramStruct logic in ConfigurationLoop()
|
|
static uint32_t pAlarm; // we scale the eeprom value from [0:255] to this value we can directly compare to the sum in pMeter[6]
|
|
static uint8_t powerTrigger1 = 0; // trigger for alarm based on power consumption
|
|
static uint16_t powerValue = 0; // last known current
|
|
static uint16_t intPowerMeterSum, intPowerTrigger1;
|
|
|
|
// **********************
|
|
// telemetry
|
|
// **********************
|
|
static uint8_t telemetry = 0;
|
|
static uint8_t telemetry_auto = 0;
|
|
|
|
// ******************
|
|
// rc functions
|
|
// ******************
|
|
#define MINCHECK 1100
|
|
#define MAXCHECK 1900
|
|
|
|
volatile int16_t failsafeCnt = 0;
|
|
static int16_t failsafeEvents = 0;
|
|
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 uint8_t rcRate8;
|
|
static uint8_t rcExpo8;
|
|
static int16_t lookupRX[7]; // lookup table for expo & RC rate
|
|
volatile uint8_t rcFrameComplete; //for serial rc receiver Spektrum
|
|
|
|
|
|
// **************
|
|
// gyro+acc IMU
|
|
// **************
|
|
static int16_t gyroData[3] = { 0, 0, 0 };
|
|
static int16_t gyroZero[3] = { 0, 0, 0 };
|
|
static int16_t accZero[3] = { 0, 0, 0 };
|
|
static int16_t magZero[3] = { 0, 0, 0 };
|
|
static int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
|
|
static int8_t smallAngle25 = 1;
|
|
|
|
// *************************
|
|
// motor and servo functions
|
|
// *************************
|
|
static int16_t axisPID[3];
|
|
static int16_t motor[8];
|
|
static int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
|
|
static uint16_t wing_left_mid = WING_LEFT_MID;
|
|
static uint16_t wing_right_mid = WING_RIGHT_MID;
|
|
static uint16_t tri_yaw_middle = TRI_YAW_MIDDLE;
|
|
|
|
// **********************
|
|
// EEPROM & LCD functions
|
|
// **********************
|
|
static uint8_t P8[8], I8[8], D8[8]; //8 bits is much faster and the code is much shorter
|
|
static uint8_t dynP8[3], dynI8[3], dynD8[3];
|
|
static uint8_t rollPitchRate;
|
|
static uint8_t yawRate;
|
|
static uint8_t dynThrPID;
|
|
static uint8_t activate1[CHECKBOXITEMS];
|
|
static uint8_t activate2[CHECKBOXITEMS];
|
|
|
|
// **********************
|
|
// GPS
|
|
// **********************
|
|
static int32_t GPS_latitude, GPS_longitude;
|
|
static int32_t GPS_latitude_home, GPS_longitude_home;
|
|
static uint8_t GPS_fix, GPS_fix_home = 0;
|
|
static uint8_t GPS_numSat;
|
|
static uint16_t GPS_distanceToHome; // in meters
|
|
static int16_t GPS_directionToHome = 0; // in degrees
|
|
static uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update
|
|
static int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction
|
|
|
|
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_TOGGLE; //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 buzzerTime, calibratedAccTime;
|
|
#if defined(LCD_TELEMETRY)
|
|
static uint16_t telemetryTimer = 0, telemetryAutoTimer = 0, psensorTimer = 0;
|
|
#endif
|
|
#if defined(LCD_TELEMETRY)
|
|
static uint8_t telemetryAutoIndex = 0;
|
|
static char telemetryAutoSequence[] = LCD_TELEMETRY_AUTO;
|
|
#endif
|
|
#ifdef VBAT
|
|
static uint8_t vbatTimer = 0;
|
|
#endif
|
|
static uint8_t buzzerFreq; //delay between buzzer ring
|
|
uint8_t axis, prop1, prop2;
|
|
#if defined(POWERMETER_HARD)
|
|
uint16_t pMeterRaw; //used for current reading
|
|
#endif
|
|
|
|
//PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
|
|
if (rcData[THROTTLE] < 1500) {
|
|
prop2 = 100;
|
|
} else if (rcData[THROTTLE] < 2000) {
|
|
prop2 = 100 - (uint16_t) dynThrPID *(rcData[THROTTLE] - 1500) / 500;
|
|
} else {
|
|
prop2 = 100 - dynThrPID;
|
|
}
|
|
|
|
for (axis = 0; axis < 3; axis++) {
|
|
uint16_t tmp = min(abs(rcData[axis] - MIDRC), 500);
|
|
#if defined(DEADBAND)
|
|
if (tmp > DEADBAND) {
|
|
tmp -= DEADBAND;
|
|
} else {
|
|
tmp = 0;
|
|
}
|
|
#endif
|
|
if (axis != 2) { //ROLL & PITCH
|
|
uint16_t tmp2 = tmp / 100;
|
|
rcCommand[axis] = lookupRX[tmp2] + (tmp - tmp2 * 100) * (lookupRX[tmp2 + 1] - lookupRX[tmp2]) / 100;
|
|
prop1 = 100 - (uint16_t) rollPitchRate *tmp / 500;
|
|
prop1 = (uint16_t) prop1 *prop2 / 100;
|
|
} else { //YAW
|
|
rcCommand[axis] = tmp;
|
|
prop1 = 100 - (uint16_t) yawRate *tmp / 500;
|
|
}
|
|
dynP8[axis] = (uint16_t) P8[axis] * prop1 / 100;
|
|
dynD8[axis] = (uint16_t) D8[axis] * prop1 / 100;
|
|
if (rcData[axis] < MIDRC)
|
|
rcCommand[axis] = -rcCommand[axis];
|
|
}
|
|
rcCommand[THROTTLE] = MINTHROTTLE + (int32_t) (MAXTHROTTLE - MINTHROTTLE) * (rcData[THROTTLE] - MINCHECK) / (2000 - MINCHECK);
|
|
|
|
if (headFreeMode) {
|
|
float radDiff = (heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533
|
|
float cosDiff = cos(radDiff);
|
|
float sinDiff = sin(radDiff);
|
|
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
|
|
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
|
|
rcCommand[PITCH] = rcCommand_PITCH;
|
|
}
|
|
#if defined(POWERMETER_HARD)
|
|
if (!(++psensorTimer % PSENSORFREQ)) {
|
|
pMeterRaw = analogRead(PSENSORPIN);
|
|
powerValue = (PSENSORNULL > pMeterRaw ? PSENSORNULL - pMeterRaw : pMeterRaw - PSENSORNULL); // do not use abs(), it would induce implicit cast to uint and overrun
|
|
if (powerValue < 333) { // only accept reasonable values. 333 is empirical
|
|
#ifdef LOG_VALUES
|
|
if (powerValue > powerMax)
|
|
powerMax = powerValue;
|
|
#endif
|
|
} else {
|
|
powerValue = 333;
|
|
}
|
|
pMeter[PMOTOR_SUM] += (uint32_t) powerValue;
|
|
}
|
|
#endif
|
|
|
|
#if defined(VBAT)
|
|
static uint8_t ind = 0;
|
|
uint16_t vbatRaw = 0;
|
|
static uint16_t vbatRawArray[8];
|
|
if (!(++vbatTimer % VBATFREQ)) {
|
|
ADCSRA |= _BV(ADPS2);
|
|
ADCSRA &= ~_BV(ADPS1);
|
|
ADCSRA &= ~_BV(ADPS0); //this speeds up analogRead without loosing too much resolution: http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1208715493/11
|
|
vbatRawArray[(ind++) % 8] = analogRead(V_BATPIN);
|
|
for (uint8_t i = 0; i < 8; i++)
|
|
vbatRaw += vbatRawArray[i];
|
|
vbat = vbatRaw / (VBATSCALE / 2); // result is Vbatt in 0.1V steps
|
|
}
|
|
if ((rcOptions1 & activate1[BOXBEEPERON]) || (rcOptions2 & activate2[BOXBEEPERON])) { // unconditional beeper on via AUXn switch
|
|
buzzerFreq = 7;
|
|
} else if (((vbat > VBATLEVEL1_3S)
|
|
#if defined(POWERMETER)
|
|
&& ((pMeter[PMOTOR_SUM] < pAlarm) || (pAlarm == 0))
|
|
#endif
|
|
) || (NO_VBAT > vbat)) // ToLuSe
|
|
{ //VBAT ok AND powermeter ok, buzzer off
|
|
buzzerFreq = 0;
|
|
buzzerState = 0;
|
|
BUZZERPIN_OFF;
|
|
#if defined(POWERMETER)
|
|
} else if (pMeter[PMOTOR_SUM] > pAlarm) { // sound alarm for powermeter
|
|
buzzerFreq = 4;
|
|
#endif
|
|
} 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 ((calibratingA > 0 && (ACC || nunchuk)) || (calibratingG > 0)) { // Calibration phasis
|
|
LEDPIN_TOGGLE;
|
|
} else {
|
|
if (calibratedACC == 1) {
|
|
LEDPIN_OFF;
|
|
}
|
|
if (armed) {
|
|
LEDPIN_ON;
|
|
}
|
|
}
|
|
|
|
#if defined(LED_RING)
|
|
static uint32_t LEDTime;
|
|
if (currentTime > LEDTime) {
|
|
LEDTime = currentTime + 50000;
|
|
i2CLedRingState();
|
|
}
|
|
#endif
|
|
|
|
if (currentTime > calibratedAccTime) {
|
|
if (smallAngle25 == 0) {
|
|
calibratedACC = 0; //the multi uses ACC and is not calibrated or is too much inclinated
|
|
LEDPIN_TOGGLE;
|
|
calibratedAccTime = currentTime + 500000;
|
|
} else
|
|
calibratedACC = 1;
|
|
}
|
|
|
|
serialCom();
|
|
|
|
#if defined(POWERMETER)
|
|
intPowerMeterSum = (pMeter[PMOTOR_SUM] / PLEVELDIV);
|
|
intPowerTrigger1 = powerTrigger1 * PLEVELSCALE;
|
|
#endif
|
|
|
|
#ifdef LCD_TELEMETRY_AUTO
|
|
if ((telemetry_auto)
|
|
&& (!(++telemetryAutoTimer % LCD_TELEMETRY_AUTO_FREQ))) {
|
|
telemetry = telemetryAutoSequence[++telemetryAutoIndex % strlen(telemetryAutoSequence)];
|
|
LCDclear(); // make sure to clear away remnants
|
|
}
|
|
#endif
|
|
#ifdef LCD_TELEMETRY
|
|
if (!(++telemetryTimer % LCD_TELEMETRY_FREQ)) {
|
|
#if (LCD_TELEMETRY_DEBUG+0 > 0)
|
|
telemetry = LCD_TELEMETRY_DEBUG;
|
|
#endif
|
|
if (telemetry)
|
|
lcd_telemetry();
|
|
}
|
|
#endif
|
|
|
|
#if defined(GPS)
|
|
static uint32_t GPSLEDTime;
|
|
if (currentTime > GPSLEDTime && (GPS_fix_home == 1)) {
|
|
GPSLEDTime = currentTime + 150000;
|
|
LEDPIN_TOGGLE;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
|
|
void setup()
|
|
{
|
|
SerialOpen(0, 115200);
|
|
LEDPIN_PINMODE;
|
|
POWERPIN_PINMODE;
|
|
BUZZERPIN_PINMODE;
|
|
STABLEPIN_PINMODE;
|
|
POWERPIN_OFF;
|
|
initOutput();
|
|
readEEPROM();
|
|
checkFirstTime();
|
|
configureReceiver();
|
|
initSensors();
|
|
previousTime = micros();
|
|
#if defined(GIMBAL)
|
|
calibratingA = 400;
|
|
#endif
|
|
calibratingG = 400;
|
|
#if defined(POWERMETER)
|
|
for (uint8_t i = 0; i <= PMOTOR_SUM; i++)
|
|
pMeter[i] = 0;
|
|
#endif
|
|
#if defined(GPS)
|
|
SerialOpen(GPS_SERIAL, GPS_BAUD);
|
|
#endif
|
|
#if defined(LCD_ETPP)
|
|
initLCD();
|
|
#elif defined(LCD_LCD03)
|
|
initLCD();
|
|
#endif
|
|
#ifdef LCD_TELEMETRY_DEBUG
|
|
telemetry_auto = 1;
|
|
#endif
|
|
#ifdef LCD_CONF_DEBUG
|
|
configurationLoop();
|
|
#endif
|
|
|
|
|
|
}
|
|
|
|
// ******** 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, errorAngle;
|
|
int16_t delta, deltaSum;
|
|
int16_t PTerm, ITerm, DTerm;
|
|
static int16_t lastGyro[3] = { 0, 0, 0 };
|
|
static int16_t delta1[3], delta2[3];
|
|
static int16_t errorGyroI[3] = { 0, 0, 0 };
|
|
static int16_t errorAngleI[2] = { 0, 0 };
|
|
static uint32_t rcTime = 0;
|
|
static int16_t initialThrottleHold;
|
|
static int16_t errorAltitudeI = 0;
|
|
int16_t AltPID = 0;
|
|
static int16_t AltHold;
|
|
|
|
#if defined(SPEKTRUM)
|
|
if (rcFrameComplete)
|
|
computeRC();
|
|
#endif
|
|
|
|
if (currentTime > rcTime) { // 50Hz
|
|
rcTime = currentTime + 20000;
|
|
#if !(defined(SPEKTRUM) ||defined(BTSERIAL))
|
|
computeRC();
|
|
#endif
|
|
// 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++)
|
|
rcData[i] = MIDRC; // after specified guard time after RC signal is lost (in 0.1sec)
|
|
rcData[THROTTLE] = FAILSAVE_THR0TTLE;
|
|
if (failsafeCnt > 5 * (FAILSAVE_DELAY + FAILSAVE_OFF_DELAY)) { // Turn OFF motors after specified Time (in 0.1sec)
|
|
armed = 0; //This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
|
|
okToArm = 0; //to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
|
|
}
|
|
failsafeEvents++;
|
|
}
|
|
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[PITCH] < MINCHECK && armed == 0) {
|
|
if (rcDelayCommand == 20)
|
|
calibratingG = 400;
|
|
} else if (rcData[YAW] > MAXCHECK && rcData[PITCH] > MAXCHECK && armed == 0) {
|
|
if (rcDelayCommand == 20) {
|
|
servo[0] = 1500; //we center the yaw gyro in conf mode
|
|
writeServos();
|
|
#if defined(LCD_CONF)
|
|
configurationLoop(); //beginning LCD configuration
|
|
#endif
|
|
previousTime = micros();
|
|
}
|
|
}
|
|
#if defined(InflightAccCalibration)
|
|
else if (armed == 0 && rcData[YAW] < MINCHECK && rcData[PITCH] > MAXCHECK && rcData[ROLL] > MAXCHECK) {
|
|
if (rcDelayCommand == 20) {
|
|
if (AccInflightCalibrationMeasurementDone) { //trigger saving into eeprom after landing
|
|
AccInflightCalibrationMeasurementDone = 0;
|
|
AccInflightCalibrationSavetoEEProm = 1;
|
|
} else {
|
|
AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
|
|
if (AccInflightCalibrationArmed) {
|
|
blinkLED(10, 1, 2);
|
|
} else {
|
|
blinkLED(10, 10, 3);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
else if ((activate1[BOXARM] > 0) || (activate2[BOXARM] > 0)) {
|
|
if (((rcOptions1 & activate1[BOXARM])
|
|
|| (rcOptions2 & activate2[BOXARM])) && okToArm) {
|
|
armed = 1;
|
|
headFreeModeHold = heading;
|
|
} else if (armed)
|
|
armed = 0;
|
|
rcDelayCommand = 0;
|
|
} else if ((rcData[YAW] < MINCHECK || rcData[ROLL] < MINCHECK)
|
|
&& armed == 1) {
|
|
if (rcDelayCommand == 20)
|
|
armed = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
|
|
} else if ((rcData[YAW] > MAXCHECK || rcData[ROLL] > MAXCHECK)
|
|
&& rcData[PITCH] < MAXCHECK && armed == 0 && calibratingG == 0 && calibratedACC == 1) {
|
|
if (rcDelayCommand == 20) {
|
|
armed = 1;
|
|
headFreeModeHold = heading;
|
|
}
|
|
#ifdef LCD_TELEMETRY_AUTO
|
|
} else if (rcData[ROLL] < MINCHECK && rcData[PITCH] > MAXCHECK && armed == 0) {
|
|
if (rcDelayCommand == 20) {
|
|
if (telemetry_auto) {
|
|
telemetry_auto = 0;
|
|
telemetry = 0;
|
|
} else
|
|
telemetry_auto = 1;
|
|
}
|
|
#endif
|
|
} else
|
|
rcDelayCommand = 0;
|
|
} else if (rcData[THROTTLE] > MAXCHECK && armed == 0) {
|
|
if (rcData[YAW] < MINCHECK && rcData[PITCH] < MINCHECK) { //throttle=max, yaw=left, pitch=min
|
|
if (rcDelayCommand == 20)
|
|
calibratingA = 400;
|
|
rcDelayCommand++;
|
|
} else if (rcData[YAW] > MAXCHECK && rcData[PITCH] < MINCHECK) { //throttle=max, yaw=right, pitch=min
|
|
if (rcDelayCommand == 20)
|
|
calibratingM = 1; // MAG calibration request
|
|
rcDelayCommand++;
|
|
} else if (rcData[PITCH] > MAXCHECK) {
|
|
accTrim[PITCH] += 2;
|
|
writeParams();
|
|
#if defined(LED_RING)
|
|
blinkLedRing();
|
|
#endif
|
|
} else if (rcData[PITCH] < MINCHECK) {
|
|
accTrim[PITCH] -= 2;
|
|
writeParams();
|
|
#if defined(LED_RING)
|
|
blinkLedRing();
|
|
#endif
|
|
} else if (rcData[ROLL] > MAXCHECK) {
|
|
accTrim[ROLL] += 2;
|
|
writeParams();
|
|
#if defined(LED_RING)
|
|
blinkLedRing();
|
|
#endif
|
|
} else if (rcData[ROLL] < MINCHECK) {
|
|
accTrim[ROLL] -= 2;
|
|
writeParams();
|
|
#if defined(LED_RING)
|
|
blinkLedRing();
|
|
#endif
|
|
} else {
|
|
rcDelayCommand = 0;
|
|
}
|
|
}
|
|
#ifdef LOG_VALUES
|
|
if (cycleTime > cycleTimeMax)
|
|
cycleTimeMax = cycleTime; // remember highscore
|
|
if (cycleTime < cycleTimeMin)
|
|
cycleTimeMin = cycleTime; // remember lowscore
|
|
#endif
|
|
|
|
#if defined(InflightAccCalibration)
|
|
if (AccInflightCalibrationArmed && armed == 1 && rcData[THROTTLE] > MINCHECK && !((rcOptions1 & activate1[BOXARM]) || (rcOptions2 & activate2[BOXARM]))) { // Copter is airborne and you are turning it off via boxarm : start measurement
|
|
InflightcalibratingA = 50;
|
|
AccInflightCalibrationArmed = 0;
|
|
}
|
|
if ((rcOptions1 & activate1[BOXPASSTHRU]) || (rcOptions2 & activate2[BOXPASSTHRU])) { //Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored
|
|
if (!AccInflightCalibrationArmed) {
|
|
AccInflightCalibrationArmed = 1;
|
|
InflightcalibratingA = 50;
|
|
}
|
|
} else if (AccInflightCalibrationMeasurementDone && armed == 0) {
|
|
AccInflightCalibrationArmed = 0;
|
|
AccInflightCalibrationMeasurementDone = 0;
|
|
AccInflightCalibrationSavetoEEProm = 1;
|
|
}
|
|
#endif
|
|
|
|
rcOptions1 = (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;
|
|
rcOptions2 = (rcData[AUX3] < 1300) + (1300 < rcData[AUX3]
|
|
&& rcData[AUX3] < 1700) * 2 + (rcData[AUX3] > 1700) * 4 + (rcData[AUX4] < 1300) * 8 + (1300 < rcData[AUX4]
|
|
&& rcData[AUX4] < 1700) * 16 + (rcData[AUX4] > 1700) * 32;
|
|
|
|
//note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
|
|
if (((rcOptions1 & activate1[BOXACC])
|
|
|| (rcOptions2 & activate2[BOXACC])
|
|
|| (failsafeCnt > 5 * FAILSAVE_DELAY)) && (ACC || nunchuk)) {
|
|
// bumpless transfer to Level mode
|
|
if (!accMode) {
|
|
errorAngleI[ROLL] = 0;
|
|
errorAngleI[PITCH] = 0;
|
|
accMode = 1;
|
|
}
|
|
} else
|
|
accMode = 0; // modified by MIS for failsave support
|
|
|
|
if ((rcOptions1 & activate1[BOXARM]) == 0 || (rcOptions2 & activate2[BOXARM]) == 0)
|
|
okToArm = 1;
|
|
if (accMode == 1)
|
|
STABLEPIN_ON;
|
|
else
|
|
STABLEPIN_OFF;
|
|
|
|
if (BARO) {
|
|
if ((rcOptions1 & activate1[BOXBARO])
|
|
|| (rcOptions2 & activate2[BOXBARO])) {
|
|
if (baroMode == 0) {
|
|
baroMode = 1;
|
|
AltHold = EstAlt;
|
|
initialThrottleHold = rcCommand[THROTTLE];
|
|
errorAltitudeI = 0;
|
|
}
|
|
} else
|
|
baroMode = 0;
|
|
}
|
|
if (MAG) {
|
|
if ((rcOptions1 & activate1[BOXMAG])
|
|
|| (rcOptions2 & activate2[BOXMAG])) {
|
|
if (magMode == 0) {
|
|
magMode = 1;
|
|
magHold = heading;
|
|
}
|
|
} else
|
|
magMode = 0;
|
|
if ((rcOptions1 & activate1[BOXHEADFREE])
|
|
|| (rcOptions2 & activate2[BOXHEADFREE])) {
|
|
if (headFreeMode == 0) {
|
|
headFreeMode = 1;
|
|
}
|
|
} else
|
|
headFreeMode = 0;
|
|
}
|
|
#if defined(GPS)
|
|
if ((rcOptions1 & activate1[BOXGPSHOME])
|
|
|| (rcOptions2 & activate2[BOXGPSHOME])) {
|
|
GPSModeHome = 1;
|
|
} else
|
|
GPSModeHome = 0;
|
|
if ((rcOptions1 & activate1[BOXGPSHOLD])
|
|
|| (rcOptions2 & activate2[BOXGPSHOLD])) {
|
|
GPSModeHold = 1;
|
|
} else
|
|
GPSModeHold = 0;
|
|
#endif
|
|
if ((rcOptions1 & activate1[BOXPASSTHRU])
|
|
|| (rcOptions2 & activate2[BOXPASSTHRU])) {
|
|
passThruMode = 1;
|
|
} else
|
|
passThruMode = 0;
|
|
}
|
|
|
|
computeIMU();
|
|
// Measure loop rate just afer reading the sensors
|
|
currentTime = micros();
|
|
cycleTime = currentTime - previousTime;
|
|
previousTime = currentTime;
|
|
|
|
if (MAG) {
|
|
if (abs(rcCommand[YAW]) < 70 && magMode) {
|
|
int16_t dif = heading - magHold;
|
|
if (dif <= -180)
|
|
dif += 360;
|
|
if (dif >= +180)
|
|
dif -= 360;
|
|
if (smallAngle25)
|
|
rcCommand[YAW] -= dif * P8[PIDMAG] / 30; //18 deg
|
|
} else
|
|
magHold = heading;
|
|
}
|
|
|
|
if (BARO) {
|
|
if (baroMode) {
|
|
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > 20) {
|
|
AltHold = EstAlt;
|
|
initialThrottleHold = rcCommand[THROTTLE];
|
|
errorAltitudeI = 0;
|
|
}
|
|
//**** Alt. Set Point stabilization PID ****
|
|
error = constrain(AltHold - EstAlt, -100, 100); // +/-10m, 1 decimeter accuracy
|
|
errorAltitudeI += error;
|
|
errorAltitudeI = constrain(errorAltitudeI, -5000, 5000);
|
|
|
|
PTerm = P8[PIDALT] * error / 10; // 16 bits is ok here
|
|
|
|
if (abs(error) > 5) // under 50cm error, we neutralize Iterm
|
|
ITerm = (int32_t) I8[PIDALT] * errorAltitudeI / 4000;
|
|
else
|
|
ITerm = 0;
|
|
|
|
AltPID = PTerm + ITerm;
|
|
|
|
//AltPID is reduced, depending of the zVelocity magnitude
|
|
AltPID = AltPID * (D8[PIDALT] - min(abs(zVelocity), D8[PIDALT] * 4 / 5)) / (D8[PIDALT] + 1);
|
|
debug3 = AltPID;
|
|
|
|
rcCommand[THROTTLE] = initialThrottleHold + constrain(AltPID, -100, +100);
|
|
}
|
|
}
|
|
#if defined(GPS)
|
|
if ((GPSModeHome == 1)) {
|
|
float radDiff = (GPS_directionToHome - heading) * 0.0174533f;
|
|
GPS_angle[ROLL] = constrain(P8[PIDGPS] * sin(radDiff) * GPS_distanceToHome / 10, -D8[PIDGPS] * 10, +D8[PIDGPS] * 10); // with P=5, 1 meter = 0.5deg inclination
|
|
GPS_angle[PITCH] = constrain(P8[PIDGPS] * cos(radDiff) * GPS_distanceToHome / 10, -D8[PIDGPS] * 10, +D8[PIDGPS] * 10); // max inclination = D deg
|
|
} else {
|
|
GPS_angle[ROLL] = 0;
|
|
GPS_angle[PITCH] = 0;
|
|
}
|
|
#endif
|
|
|
|
//**** PITCH & ROLL & YAW PID ****
|
|
for (axis = 0; axis < 3; axis++) {
|
|
if (accMode == 1 && axis < 2) { //LEVEL MODE
|
|
// 50 degrees max inclination
|
|
errorAngle = constrain(2 * rcCommand[axis] - GPS_angle[axis], -500, +500) - angle[axis] + accTrim[axis]; //16 bits is ok here
|
|
#ifdef LEVEL_PDF
|
|
PTerm = -(int32_t) angle[axis] * P8[PIDLEVEL] / 100;
|
|
#else
|
|
PTerm = (int32_t) errorAngle *P8[PIDLEVEL] / 100; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
|
|
#endif
|
|
PTerm = constrain(PTerm, -D8[PIDLEVEL], +D8[PIDLEVEL]);
|
|
|
|
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); //WindUp //16 bits is ok here
|
|
ITerm = ((int32_t) errorAngleI[axis] * I8[PIDLEVEL]) >> 12; //32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
|
|
} else { //ACRO MODE or YAW axis
|
|
if (abs(rcCommand[axis]) < 350)
|
|
error = rcCommand[axis] * 10 * 8 / P8[axis]; //16 bits is needed for calculation: 350*10*8 = 28000 16 bits is ok for result if P8>2 (P>0.2)
|
|
else
|
|
error = (int32_t) rcCommand[axis] * 10 * 8 / P8[axis]; //32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
|
|
error -= gyroData[axis];
|
|
|
|
PTerm = rcCommand[axis];
|
|
|
|
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); //WindUp //16 bits is ok here
|
|
if (abs(gyroData[axis]) > 640)
|
|
errorGyroI[axis] = 0;
|
|
ITerm = (errorGyroI[axis] / 125 * I8[axis]) >> 6; //16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
|
|
}
|
|
if (abs(gyroData[axis]) < 160)
|
|
PTerm -= gyroData[axis] * dynP8[axis] / 10 / 8; //16 bits is needed for calculation 160*200 = 32000 16 bits is ok for result
|
|
else
|
|
PTerm -= (int32_t) gyroData[axis] * dynP8[axis] / 10 / 8; //32 bits is needed for calculation
|
|
|
|
delta = gyroData[axis] - lastGyro[axis]; //16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
|
|
lastGyro[axis] = gyroData[axis];
|
|
deltaSum = delta1[axis] + delta2[axis] + delta;
|
|
delta2[axis] = delta1[axis];
|
|
delta1[axis] = delta;
|
|
|
|
if (abs(deltaSum) < 640)
|
|
DTerm = (deltaSum * dynD8[axis]) >> 5; //16 bits is needed for calculation 640*50 = 32000 16 bits is ok for result
|
|
else
|
|
DTerm = ((int32_t) deltaSum * dynD8[axis]) >> 5; //32 bits is needed for calculation
|
|
|
|
axisPID[axis] = PTerm + ITerm - DTerm;
|
|
}
|
|
|
|
mixTable();
|
|
writeServos();
|
|
writeMotors();
|
|
|
|
#if defined(GPS)
|
|
while (SerialAvailable(GPS_SERIAL)) {
|
|
if (GPS_newFrame(SerialRead(GPS_SERIAL))) {
|
|
if (GPS_update == 1)
|
|
GPS_update = 0;
|
|
else
|
|
GPS_update = 1;
|
|
if (GPS_fix == 1 && GPS_numSat == 4) {
|
|
if (GPS_fix_home == 0) {
|
|
GPS_fix_home = 1;
|
|
GPS_latitude_home = GPS_latitude;
|
|
GPS_longitude_home = GPS_longitude;
|
|
}
|
|
GPS_distance(GPS_latitude_home, GPS_longitude_home, GPS_latitude, GPS_longitude, &GPS_distanceToHome, &GPS_directionToHome);
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
}
|