merged multiwii_dev-20120219
untested, but compiles and works in gui. git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@91 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
parent
5ff5b69db6
commit
e1afcdca09
8
config.c
8
config.c
|
@ -5,7 +5,7 @@
|
|||
#define FLASH_PAGE_SIZE ((uint16_t)0x400)
|
||||
#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * 63) // use the last KB for storage
|
||||
|
||||
static uint8_t checkNewConf = 150;
|
||||
static uint8_t checkNewConf = 151;
|
||||
|
||||
typedef struct eep_entry_t {
|
||||
void *var;
|
||||
|
@ -121,9 +121,9 @@ void checkFirstTime(void)
|
|||
P8[YAW] = 85;
|
||||
I8[YAW] = 0;
|
||||
D8[YAW] = 0;
|
||||
P8[PIDALT] = 47;
|
||||
I8[PIDALT] = 0;
|
||||
D8[PIDALT] = 30;
|
||||
P8[PIDALT] = 16;
|
||||
I8[PIDALT] = 15;
|
||||
D8[PIDALT] = 7;
|
||||
P8[PIDGPS] = 10;
|
||||
I8[PIDGPS] = 0;
|
||||
D8[PIDGPS] = 0;
|
||||
|
|
|
@ -130,7 +130,7 @@ bool bmp085Init(void)
|
|||
i2cRead(p_bmp085->dev_addr, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */
|
||||
p_bmp085->chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
|
||||
p_bmp085->number_of_samples = 1;
|
||||
p_bmp085->oversampling_setting = 3;
|
||||
p_bmp085->oversampling_setting = 2;
|
||||
|
||||
if (p_bmp085->chip_id == BMP085_CHIP_ID) { /* get bitslice */
|
||||
p_bmp085->sensortype = BOSCH_PRESSURE_BMP085;
|
||||
|
|
88
imu.c
88
imu.c
|
@ -6,9 +6,12 @@
|
|||
int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
|
||||
int16_t acc_25deg = 0;
|
||||
int32_t pressure = 0;
|
||||
int16_t BaroAlt = 0;
|
||||
int16_t EstAlt = 0; // in cm
|
||||
int16_t zVelocity = 0;
|
||||
int32_t BaroAlt;
|
||||
int32_t EstAlt; // in cm
|
||||
int16_t BaroPID = 0;
|
||||
int32_t AltHold;
|
||||
int16_t errorAltitudeI = 0;
|
||||
int32_t EstVelocity;
|
||||
|
||||
// **************
|
||||
// gyro+acc IMU
|
||||
|
@ -21,7 +24,6 @@ int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.
|
|||
int8_t smallAngle25 = 1;
|
||||
|
||||
static void getEstimatedAttitude(void);
|
||||
static void getEstimatedAltitude(void);
|
||||
|
||||
void imuInit(void)
|
||||
{
|
||||
|
@ -39,20 +41,11 @@ void computeIMU(void)
|
|||
int16_t gyroADCp[3];
|
||||
int16_t gyroADCinter[3];
|
||||
static uint32_t timeInterleave = 0;
|
||||
#if defined(TRI)
|
||||
static int16_t gyroYawSmooth = 0;
|
||||
#endif
|
||||
|
||||
if (sensors(SENSOR_MAG))
|
||||
Mag_getADC();
|
||||
if (sensors(SENSOR_BARO))
|
||||
Baro_update();
|
||||
|
||||
if (sensors(SENSOR_ACC)) {
|
||||
ACC_getADC();
|
||||
getEstimatedAttitude();
|
||||
if (sensors(SENSOR_BARO))
|
||||
getEstimatedAltitude();
|
||||
}
|
||||
|
||||
Gyro_getADC();
|
||||
|
@ -293,58 +286,59 @@ int32_t isq(int32_t x)
|
|||
|
||||
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
|
||||
#define INIT_DELAY 4000000 // 4 sec initialization delay
|
||||
#define BARO_TAB_SIZE 40
|
||||
#define Kp1 5.5f // PI observer velocity gain
|
||||
#define Kp2 10.0f // PI observer position gain
|
||||
#define Ki 0.01f // PI observer integral gain (bias cancellation)
|
||||
#define dt (UPDATE_INTERVAL / 1000000.0f)
|
||||
|
||||
static void getEstimatedAltitude(void)
|
||||
void getEstimatedAltitude(void)
|
||||
{
|
||||
static uint8_t inited = 0;
|
||||
static int16_t AltErrorI = 0;
|
||||
static float AccScale;
|
||||
static uint32_t deadLine = INIT_DELAY;
|
||||
int16_t AltError;
|
||||
int16_t InstAcc;
|
||||
static int32_t tmpAlt;
|
||||
static int16_t EstVelocity = 0;
|
||||
static uint32_t velTimer;
|
||||
static int16_t lastAlt;
|
||||
static int16_t BaroHistTab[BARO_TAB_SIZE];
|
||||
static int8_t BaroHistIdx = 0;
|
||||
int32_t BaroHigh, BaroLow;
|
||||
int32_t temp32;
|
||||
|
||||
if (currentTime < deadLine)
|
||||
return;
|
||||
deadLine = currentTime + UPDATE_INTERVAL;
|
||||
// Soft start
|
||||
|
||||
if (!inited) {
|
||||
inited = 1;
|
||||
tmpAlt = BaroAlt * 10;
|
||||
AccScale = 100 * 9.80665f / acc_1G;
|
||||
EstAlt = BaroAlt;
|
||||
}
|
||||
// Estimation Error
|
||||
AltError = BaroAlt - EstAlt;
|
||||
AltErrorI += AltError;
|
||||
AltErrorI = constrain(AltErrorI, -2500, +2500);
|
||||
// Gravity vector correction and projection to the local Z
|
||||
//InstAcc = (accADC[YAW] * (1 - acc_1G * InvSqrt(isq(accADC[ROLL]) + isq(accADC[PITCH]) + isq(accADC[YAW])))) * AccScale + (Ki) * AltErrorI;
|
||||
#if defined(TRUSTED_ACCZ)
|
||||
InstAcc = (accADC[YAW] * (1 - acc_1G * InvSqrt(isq(accADC[ROLL]) + isq(accADC[PITCH]) + isq(accADC[YAW])))) * AccScale + AltErrorI / 100;
|
||||
#else
|
||||
InstAcc = AltErrorI / 100;
|
||||
#endif
|
||||
|
||||
// Integrators
|
||||
tmpAlt += EstVelocity * (dt * dt) + (Kp2 * dt) * AltError;
|
||||
EstVelocity += InstAcc + Kp1 * AltError;
|
||||
EstVelocity = constrain(EstVelocity, -10000, +10000);
|
||||
//**** Alt. Set Point stabilization PID ****
|
||||
//calculate speed for D calculation
|
||||
BaroHistTab[BaroHistIdx] = BaroAlt;
|
||||
BaroHigh = 0;
|
||||
BaroLow = 0;
|
||||
BaroPID = 0;
|
||||
for (temp32 = 0; temp32 < BARO_TAB_SIZE / 2; temp32++) {
|
||||
BaroHigh += BaroHistTab[(BaroHistIdx - temp32 + BARO_TAB_SIZE) % BARO_TAB_SIZE]; // sum last half samples
|
||||
BaroLow += BaroHistTab[(BaroHistIdx + temp32 + BARO_TAB_SIZE) % BARO_TAB_SIZE]; // sum older samples
|
||||
}
|
||||
BaroHistIdx++;
|
||||
if (BaroHistIdx >= BARO_TAB_SIZE)
|
||||
BaroHistIdx = 0;
|
||||
|
||||
EstAlt = tmpAlt / 10;
|
||||
temp32 = D8[PIDALT] * (BaroHigh - BaroLow) / 400;
|
||||
BaroPID -= temp32;
|
||||
|
||||
if (currentTime < velTimer)
|
||||
return;
|
||||
velTimer = currentTime + 500000;
|
||||
zVelocity = tmpAlt - lastAlt;
|
||||
lastAlt = tmpAlt;
|
||||
EstAlt = BaroHigh / (BARO_TAB_SIZE / 2);
|
||||
|
||||
debug4 = zVelocity;
|
||||
temp32 = constrain(AltHold - EstAlt, -1000, 1000);
|
||||
if (abs(temp32) < 10 && BaroPID < 10)
|
||||
BaroPID = 0; // remove small D parametr to reduce noise near zoro position
|
||||
// P
|
||||
BaroPID += P8[PIDALT] * constrain(temp32, (-2) * P8[PIDALT], 2 * P8[PIDALT]) / 100;
|
||||
BaroPID = constrain(BaroPID, -150, +150); // sum of P and D should be in range 150
|
||||
|
||||
// I
|
||||
errorAltitudeI += temp32 * I8[PIDALT] / 50;
|
||||
errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
|
||||
temp32 = errorAltitudeI / 500; // I in range +/-60
|
||||
BaroPID += temp32;
|
||||
}
|
||||
|
|
45
mixer.c
45
mixer.c
|
@ -19,8 +19,8 @@
|
|||
#define PRI_SERVO_TO 6
|
||||
#elif defined(TRI)
|
||||
#define NUMBER_MOTOR 3
|
||||
#define PRI_SERVO_FROM 5 // use only servo 5
|
||||
#define PRI_SERVO_TO 5
|
||||
#define PRI_SERVO_FROM 6 // use only servo 5
|
||||
#define PRI_SERVO_TO 6
|
||||
#elif defined(QUADP) || defined(QUADX) || defined(Y4)
|
||||
#define NUMBER_MOTOR 4
|
||||
#elif defined(Y6) || defined(HEX6) || defined(HEX6X)
|
||||
|
@ -78,7 +78,8 @@ void mixTable(void)
|
|||
motor[0] = PIDMIX(0, +4 / 3, 0); //REAR
|
||||
motor[1] = PIDMIX(-1, -2 / 3, 0); //RIGHT
|
||||
motor[2] = PIDMIX(+1, -2 / 3, 0); //LEFT
|
||||
servo[4] = constrain(tri_yaw_middle + YAW_DIRECTION * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
|
||||
servo[5] = constrain(tri_yaw_middle + YAW_DIRECTION * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
|
||||
|
||||
#endif
|
||||
#ifdef QUADP
|
||||
motor[0] = PIDMIX(0, +1, -1); //REAR
|
||||
|
@ -152,15 +153,25 @@ void mixTable(void)
|
|||
motor[6] = PIDMIX(-1 / 2, +1, -1); //REAR_R
|
||||
motor[7] = PIDMIX(+1, +1 / 2, -1); //MIDREAR_L
|
||||
#endif
|
||||
#ifdef VTAIL4
|
||||
motor[0] = PIDMIX(+0, +1, -1 / 2); //REAR_R
|
||||
motor[1] = PIDMIX(-1, -1, +2 / 10); //FRONT_R
|
||||
motor[2] = PIDMIX(+0, +1, +1 / 2); //REAR_L
|
||||
motor[3] = PIDMIX(+1, -1, -2 / 10); //FRONT_L
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef SERVO_TILT
|
||||
if ((rcOptions1 & activate1[BOXCAMSTAB]) || (rcOptions2 & activate2[BOXCAMSTAB])) {
|
||||
servo[0] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] / 16 + rcData[AUX3] - 1500, TILT_PITCH_MIN, TILT_PITCH_MAX);
|
||||
servo[1] = constrain(TILT_ROLL_MIDDLE + TILT_ROLL_PROP * angle[ROLL] / 16 + rcData[AUX4] - 1500, TILT_ROLL_MIN, TILT_ROLL_MAX);
|
||||
} else {
|
||||
servo[0] = constrain(TILT_PITCH_MIDDLE + rcData[AUX3] - 1500, TILT_PITCH_MIN, TILT_PITCH_MAX);
|
||||
servo[1] = constrain(TILT_ROLL_MIDDLE + rcData[AUX4] - 1500, TILT_ROLL_MIN, TILT_ROLL_MAX);
|
||||
servo[0] = TILT_PITCH_MIDDLE + rcData[AUX3] - 1500;
|
||||
servo[1] = TILT_ROLL_MIDDLE + rcData[AUX4] - 1500;
|
||||
|
||||
if (rcOptions[BOXCAMSTAB]) {
|
||||
servo[0] += TILT_PITCH_PROP * angle[PITCH] / 16;
|
||||
servo[1] += TILT_ROLL_PROP * angle[ROLL] / 16;
|
||||
}
|
||||
|
||||
servo[0] = constrain(servo[0], TILT_PITCH_MIN, TILT_PITCH_MAX);
|
||||
servo[1] = constrain(servo[1], TILT_ROLL_MIN, TILT_ROLL_MAX);
|
||||
#endif
|
||||
#ifdef GIMBAL
|
||||
servo[0] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] / 16 + rcCommand[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX);
|
||||
|
@ -168,13 +179,15 @@ void mixTable(void)
|
|||
#endif
|
||||
#ifdef FLYING_WING
|
||||
motor[0] = rcCommand[THROTTLE];
|
||||
if (passThruMode) { // use raw stick values to drive output
|
||||
servo[0] = constrain(wing_left_mid + PITCH_DIRECTION_L * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_L * (rcData[ROLL] - MIDRC), WING_LEFT_MIN, WING_LEFT_MAX); //LEFT
|
||||
servo[1] = constrain(wing_right_mid + PITCH_DIRECTION_R * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_R * (rcData[ROLL] - MIDRC), WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT
|
||||
if (passThruMode) {// do not use sensors for correction, simple 2 channel mixing
|
||||
servo[0] = PITCH_DIRECTION_L * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_L * (rcData[ROLL] - MIDRC);
|
||||
servo[1] = PITCH_DIRECTION_R * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_R * (rcData[ROLL] - MIDRC);
|
||||
} else { // use sensors to correct (gyro only or gyro+acc according to aux1/aux2 configuration
|
||||
servo[0] = constrain(wing_left_mid + PITCH_DIRECTION_L * axisPID[PITCH] + ROLL_DIRECTION_L * axisPID[ROLL], WING_LEFT_MIN, WING_LEFT_MAX); //LEFT
|
||||
servo[1] = constrain(wing_right_mid + PITCH_DIRECTION_R * axisPID[PITCH] + ROLL_DIRECTION_R * axisPID[ROLL], WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT
|
||||
servo[0] = PITCH_DIRECTION_L * axisPID[PITCH] + ROLL_DIRECTION_L * axisPID[ROLL];
|
||||
servo[1] = PITCH_DIRECTION_R * axisPID[PITCH] + ROLL_DIRECTION_R * axisPID[ROLL];
|
||||
}
|
||||
servo[0] = constrain(servo[0] + wing_left_mid , WING_LEFT_MIN, WING_LEFT_MAX);
|
||||
servo[1] = constrain(servo[1] + wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX);
|
||||
#endif
|
||||
#if defined(CAMTRIG)
|
||||
if (camCycle == 1) {
|
||||
|
@ -195,7 +208,7 @@ void mixTable(void)
|
|||
}
|
||||
}
|
||||
}
|
||||
if ((rcOptions1 & activate1[BOXCAMTRIG]) || (rcOptions1 & activate2[BOXCAMTRIG]))
|
||||
if (rcOptions[BOXCAMTRIG])
|
||||
camCycle = 1;
|
||||
#endif
|
||||
|
||||
|
@ -225,7 +238,7 @@ void mixTable(void)
|
|||
0, 2, 6, 15, 30, 52, 82, 123, 175, 240, 320, 415, 528, 659, 811, 984, 1181, 1402, 1648, 1923, 2226, 2559, 2924, 3322, 3755, 4224, 4730, 5276, 5861, 6489, 7160, 7875, 8637, 9446, 10304, 11213, 12173, 13187, 14256, 15381, 16564, 17805, 19108, 20472, 21900, 23392, 24951, 26578, 28274, 30041, 31879, 33792, 35779, 37843, 39984, 42205, 44507, 46890, 49358, 51910, 54549, 57276, 60093, 63000};
|
||||
|
||||
if (vbat) { // by all means - must avoid division by zero
|
||||
for (uint8_t i = 0; i < NUMBER_MOTOR; i++) {
|
||||
for (i = 0; i < NUMBER_MOTOR; i++) {
|
||||
amp = amperes[((motor[i] - 1000) >> 4)] / vbat; // range mapped from [1000:2000] => [0:1000]; then break that up into 64 ranges; lookup amp
|
||||
#if (LOG_VALUES == 2)
|
||||
pMeter[i] += amp; // sum up over time the mapped ESC input
|
||||
|
|
97
mw.c
97
mw.c
|
@ -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)
|
||||
|
|
11
mw.h
11
mw.h
|
@ -99,6 +99,8 @@
|
|||
#define MULTITYPE 11 //the GUI is the same for all 8 motor configs
|
||||
#elif defined(OCTOFLATX)
|
||||
#define MULTITYPE 11 //the GUI is the same for all 8 motor configs
|
||||
#elif defined(VTAIL4)
|
||||
#define MULTITYPE 15
|
||||
#endif
|
||||
|
||||
/*********** RC alias *****************/
|
||||
|
@ -147,7 +149,6 @@ extern int16_t gyroZero[3];
|
|||
extern int16_t magZero[3];
|
||||
extern int16_t gyroData[3];
|
||||
extern int16_t angle[2];
|
||||
extern int16_t EstAlt;
|
||||
extern int16_t axisPID[3];
|
||||
extern int16_t rcCommand[4];
|
||||
|
||||
|
@ -165,7 +166,12 @@ extern uint16_t calibratingG;
|
|||
extern int16_t heading;
|
||||
extern int16_t annex650_overrun_count;
|
||||
extern int32_t pressure;
|
||||
extern int16_t BaroAlt;
|
||||
extern int32_t BaroAlt;
|
||||
extern int32_t EstAlt;
|
||||
extern int32_t AltHold;
|
||||
extern int16_t errorAltitudeI;
|
||||
extern int32_t EstVelocity;
|
||||
extern int16_t BaroPID;
|
||||
extern uint8_t headFreeMode;
|
||||
extern int16_t headFreeModeHold;
|
||||
extern int8_t smallAngle25;
|
||||
|
@ -208,6 +214,7 @@ void imuInit(void);
|
|||
void annexCode(void);
|
||||
void computeIMU(void);
|
||||
void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat);
|
||||
void getEstimatedAltitude(void);
|
||||
|
||||
// Sensors
|
||||
void sensorsAutodetect(void);
|
||||
|
|
13
sensors.c
13
sensors.c
|
@ -71,6 +71,7 @@ static void ACC_Common(void)
|
|||
}
|
||||
//all values are measured
|
||||
if (InflightcalibratingA == 1) {
|
||||
AccInflightCalibrationActive = 0;
|
||||
AccInflightCalibrationMeasurementDone = 1;
|
||||
blinkLED(10, 10, 2); //buzzer for indicatiing the start inflight
|
||||
// recover saved values to maintain current flight behavior until new values are transferred
|
||||
|
@ -135,16 +136,16 @@ void Baro_update(void)
|
|||
case 2:
|
||||
bmp085_start_up();
|
||||
baroState++;
|
||||
baroDeadline += 26000;
|
||||
baroDeadline += 14000;
|
||||
break;
|
||||
case 3:
|
||||
baroUP = bmp085_get_up();
|
||||
baroTemp = bmp085_get_temperature(baroUT);
|
||||
pressure = bmp085_get_pressure(baroUP);
|
||||
|
||||
BaroAlt = (1.0f - pow(pressure / 101325.0f, 0.190295f)) * 443300.0f; //decimeter
|
||||
BaroAlt = (1.0f - pow(pressure / 101325.0f, 0.190295f)) * 4433000.0f; // centimeter
|
||||
baroState = 0;
|
||||
baroDeadline += 20000;
|
||||
baroDeadline += 5000;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
@ -219,9 +220,9 @@ void Mag_init(void)
|
|||
Mag_getRawADC();
|
||||
delay(10);
|
||||
|
||||
magCal[ROLL] = 1000.0 / magADC[ROLL];
|
||||
magCal[PITCH] = 1000.0 / magADC[PITCH];
|
||||
magCal[YAW] = -1000.0 / magADC[YAW];
|
||||
magCal[ROLL] = 1000.0 / abs(magADC[ROLL]);
|
||||
magCal[PITCH] = 1000.0 / abs(magADC[PITCH]);
|
||||
magCal[YAW] = 1000.0 / abs(magADC[YAW]);
|
||||
|
||||
hmc5883lFinishCal();
|
||||
magInit = 1;
|
||||
|
|
9
serial.c
9
serial.c
|
@ -152,6 +152,7 @@ void serialCom(void)
|
|||
#else
|
||||
serialize16(cycleTime);
|
||||
#endif
|
||||
serialize16(i2cGetErrorCounter());
|
||||
for (i = 0; i < 2; i++)
|
||||
serialize16(angle[i]);
|
||||
serialize8(MULTITYPE);
|
||||
|
@ -177,10 +178,10 @@ void serialCom(void)
|
|||
serialize16(intPowerMeterSum);
|
||||
serialize16(intPowerTrigger1);
|
||||
serialize8(vbat);
|
||||
serialize16(BaroAlt); // 4 variables are here for general monitoring purpose
|
||||
serialize16(i2cGetErrorCounter()); // debug2
|
||||
serialize16(debug3); // debug3
|
||||
serialize16(debug4); // debug4
|
||||
serialize16(BaroAlt / 10); // 4 variables are here for general monitoring purpose
|
||||
serialize16(0); // debug2
|
||||
serialize16(debug3); // debug3
|
||||
serialize16(debug4); // debug4
|
||||
serialize8('M');
|
||||
// UartSendData();
|
||||
break;
|
||||
|
|
Loading…
Reference in New Issue