mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-25 17:25:20 +03:00
moved source files around in preparation to adding makefile build way
added makefile (thx gke) added linker script (thx gke) moved startups into src directory as well no code changes. git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@105 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop
parent
aae8ef4c3d
commit
5091f3e9ff
45 changed files with 229 additions and 14182 deletions
678
src/mw.c
Executable file
678
src/mw.c
Executable file
|
|
@ -0,0 +1,678 @@
|
|||
#include "board.h"
|
||||
#include "mw.h"
|
||||
|
||||
// March 2012 V2.0_pre_version_1
|
||||
|
||||
#define CHECKBOXITEMS 11
|
||||
#define PIDITEMS 8
|
||||
|
||||
int16_t debug1, debug2, debug3, debug4;
|
||||
uint8_t buzzerState = 0;
|
||||
uint32_t currentTime = 0;
|
||||
uint32_t previousTime = 0;
|
||||
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
|
||||
uint8_t GPSModeHome = 0; // if GPS RTH is activated
|
||||
uint8_t GPSModeHold = 0; // if GPS PH is activated
|
||||
uint8_t headFreeMode = 0; // if head free mode is a activated
|
||||
uint8_t passThruMode = 0; // if passthrough mode is activated
|
||||
int16_t headFreeModeHold;
|
||||
int16_t annex650_overrun_count = 0;
|
||||
uint8_t armed = 0;
|
||||
uint8_t vbat; // battery voltage in 0.1V steps
|
||||
|
||||
volatile int16_t failsafeCnt = 0;
|
||||
int16_t failsafeEvents = 0;
|
||||
int16_t rcData[8]; // interval [1000;2000]
|
||||
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
|
||||
//uint8_t rcRate8;
|
||||
//uint8_t rcExpo8;
|
||||
int16_t lookupRX[7]; // lookup table for expo & RC rate
|
||||
|
||||
// uint8_t P8[8], I8[8], D8[8]; //8 bits is much faster and the code is much shorter
|
||||
uint8_t dynP8[3], dynI8[3], dynD8[3];
|
||||
// uint8_t rollPitchRate;
|
||||
// uint8_t yawRate;
|
||||
// uint8_t dynThrPID;
|
||||
// uint8_t activate1[CHECKBOXITEMS];
|
||||
// uint8_t activate2[CHECKBOXITEMS];
|
||||
uint8_t rcOptions[CHECKBOXITEMS];
|
||||
uint8_t okToArm = 0;
|
||||
uint8_t accMode = 0; // if level mode is a activated
|
||||
uint8_t magMode = 0; // if compass heading hold is a activated
|
||||
uint8_t baroMode = 0; // if altitude hold is activated
|
||||
|
||||
int16_t axisPID[3];
|
||||
volatile uint16_t rcValue[18] = { 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502 }; // interval [1000;2000]
|
||||
uint8_t rcChannel[8] = { ROLL, PITCH, THROTTLE, YAW, AUX1, AUX2, AUX3, AUX4 };
|
||||
|
||||
// **********************
|
||||
// GPS
|
||||
// **********************
|
||||
int32_t GPS_latitude, GPS_longitude;
|
||||
int32_t GPS_latitude_home, GPS_longitude_home;
|
||||
uint8_t GPS_fix, GPS_fix_home = 0;
|
||||
uint8_t GPS_numSat;
|
||||
uint16_t GPS_distanceToHome; // in meters
|
||||
int16_t GPS_directionToHome = 0; // in degrees
|
||||
uint16_t GPS_distanceToHome, GPS_distanceToHold; // distance to home or hold point in meters
|
||||
int16_t GPS_directionToHome, GPS_directionToHold; // direction to home or hol point in degrees
|
||||
uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update
|
||||
int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction
|
||||
|
||||
|
||||
//Automatic ACC Offset Calibration
|
||||
// **********************
|
||||
uint16_t InflightcalibratingA = 0;
|
||||
int16_t AccInflightCalibrationArmed;
|
||||
uint16_t AccInflightCalibrationMeasurementDone = 0;
|
||||
uint16_t AccInflightCalibrationSavetoEEProm = 0;
|
||||
uint16_t AccInflightCalibrationActive = 0;
|
||||
|
||||
// **********************
|
||||
// power meter
|
||||
// **********************
|
||||
#define PMOTOR_SUM 8 // index into pMeter[] for sum
|
||||
uint32_t pMeter[PMOTOR_SUM + 1]; //we use [0:7] for eight motors,one extra for sum
|
||||
uint8_t pMeterV; // dummy to satisfy the paramStruct logic in ConfigurationLoop()
|
||||
uint32_t pAlarm; // we scale the eeprom value from [0:255] to this value we can directly compare to the sum in pMeter[6]
|
||||
// uint8_t powerTrigger1 = 0;
|
||||
uint16_t powerValue = 0; // last known current
|
||||
uint16_t intPowerMeterSum, intPowerTrigger1;
|
||||
|
||||
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++) {
|
||||
LED0_TOGGLE; // switch LEDPIN state
|
||||
BEEP_ON;
|
||||
delay(wait);
|
||||
BEEP_OFF;
|
||||
}
|
||||
delay(60);
|
||||
}
|
||||
}
|
||||
|
||||
void annexCode(void)
|
||||
{ //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
|
||||
static uint8_t vbatTimer = 0;
|
||||
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
|
||||
static uint8_t ind = 0;
|
||||
uint16_t vbatRaw = 0;
|
||||
static uint16_t vbatRawArray[8];
|
||||
uint8_t i;
|
||||
|
||||
//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) cfg.dynThrPID *(rcData[THROTTLE] - 1500) / 500;
|
||||
} else {
|
||||
prop2 = 100 - cfg.dynThrPID;
|
||||
}
|
||||
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
uint16_t tmp = min(abs(rcData[axis] - cfg.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) cfg.rollPitchRate *tmp / 500;
|
||||
prop1 = (uint16_t) prop1 *prop2 / 100;
|
||||
} else { //YAW
|
||||
rcCommand[axis] = tmp;
|
||||
prop1 = 100 - (uint16_t) cfg.yawRate * tmp / 500;
|
||||
}
|
||||
dynP8[axis] = (uint16_t) cfg.P8[axis] * prop1 / 100;
|
||||
dynD8[axis] = (uint16_t) cfg.D8[axis] * prop1 / 100;
|
||||
if (rcData[axis] < cfg.midrc)
|
||||
rcCommand[axis] = -rcCommand[axis];
|
||||
}
|
||||
rcCommand[THROTTLE] = cfg.minthrottle + (int32_t) (cfg.maxthrottle - cfg.minthrottle) * (rcData[THROTTLE] - MINCHECK) / (2000 - MINCHECK);
|
||||
|
||||
if (headFreeMode) {
|
||||
float radDiff = (heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533
|
||||
float cosDiff = cosf(radDiff);
|
||||
float sinDiff = sinf(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
|
||||
|
||||
#define ADC_REF_VOLTAGE 3.3f
|
||||
#define ADC_TO_VOLTAGE (ADC_REF_VOLTAGE / (1<<12)) // 12 bit ADC resolution
|
||||
#define ADC_VOLTS_PRECISION 12
|
||||
#define ADC_VOLTS_SLOPE ((10.0f + 1.0f) / 1.0f) // Rtop = 10K, Rbot = 1.0K
|
||||
#define ADC_TO_VOLTS ((ADC_TO_VOLTAGE / ((1<<(ADC_VOLTS_PRECISION))+1)) * ADC_VOLTS_SLOPE)
|
||||
|
||||
if (feature(FEATURE_VBAT)) {
|
||||
if (!(++vbatTimer % VBATFREQ)) {
|
||||
// avgVolts = adcAvgVolts * ADC_TO_VOLTS;
|
||||
vbatRawArray[(ind++) % 8] = adcGetBattery();
|
||||
for (i = 0; i < 8; i++)
|
||||
vbatRaw += vbatRawArray[i];
|
||||
vbat = vbatRaw / (VBATSCALE / 2); // result is Vbatt in 0.1V steps
|
||||
}
|
||||
if (rcOptions[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;
|
||||
BEEP_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;
|
||||
BEEP_OFF;
|
||||
buzzerTime = currentTime;
|
||||
} else if (!buzzerState && (currentTime > (buzzerTime + (2000000 >> buzzerFreq)))) {
|
||||
buzzerState = 1;
|
||||
BEEP_ON;
|
||||
buzzerTime = currentTime;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis
|
||||
LED0_TOGGLE;
|
||||
} else {
|
||||
if (calibratedACC == 1) {
|
||||
LED0_OFF;
|
||||
}
|
||||
if (armed) {
|
||||
LED0_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
|
||||
LED0_TOGGLE;
|
||||
calibratedAccTime = currentTime + 500000;
|
||||
} else
|
||||
calibratedACC = 1;
|
||||
}
|
||||
|
||||
serialCom();
|
||||
|
||||
#if defined(POWERMETER)
|
||||
intPowerMeterSum = (pMeter[PMOTOR_SUM] / PLEVELDIV);
|
||||
intPowerTrigger1 = cfg.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 GPS
|
||||
static uint32_t GPSLEDTime;
|
||||
if (currentTime > GPSLEDTime && (GPS_fix_home == 1)) {
|
||||
GPSLEDTime = currentTime + 150000;
|
||||
LEDPIN_TOGGLE;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
uint16_t readRawRC(uint8_t chan)
|
||||
{
|
||||
uint16_t data;
|
||||
|
||||
failsafeCnt = 0;
|
||||
data = pwmRead(rcChannel[chan]);
|
||||
if (data < 750 || data > 2250)
|
||||
data = 1500;
|
||||
|
||||
return data;
|
||||
}
|
||||
|
||||
void computeRC(void)
|
||||
{
|
||||
static int16_t rcData4Values[8][4], rcDataMean[8];
|
||||
static uint8_t rc4ValuesIndex = 0;
|
||||
uint8_t chan, a;
|
||||
|
||||
#if defined(SBUS)
|
||||
readSBus();
|
||||
#endif
|
||||
rc4ValuesIndex++;
|
||||
for (chan = 0; chan < 8; chan++) {
|
||||
rcData4Values[chan][rc4ValuesIndex % 4] = readRawRC(chan);
|
||||
rcDataMean[chan] = 0;
|
||||
for (a = 0; a < 4; a++)
|
||||
rcDataMean[chan] += rcData4Values[chan][a];
|
||||
|
||||
rcDataMean[chan] = (rcDataMean[chan] + 2) / 4;
|
||||
if (rcDataMean[chan] < rcData[chan] - 3)
|
||||
rcData[chan] = rcDataMean[chan] + 2;
|
||||
if (rcDataMean[chan] > rcData[chan] + 3)
|
||||
rcData[chan] = rcDataMean[chan] - 2;
|
||||
}
|
||||
}
|
||||
|
||||
void loop(void)
|
||||
{
|
||||
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;
|
||||
|
||||
#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 ((cfg.activate1[BOXARM] > 0) || (cfg.activate2[BOXARM] > 0)) {
|
||||
if (rcOptions[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) {
|
||||
cfg.accTrim[PITCH] += 2;
|
||||
writeParams();
|
||||
#if defined(LED_RING)
|
||||
blinkLedRing();
|
||||
#endif
|
||||
} else if (rcData[PITCH] < MINCHECK) {
|
||||
cfg.accTrim[PITCH] -= 2;
|
||||
writeParams();
|
||||
#if defined(LED_RING)
|
||||
blinkLedRing();
|
||||
#endif
|
||||
} else if (rcData[ROLL] > MAXCHECK) {
|
||||
cfg.accTrim[ROLL] += 2;
|
||||
writeParams();
|
||||
#if defined(LED_RING)
|
||||
blinkLedRing();
|
||||
#endif
|
||||
} else if (rcData[ROLL] < MINCHECK) {
|
||||
cfg.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 && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
|
||||
InflightcalibratingA = 50;
|
||||
AccInflightCalibrationArmed = 0;
|
||||
}
|
||||
if (rcOptions[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
|
||||
|
||||
for(i = 0; i < CHECKBOXITEMS; i++) {
|
||||
rcOptions[i] = (((rcData[AUX1] < 1300) | (1300 < rcData[AUX1] && rcData[AUX1] < 1700) << 1 | (rcData[AUX1] > 1700) << 2 | (rcData[AUX2] < 1300) << 3 | (1300 < rcData[AUX2] && rcData[AUX2] < 1700) << 4 | (rcData[AUX2] > 1700) << 5) & cfg.activate1[i]) || (((rcData[AUX3] < 1300) | (1300 < rcData[AUX3] && rcData[AUX3] < 1700) << 1 | (rcData[AUX3] > 1700) << 2 | (rcData[AUX4] < 1300) << 3 | (1300 < rcData[AUX4] && rcData[AUX4] < 1700) << 4 | (rcData[AUX4] > 1700) << 5) & cfg.activate2[i]);
|
||||
}
|
||||
|
||||
//note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
|
||||
if ((rcOptions[BOXACC] || (failsafeCnt > 5 * FAILSAVE_DELAY)) && (sensors(SENSOR_ACC))) {
|
||||
// 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 ((rcOptions[BOXARM]) == 0)
|
||||
okToArm = 1;
|
||||
if (accMode == 1) {
|
||||
LED1_ON;
|
||||
} else {
|
||||
LED1_OFF;
|
||||
}
|
||||
|
||||
if (sensors(SENSOR_BARO)) {
|
||||
if (rcOptions[BOXBARO]) {
|
||||
if (baroMode == 0) {
|
||||
baroMode = 1;
|
||||
AltHold = EstAlt;
|
||||
initialThrottleHold = rcCommand[THROTTLE];
|
||||
errorAltitudeI = 0;
|
||||
BaroPID = 0;
|
||||
}
|
||||
} else
|
||||
baroMode = 0;
|
||||
}
|
||||
if (sensors(SENSOR_MAG)) {
|
||||
if (rcOptions[BOXMAG]) {
|
||||
if (magMode == 0) {
|
||||
magMode = 1;
|
||||
magHold = heading;
|
||||
}
|
||||
} else
|
||||
magMode = 0;
|
||||
if (rcOptions[BOXHEADFREE]) {
|
||||
if (headFreeMode == 0) {
|
||||
headFreeMode = 1;
|
||||
}
|
||||
} else
|
||||
headFreeMode = 0;
|
||||
}
|
||||
#if defined(GPS)
|
||||
if (rcOptions[BOXGPSHOME]) {
|
||||
GPSModeHome = 1;
|
||||
} else
|
||||
GPSModeHome = 0;
|
||||
if (rcOptions[BOXGPSHOLD]) {
|
||||
if (GPSModeHold == 0) {
|
||||
GPSModeHold = 1;
|
||||
GPS_latitude_hold = GPS_latitude;
|
||||
GPS_longitude_hold = GPS_longitude;
|
||||
}
|
||||
} else {
|
||||
GPSModeHold = 0;
|
||||
}
|
||||
#endif
|
||||
if (rcOptions[BOXPASSTHRU]) {
|
||||
passThruMode = 1;
|
||||
} else
|
||||
passThruMode = 0;
|
||||
} else { // not in rc loop
|
||||
static int8_t taskOrder = 0; //never call all function in the same loop, to avoid high delay spikes
|
||||
switch (taskOrder) {
|
||||
case 0:
|
||||
taskOrder++;
|
||||
if (sensors(SENSOR_MAG))
|
||||
Mag_getADC();
|
||||
break;
|
||||
case 1:
|
||||
taskOrder++;
|
||||
if (sensors(SENSOR_BARO))
|
||||
Baro_update();
|
||||
break;
|
||||
case 2:
|
||||
taskOrder++;
|
||||
if (sensors(SENSOR_BARO))
|
||||
getEstimatedAltitude();
|
||||
break;
|
||||
case 3:
|
||||
taskOrder++;
|
||||
#if GPS
|
||||
GPS_NewData();
|
||||
#endif
|
||||
break;
|
||||
|
||||
default:
|
||||
taskOrder = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
computeIMU();
|
||||
|
||||
// Measure loop rate just afer reading the sensors
|
||||
currentTime = micros();
|
||||
cycleTime = currentTime - previousTime;
|
||||
previousTime = currentTime;
|
||||
|
||||
if (sensors(SENSOR_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 * cfg.P8[PIDMAG] / 30; //18 deg
|
||||
} else
|
||||
magHold = heading;
|
||||
}
|
||||
|
||||
if (sensors(SENSOR_BARO)) {
|
||||
if (baroMode) {
|
||||
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > 20) {
|
||||
baroMode = 0; // so that a new althold reference is defined
|
||||
}
|
||||
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
|
||||
}
|
||||
}
|
||||
#if GPS
|
||||
uint16_t GPS_dist;
|
||||
int16_t GPS_dir;
|
||||
|
||||
if ((GPSModeHome == 0 && GPSModeHold == 0) || (GPS_fix_home == 0)) {
|
||||
GPS_angle[ROLL] = 0;
|
||||
GPS_angle[PITCH] = 0;
|
||||
} else {
|
||||
if (GPSModeHome == 1) {
|
||||
GPS_dist = GPS_distanceToHome;
|
||||
GPS_dir = GPS_directionToHome;
|
||||
}
|
||||
if (GPSModeHold == 1) {
|
||||
GPS_dist = GPS_distanceToHold;
|
||||
GPS_dir = GPS_directionToHold;
|
||||
}
|
||||
float radDiff = (GPS_dir - heading) * 0.0174533f;
|
||||
GPS_angle[ROLL] = constrain(cfg.P8[PIDGPS] * sin(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // with P=5.0, a distance of 1 meter = 0.5deg inclination
|
||||
GPS_angle[PITCH] = constrain(cfg.P8[PIDGPS] * cos(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // max inclination = D deg
|
||||
}
|
||||
#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] + cfg.accTrim[axis]; //16 bits is ok here
|
||||
#ifdef LEVEL_PDF
|
||||
PTerm = -(int32_t) angle[axis] * cfg.P8[PIDLEVEL] / 100;
|
||||
#else
|
||||
PTerm = (int32_t) errorAngle * cfg.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, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
|
||||
|
||||
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); //WindUp //16 bits is ok here
|
||||
ITerm = ((int32_t) errorAngleI[axis] * cfg.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
|
||||
error = (int32_t) rcCommand[axis] * 10 * 8 / cfg.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 * cfg.I8[axis]) >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
|
||||
}
|
||||
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;
|
||||
|
||||
DTerm = ((int32_t) deltaSum * dynD8[axis]) >> 5; //32 bits is needed for calculation
|
||||
|
||||
axisPID[axis] = PTerm + ITerm - DTerm;
|
||||
}
|
||||
|
||||
mixTable();
|
||||
writeServos();
|
||||
writeMotors();
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue