|
|
|
@ -1,6 +1,8 @@ |
|
|
|
#include "board.h" |
|
|
|
#include "mw.h" |
|
|
|
|
|
|
|
// February 2012 V1.dev |
|
|
|
|
|
|
|
#define CHECKBOXITEMS 11 |
|
|
|
#define PIDITEMS 8 |
|
|
|
|
|
|
|
@ -33,8 +35,8 @@ uint8_t yawRate; |
|
|
|
uint8_t dynThrPID; |
|
|
|
uint8_t activate1[CHECKBOXITEMS]; |
|
|
|
uint8_t activate2[CHECKBOXITEMS]; |
|
|
|
uint8_t rcOptions[CHECKBOXITEMS]; |
|
|
|
uint8_t okToArm = 0; |
|
|
|
uint8_t rcOptions1, rcOptions2; |
|
|
|
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 |
|
|
|
@ -62,6 +64,14 @@ uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position upda |
|
|
|
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 |
|
|
|
// ********************** |
|
|
|
@ -181,7 +191,7 @@ void annexCode(void) |
|
|
|
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 |
|
|
|
if (rcOptions[BOXBEEPERON]) { // unconditional beeper on via AUXn switch |
|
|
|
buzzerFreq = 7; |
|
|
|
} else if (((vbat > VBATLEVEL1_3S) |
|
|
|
#if defined(POWERMETER) |
|
|
|
@ -267,7 +277,7 @@ void annexCode(void) |
|
|
|
} |
|
|
|
#endif |
|
|
|
|
|
|
|
#if defined(GPS) |
|
|
|
#if GPS |
|
|
|
static uint32_t GPSLEDTime; |
|
|
|
if (currentTime > GPSLEDTime && (GPS_fix_home == 1)) { |
|
|
|
GPSLEDTime = currentTime + 150000; |
|
|
|
@ -325,9 +335,6 @@ void loop(void) |
|
|
|
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) |
|
|
|
@ -392,8 +399,7 @@ void loop(void) |
|
|
|
} |
|
|
|
#endif |
|
|
|
else if ((activate1[BOXARM] > 0) || (activate2[BOXARM] > 0)) { |
|
|
|
if (((rcOptions1 & activate1[BOXARM]) |
|
|
|
|| (rcOptions2 & activate2[BOXARM])) && okToArm) { |
|
|
|
if (rcOptions[BOXARM] && okToArm) { |
|
|
|
armed = 1; |
|
|
|
headFreeModeHold = heading; |
|
|
|
} else if (armed) |
|
|
|
@ -466,11 +472,11 @@ void loop(void) |
|
|
|
#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 |
|
|
|
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 ((rcOptions1 & activate1[BOXPASSTHRU]) || (rcOptions2 & activate2[BOXPASSTHRU])) { //Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored |
|
|
|
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; |
|
|
|
@ -482,11 +488,12 @@ void loop(void) |
|
|
|
} |
|
|
|
#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; |
|
|
|
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) & 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) & activate2[i]); |
|
|
|
} |
|
|
|
|
|
|
|
//note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false |
|
|
|
if (((rcOptions1 & activate1[BOXACC]) || (rcOptions2 & activate2[BOXACC]) || (failsafeCnt > 5 * FAILSAVE_DELAY)) && (sensors(SENSOR_ACC))) { |
|
|
|
if ((rcOptions[BOXACC] || (failsafeCnt > 5 * FAILSAVE_DELAY)) && (sensors(SENSOR_ACC))) { |
|
|
|
// bumpless transfer to Level mode |
|
|
|
if (!accMode) { |
|
|
|
errorAngleI[ROLL] = 0; |
|
|
|
@ -496,7 +503,7 @@ void loop(void) |
|
|
|
} else |
|
|
|
accMode = 0; // modified by MIS for failsave support |
|
|
|
|
|
|
|
if ((rcOptions1 & activate1[BOXARM]) == 0 || (rcOptions2 & activate2[BOXARM]) == 0) |
|
|
|
if ((rcOptions[BOXARM]) == 0) |
|
|
|
okToArm = 1; |
|
|
|
if (accMode == 1) { |
|
|
|
LED1_ON; |
|
|
|
@ -505,25 +512,27 @@ void loop(void) |
|
|
|
} |
|
|
|
|
|
|
|
if (sensors(SENSOR_BARO)) { |
|
|
|
if ((rcOptions1 & activate1[BOXBARO]) || (rcOptions2 & activate2[BOXBARO])) { |
|
|
|
if (rcOptions[BOXBARO]) { |
|
|
|
if (baroMode == 0) { |
|
|
|
baroMode = 1; |
|
|
|
AltHold = EstAlt; |
|
|
|
initialThrottleHold = rcCommand[THROTTLE]; |
|
|
|
errorAltitudeI = 0; |
|
|
|
EstVelocity = 0; |
|
|
|
BaroPID = 0; |
|
|
|
} |
|
|
|
} else |
|
|
|
baroMode = 0; |
|
|
|
} |
|
|
|
if (sensors(SENSOR_MAG)) { |
|
|
|
if ((rcOptions1 & activate1[BOXMAG]) || (rcOptions2 & activate2[BOXMAG])) { |
|
|
|
if (rcOptions[BOXMAG]) { |
|
|
|
if (magMode == 0) { |
|
|
|
magMode = 1; |
|
|
|
magHold = heading; |
|
|
|
} |
|
|
|
} else |
|
|
|
magMode = 0; |
|
|
|
if ((rcOptions1 & activate1[BOXHEADFREE]) || (rcOptions2 & activate2[BOXHEADFREE])) { |
|
|
|
if (rcOptions[BOXHEADFREE]) { |
|
|
|
if (headFreeMode == 0) { |
|
|
|
headFreeMode = 1; |
|
|
|
} |
|
|
|
@ -531,19 +540,39 @@ void loop(void) |
|
|
|
headFreeMode = 0; |
|
|
|
} |
|
|
|
#if defined(GPS) |
|
|
|
if ((rcOptions1 & activate1[BOXGPSHOME]) || (rcOptions2 & activate2[BOXGPSHOME])) { |
|
|
|
if (rcOptions[BOXGPSHOME]) { |
|
|
|
GPSModeHome = 1; |
|
|
|
} else |
|
|
|
GPSModeHome = 0; |
|
|
|
if ((rcOptions1 & activate1[BOXGPSHOLD]) || (rcOptions2 & activate2[BOXGPSHOLD])) { |
|
|
|
if (rcOptions[BOXGPSHOLD]) { |
|
|
|
GPSModeHold = 1; |
|
|
|
} else |
|
|
|
GPSModeHold = 0; |
|
|
|
#endif |
|
|
|
if ((rcOptions1 & activate1[BOXPASSTHRU]) || (rcOptions2 & activate2[BOXPASSTHRU])) { |
|
|
|
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 |
|
|
|
switch (taskOrder) { |
|
|
|
case 0: |
|
|
|
taskOrder++; |
|
|
|
if (sensors(SENSOR_MAG)) |
|
|
|
Mag_getADC(); |
|
|
|
break; |
|
|
|
case 1: |
|
|
|
taskOrder++; |
|
|
|
if (sensors(SENSOR_BARO)) |
|
|
|
Baro_update(); |
|
|
|
case 2: |
|
|
|
taskOrder++; |
|
|
|
if (sensors(SENSOR_BARO)) |
|
|
|
getEstimatedAltitude(); |
|
|
|
default: |
|
|
|
taskOrder = 0; |
|
|
|
break; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
computeIMU(); |
|
|
|
@ -572,29 +601,13 @@ void loop(void) |
|
|
|
AltHold = EstAlt; |
|
|
|
initialThrottleHold = rcCommand[THROTTLE]; |
|
|
|
errorAltitudeI = 0; |
|
|
|
EstVelocity = 0; |
|
|
|
BaroPID = 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); |
|
|
|
rcCommand[THROTTLE] = initialThrottleHold + BaroPID; |
|
|
|
} |
|
|
|
} |
|
|
|
#if defined(GPS) |
|
|
|
#if GPS |
|
|
|
if ((GPSModeHome == 1)) { |
|
|
|
float radDiff = (GPS_directionToHome - heading) * 0.0174533f; |
|
|
|
GPS_angle[ROLL] = constrain(P8[PIDGPS] * sinf(radDiff) * GPS_distanceToHome / 10, -D8[PIDGPS] * 10, +D8[PIDGPS] * 10); // with P=5, 1 meter = 0.5deg inclination |
|
|
|
@ -615,7 +628,7 @@ void loop(void) |
|
|
|
#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]); |
|
|
|
PTerm = constrain(PTerm, -D8[PIDLEVEL] * 5, +D8[PIDLEVEL] * 5); |
|
|
|
|
|
|
|
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 |
|
|
|
@ -647,7 +660,7 @@ void loop(void) |
|
|
|
writeServos(); |
|
|
|
writeMotors(); |
|
|
|
|
|
|
|
#if defined(GPS) |
|
|
|
#if GPS |
|
|
|
while (SerialAvailable(GPS_SERIAL)) { |
|
|
|
if (GPS_newFrame(SerialRead(GPS_SERIAL))) { |
|
|
|
if (GPS_update == 1) |
|
|
|
|
timecop
13 years ago