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_PAGE_SIZE ((uint16_t)0x400)
#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * 63) // use the last KB for storage #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 { typedef struct eep_entry_t {
void *var; void *var;
@ -121,9 +121,9 @@ void checkFirstTime(void)
P8[YAW] = 85; P8[YAW] = 85;
I8[YAW] = 0; I8[YAW] = 0;
D8[YAW] = 0; D8[YAW] = 0;
P8[PIDALT] = 47; P8[PIDALT] = 16;
I8[PIDALT] = 0; I8[PIDALT] = 15;
D8[PIDALT] = 30; D8[PIDALT] = 7;
P8[PIDGPS] = 10; P8[PIDGPS] = 10;
I8[PIDGPS] = 0; I8[PIDGPS] = 0;
D8[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 */ 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->chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
p_bmp085->number_of_samples = 1; 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 */ if (p_bmp085->chip_id == BMP085_CHIP_ID) { /* get bitslice */
p_bmp085->sensortype = BOSCH_PRESSURE_BMP085; p_bmp085->sensortype = BOSCH_PRESSURE_BMP085;

88
imu.c
View File

@ -6,9 +6,12 @@
int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3]; int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
int16_t acc_25deg = 0; int16_t acc_25deg = 0;
int32_t pressure = 0; int32_t pressure = 0;
int16_t BaroAlt = 0; int32_t BaroAlt;
int16_t EstAlt = 0; // in cm int32_t EstAlt; // in cm
int16_t zVelocity = 0; int16_t BaroPID = 0;
int32_t AltHold;
int16_t errorAltitudeI = 0;
int32_t EstVelocity;
// ************** // **************
// gyro+acc IMU // gyro+acc IMU
@ -21,7 +24,6 @@ int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.
int8_t smallAngle25 = 1; int8_t smallAngle25 = 1;
static void getEstimatedAttitude(void); static void getEstimatedAttitude(void);
static void getEstimatedAltitude(void);
void imuInit(void) void imuInit(void)
{ {
@ -39,20 +41,11 @@ void computeIMU(void)
int16_t gyroADCp[3]; int16_t gyroADCp[3];
int16_t gyroADCinter[3]; int16_t gyroADCinter[3];
static uint32_t timeInterleave = 0; static uint32_t timeInterleave = 0;
#if defined(TRI)
static int16_t gyroYawSmooth = 0; static int16_t gyroYawSmooth = 0;
#endif
if (sensors(SENSOR_MAG))
Mag_getADC();
if (sensors(SENSOR_BARO))
Baro_update();
if (sensors(SENSOR_ACC)) { if (sensors(SENSOR_ACC)) {
ACC_getADC(); ACC_getADC();
getEstimatedAttitude(); getEstimatedAttitude();
if (sensors(SENSOR_BARO))
getEstimatedAltitude();
} }
Gyro_getADC(); Gyro_getADC();
@ -293,58 +286,59 @@ int32_t isq(int32_t x)
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc) #define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY 4000000 // 4 sec initialization delay #define INIT_DELAY 4000000 // 4 sec initialization delay
#define BARO_TAB_SIZE 40
#define Kp1 5.5f // PI observer velocity gain #define Kp1 5.5f // PI observer velocity gain
#define Kp2 10.0f // PI observer position gain #define Kp2 10.0f // PI observer position gain
#define Ki 0.01f // PI observer integral gain (bias cancellation) #define Ki 0.01f // PI observer integral gain (bias cancellation)
#define dt (UPDATE_INTERVAL / 1000000.0f) #define dt (UPDATE_INTERVAL / 1000000.0f)
static void getEstimatedAltitude(void) void getEstimatedAltitude(void)
{ {
static uint8_t inited = 0; static uint8_t inited = 0;
static int16_t AltErrorI = 0;
static float AccScale;
static uint32_t deadLine = INIT_DELAY; static uint32_t deadLine = INIT_DELAY;
int16_t AltError; static int16_t BaroHistTab[BARO_TAB_SIZE];
int16_t InstAcc; static int8_t BaroHistIdx = 0;
static int32_t tmpAlt; int32_t BaroHigh, BaroLow;
static int16_t EstVelocity = 0; int32_t temp32;
static uint32_t velTimer;
static int16_t lastAlt;
if (currentTime < deadLine) if (currentTime < deadLine)
return; return;
deadLine = currentTime + UPDATE_INTERVAL; deadLine = currentTime + UPDATE_INTERVAL;
// Soft start
if (!inited) { if (!inited) {
inited = 1; inited = 1;
tmpAlt = BaroAlt * 10; EstAlt = BaroAlt;
AccScale = 100 * 9.80665f / acc_1G;
} }
// 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 //**** Alt. Set Point stabilization PID ****
tmpAlt += EstVelocity * (dt * dt) + (Kp2 * dt) * AltError; //calculate speed for D calculation
EstVelocity += InstAcc + Kp1 * AltError; BaroHistTab[BaroHistIdx] = BaroAlt;
EstVelocity = constrain(EstVelocity, -10000, +10000); 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) EstAlt = BaroHigh / (BARO_TAB_SIZE / 2);
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 #define PRI_SERVO_TO 6
#elif defined(TRI) #elif defined(TRI)
#define NUMBER_MOTOR 3 #define NUMBER_MOTOR 3
#define PRI_SERVO_FROM 5 // use only servo 5 #define PRI_SERVO_FROM 6 // use only servo 5
#define PRI_SERVO_TO 5 #define PRI_SERVO_TO 6
#elif defined(QUADP) || defined(QUADX) || defined(Y4) #elif defined(QUADP) || defined(QUADX) || defined(Y4)
#define NUMBER_MOTOR 4 #define NUMBER_MOTOR 4
#elif defined(Y6) || defined(HEX6) || defined(HEX6X) #elif defined(Y6) || defined(HEX6) || defined(HEX6X)
@ -78,7 +78,8 @@ void mixTable(void)
motor[0] = PIDMIX(0, +4 / 3, 0); //REAR motor[0] = PIDMIX(0, +4 / 3, 0); //REAR
motor[1] = PIDMIX(-1, -2 / 3, 0); //RIGHT motor[1] = PIDMIX(-1, -2 / 3, 0); //RIGHT
motor[2] = PIDMIX(+1, -2 / 3, 0); //LEFT 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 #endif
#ifdef QUADP #ifdef QUADP
motor[0] = PIDMIX(0, +1, -1); //REAR motor[0] = PIDMIX(0, +1, -1); //REAR
@ -152,15 +153,25 @@ void mixTable(void)
motor[6] = PIDMIX(-1 / 2, +1, -1); //REAR_R motor[6] = PIDMIX(-1 / 2, +1, -1); //REAR_R
motor[7] = PIDMIX(+1, +1 / 2, -1); //MIDREAR_L motor[7] = PIDMIX(+1, +1 / 2, -1); //MIDREAR_L
#endif #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 #ifdef SERVO_TILT
if ((rcOptions1 & activate1[BOXCAMSTAB]) || (rcOptions2 & activate2[BOXCAMSTAB])) { servo[0] = TILT_PITCH_MIDDLE + rcData[AUX3] - 1500;
servo[0] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] / 16 + rcData[AUX3] - 1500, TILT_PITCH_MIN, TILT_PITCH_MAX); servo[1] = TILT_ROLL_MIDDLE + rcData[AUX4] - 1500;
servo[1] = constrain(TILT_ROLL_MIDDLE + TILT_ROLL_PROP * angle[ROLL] / 16 + rcData[AUX4] - 1500, TILT_ROLL_MIN, TILT_ROLL_MAX);
} else { if (rcOptions[BOXCAMSTAB]) {
servo[0] = constrain(TILT_PITCH_MIDDLE + rcData[AUX3] - 1500, TILT_PITCH_MIN, TILT_PITCH_MAX); servo[0] += TILT_PITCH_PROP * angle[PITCH] / 16;
servo[1] = constrain(TILT_ROLL_MIDDLE + rcData[AUX4] - 1500, TILT_ROLL_MIN, TILT_ROLL_MAX); 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 #endif
#ifdef GIMBAL #ifdef GIMBAL
servo[0] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] / 16 + rcCommand[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX); 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 #endif
#ifdef FLYING_WING #ifdef FLYING_WING
motor[0] = rcCommand[THROTTLE]; motor[0] = rcCommand[THROTTLE];
if (passThruMode) { // use raw stick values to drive output if (passThruMode) {// do not use sensors for correction, simple 2 channel mixing
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[0] = PITCH_DIRECTION_L * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_L * (rcData[ROLL] - MIDRC);
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 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 } 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[0] = PITCH_DIRECTION_L * axisPID[PITCH] + ROLL_DIRECTION_L * axisPID[ROLL];
servo[1] = constrain(wing_right_mid + PITCH_DIRECTION_R * axisPID[PITCH] + ROLL_DIRECTION_R * axisPID[ROLL], WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT 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 #endif
#if defined(CAMTRIG) #if defined(CAMTRIG)
if (camCycle == 1) { if (camCycle == 1) {
@ -195,7 +208,7 @@ void mixTable(void)
} }
} }
} }
if ((rcOptions1 & activate1[BOXCAMTRIG]) || (rcOptions1 & activate2[BOXCAMTRIG])) if (rcOptions[BOXCAMTRIG])
camCycle = 1; camCycle = 1;
#endif #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}; 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 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 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) #if (LOG_VALUES == 2)
pMeter[i] += amp; // sum up over time the mapped ESC input pMeter[i] += amp; // sum up over time the mapped ESC input

