rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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:
timecop 2012-02-21 09:15:51 +00:00
parent 5ff5b69db6
commit e1afcdca09
8 changed files with 152 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
config.c
View File

@ -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;

2
drv_bmp085.c
View File

@ -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;

90
imu.c
View File

@ -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;
EstAlt = BaroHigh / (BARO_TAB_SIZE / 2);
if (currentTime < velTimer)
return;
velTimer = currentTime + 500000;
zVelocity = tmpAlt - lastAlt;
lastAlt = tmpAlt;
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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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;