97
mw.c
View File

@ -1,6 +1,8 @@
#include "board.h" #include "board.h"
#include "mw.h" #include "mw.h"
// February 2012 V1.dev
#define CHECKBOXITEMS 11 #define CHECKBOXITEMS 11
#define PIDITEMS 8 #define PIDITEMS 8
@ -33,8 +35,8 @@ uint8_t yawRate;
uint8_t dynThrPID; uint8_t dynThrPID;
uint8_t activate1[CHECKBOXITEMS]; uint8_t activate1[CHECKBOXITEMS];
uint8_t activate2[CHECKBOXITEMS]; uint8_t activate2[CHECKBOXITEMS];
uint8_t rcOptions[CHECKBOXITEMS];
uint8_t okToArm = 0; uint8_t okToArm = 0;
uint8_t rcOptions1, rcOptions2;
uint8_t accMode = 0; // if level mode is a activated uint8_t accMode = 0; // if level mode is a activated
uint8_t magMode = 0; // if compass heading hold is a activated uint8_t magMode = 0; // if compass heading hold is a activated
uint8_t baroMode = 0; // if altitude hold is 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 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 // power meter
// ********************** // **********************
@ -181,7 +191,7 @@ void annexCode(void)
vbatRaw += vbatRawArray[i]; vbatRaw += vbatRawArray[i];
vbat = vbatRaw / (VBATSCALE / 2); // result is Vbatt in 0.1V steps 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; buzzerFreq = 7;
} else if (((vbat > VBATLEVEL1_3S) } else if (((vbat > VBATLEVEL1_3S)
#if defined(POWERMETER) #if defined(POWERMETER)
@ -267,7 +277,7 @@ void annexCode(void)
} }
#endif #endif
#if defined(GPS) #if GPS
static uint32_t GPSLEDTime; static uint32_t GPSLEDTime;
if (currentTime > GPSLEDTime && (GPS_fix_home == 1)) { if (currentTime > GPSLEDTime && (GPS_fix_home == 1)) {
GPSLEDTime = currentTime + 150000; GPSLEDTime = currentTime + 150000;
@ -325,9 +335,6 @@ void loop(void)
static int16_t errorAngleI[2] = { 0, 0 }; static int16_t errorAngleI[2] = { 0, 0 };
static uint32_t rcTime = 0; static uint32_t rcTime = 0;
static int16_t initialThrottleHold; static int16_t initialThrottleHold;
static int16_t errorAltitudeI = 0;
int16_t AltPID = 0;
static int16_t AltHold;
#if defined(SPEKTRUM) #if defined(SPEKTRUM)
if (rcFrameComplete) if (rcFrameComplete)
@ -392,8 +399,7 @@ void loop(void)
} }
#endif #endif
else if ((activate1[BOXARM] > 0) || (activate2[BOXARM] > 0)) { else if ((activate1[BOXARM] > 0) || (activate2[BOXARM] > 0)) {
if (((rcOptions1 & activate1[BOXARM]) if (rcOptions[BOXARM] && okToArm) {
|| (rcOptions2 & activate2[BOXARM])) && okToArm) {
armed = 1; armed = 1;
headFreeModeHold = heading; headFreeModeHold = heading;
} else if (armed) } else if (armed)
@ -466,11 +472,11 @@ void loop(void)
#endif #endif
#if defined(InflightAccCalibration) #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; InflightcalibratingA = 50;
AccInflightCalibrationArmed = 0; 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) { if (!AccInflightCalibrationArmed) {
AccInflightCalibrationArmed = 1; AccInflightCalibrationArmed = 1;
InflightcalibratingA = 50; InflightcalibratingA = 50;
@ -482,11 +488,12 @@ void loop(void)
} }
#endif #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; for(i = 0; i < CHECKBOXITEMS; i++) {
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; 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 //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 // bumpless transfer to Level mode
if (!accMode) { if (!accMode) {
errorAngleI[ROLL] = 0; errorAngleI[ROLL] = 0;
@ -496,7 +503,7 @@ void loop(void)
} else } else
accMode = 0; // modified by MIS for failsave support accMode = 0; // modified by MIS for failsave support
if ((rcOptions1 & activate1[BOXARM]) == 0 || (rcOptions2 & activate2[BOXARM]) == 0) if ((rcOptions[BOXARM]) == 0)
okToArm = 1; okToArm = 1;
if (accMode == 1) { if (accMode == 1) {
LED1_ON; LED1_ON;
@ -505,25 +512,27 @@ void loop(void)
} }
if (sensors(SENSOR_BARO)) { if (sensors(SENSOR_BARO)) {
if ((rcOptions1 & activate1[BOXBARO]) || (rcOptions2 & activate2[BOXBARO])) { if (rcOptions[BOXBARO]) {
if (baroMode == 0) { if (baroMode == 0) {
baroMode = 1; baroMode = 1;
AltHold = EstAlt; AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE]; initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0; errorAltitudeI = 0;
EstVelocity = 0;
BaroPID = 0;
} }
} else } else
baroMode = 0; baroMode = 0;
} }
if (sensors(SENSOR_MAG)) { if (sensors(SENSOR_MAG)) {
if ((rcOptions1 & activate1[BOXMAG]) || (rcOptions2 & activate2[BOXMAG])) { if (rcOptions[BOXMAG]) {
if (magMode == 0) { if (magMode == 0) {
magMode = 1; magMode = 1;
magHold = heading; magHold = heading;
} }
} else } else
magMode = 0; magMode = 0;
if ((rcOptions1 & activate1[BOXHEADFREE]) || (rcOptions2 & activate2[BOXHEADFREE])) { if (rcOptions[BOXHEADFREE]) {
if (headFreeMode == 0) { if (headFreeMode == 0) {
headFreeMode = 1; headFreeMode = 1;
} }
@ -531,19 +540,39 @@ void loop(void)
headFreeMode = 0; headFreeMode = 0;
} }
#if defined(GPS) #if defined(GPS)
if ((rcOptions1 & activate1[BOXGPSHOME]) || (rcOptions2 & activate2[BOXGPSHOME])) { if (rcOptions[BOXGPSHOME]) {
GPSModeHome = 1; GPSModeHome = 1;
} else } else
GPSModeHome = 0; GPSModeHome = 0;
if ((rcOptions1 & activate1[BOXGPSHOLD]) || (rcOptions2 & activate2[BOXGPSHOLD])) { if (rcOptions[BOXGPSHOLD]) {
GPSModeHold = 1; GPSModeHold = 1;
} else } else
GPSModeHold = 0; GPSModeHold = 0;
#endif #endif
if ((rcOptions1 & activate1[BOXPASSTHRU]) || (rcOptions2 & activate2[BOXPASSTHRU])) { if (rcOptions[BOXPASSTHRU]) {
passThruMode = 1; passThruMode = 1;
} else } else
passThruMode = 0; 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(); computeIMU();
@ -572,29 +601,13 @@ void loop(void)
AltHold = EstAlt; AltHold = EstAlt;
initialThrottleHold = rcCommand[THROTTLE]; initialThrottleHold = rcCommand[THROTTLE];
errorAltitudeI = 0; errorAltitudeI = 0;
EstVelocity = 0;
BaroPID = 0;
} }
//**** Alt. Set Point stabilization PID **** rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
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 GPS
if ((GPSModeHome == 1)) { if ((GPSModeHome == 1)) {
float radDiff = (GPS_directionToHome - heading) * 0.0174533f; 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 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 #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 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 #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 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 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(); writeServos();
writeMotors(); writeMotors();
#if defined(GPS) #if GPS
while (SerialAvailable(GPS_SERIAL)) { while (SerialAvailable(GPS_SERIAL)) {
if (GPS_newFrame(SerialRead(GPS_SERIAL))) { if (GPS_newFrame(SerialRead(GPS_SERIAL))) {
if (GPS_update == 1) 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 #define MULTITYPE 11 //the GUI is the same for all 8 motor configs
#elif defined(OCTOFLATX) #elif defined(OCTOFLATX)
#define MULTITYPE 11 //the GUI is the same for all 8 motor configs #define MULTITYPE 11 //the GUI is the same for all 8 motor configs
#elif defined(VTAIL4)
#define MULTITYPE 15
#endif #endif
/*********** RC alias *****************/ /*********** RC alias *****************/
@ -147,7 +149,6 @@ extern int16_t gyroZero[3];
extern int16_t magZero[3]; extern int16_t magZero[3];
extern int16_t gyroData[3]; extern int16_t gyroData[3];
extern int16_t angle[2]; extern int16_t angle[2];
extern int16_t EstAlt;
extern int16_t axisPID[3]; extern int16_t axisPID[3];
extern int16_t rcCommand[4]; extern int16_t rcCommand[4];
@ -165,7 +166,12 @@ extern uint16_t calibratingG;
extern int16_t heading; extern int16_t heading;
extern int16_t annex650_overrun_count; extern int16_t annex650_overrun_count;
extern int32_t pressure; 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 uint8_t headFreeMode;
extern int16_t headFreeModeHold; extern int16_t headFreeModeHold;
extern int8_t smallAngle25; extern int8_t smallAngle25;
@ -208,6 +214,7 @@ void imuInit(void);
void annexCode(void); void annexCode(void);
void computeIMU(void); void computeIMU(void);
void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat); void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat);
void getEstimatedAltitude(void);
// Sensors // Sensors
void sensorsAutodetect(void); void sensorsAutodetect(void);

13
sensors.c
View File

@ -71,6 +71,7 @@ static void ACC_Common(void)
} }
//all values are measured //all values are measured
if (InflightcalibratingA == 1) { if (InflightcalibratingA == 1) {
AccInflightCalibrationActive = 0;
AccInflightCalibrationMeasurementDone = 1; AccInflightCalibrationMeasurementDone = 1;
blinkLED(10, 10, 2); //buzzer for indicatiing the start inflight blinkLED(10, 10, 2); //buzzer for indicatiing the start inflight
// recover saved values to maintain current flight behavior until new values are transferred // recover saved values to maintain current flight behavior until new values are transferred
@ -135,16 +136,16 @@ void Baro_update(void)
case 2: case 2:
bmp085_start_up(); bmp085_start_up();
baroState++; baroState++;
baroDeadline += 26000; baroDeadline += 14000;
break; break;
case 3: case 3:
baroUP = bmp085_get_up(); baroUP = bmp085_get_up();
baroTemp = bmp085_get_temperature(baroUT); baroTemp = bmp085_get_temperature(baroUT);
pressure = bmp085_get_pressure(baroUP); 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; baroState = 0;
baroDeadline += 20000; baroDeadline += 5000;
break; break;
} }
} }
@ -219,9 +220,9 @@ void Mag_init(void)
Mag_getRawADC(); Mag_getRawADC();
delay(10); delay(10);
magCal[ROLL] = 1000.0 / magADC[ROLL]; magCal[ROLL] = 1000.0 / abs(magADC[ROLL]);
magCal[PITCH] = 1000.0 / magADC[PITCH]; magCal[PITCH] = 1000.0 / abs(magADC[PITCH]);
magCal[YAW] = -1000.0 / magADC[YAW]; magCal[YAW] = 1000.0 / abs(magADC[YAW]);
hmc5883lFinishCal(); hmc5883lFinishCal();
magInit = 1; magInit = 1;

5
serial.c
View File

@ -152,6 +152,7 @@ void serialCom(void)
#else #else
serialize16(cycleTime); serialize16(cycleTime);
#endif #endif
serialize16(i2cGetErrorCounter());
for (i = 0; i < 2; i++) for (i = 0; i < 2; i++)
serialize16(angle[i]); serialize16(angle[i]);
serialize8(MULTITYPE); serialize8(MULTITYPE);
@ -177,8 +178,8 @@ void serialCom(void)
serialize16(intPowerMeterSum); serialize16(intPowerMeterSum);
serialize16(intPowerTrigger1); serialize16(intPowerTrigger1);
serialize8(vbat); serialize8(vbat);
serialize16(BaroAlt); // 4 variables are here for general monitoring purpose serialize16(BaroAlt / 10); // 4 variables are here for general monitoring purpose
serialize16(i2cGetErrorCounter()); // debug2 serialize16(0); // debug2
serialize16(debug3); // debug3 serialize16(debug3); // debug3
serialize16(debug4); // debug4 serialize16(debug4); // debug4
serialize8('M'); serialize8('M');