fix for airplane pwm mode - variable wasn't initialized, resulting in non-working motors on some compilers.
synced angle/horizon mode stuff from mwc. no idea what it does. perhaps the most important part of this update: -errorAngle = constrain(2 * rcCommand[axis] - GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis]; +errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis]; (which means GPS might actually work). git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@209 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com
parent
ad1939591f
commit
4ed57b2696
5370
obj/baseflight.hex
5370
obj/baseflight.hex
File diff suppressed because it is too large
Load Diff
|
|
@ -61,6 +61,8 @@ int main(void)
|
|||
// when using airplane/wing mixer, servo/motor outputs are remapped
|
||||
if (cfg.mixerConfiguration == MULTITYPE_AIRPLANE || cfg.mixerConfiguration == MULTITYPE_FLYING_WING)
|
||||
pwm_params.airplane = true;
|
||||
else
|
||||
pwm_params.airplane = false;
|
||||
pwm_params.usePPM = feature(FEATURE_PPM);
|
||||
pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum
|
||||
pwm_params.useServos = useServo;
|
||||
|
|
|
|||
102
src/mw.c
102
src/mw.c
|
|
@ -243,20 +243,13 @@ void computeRC(void)
|
|||
}
|
||||
}
|
||||
|
||||
// #define TIMINGDEBUG
|
||||
|
||||
#ifdef TIMINGDEBUG
|
||||
uint32_t trollTime = 0;
|
||||
uint16_t cn = 0xffff, cx = 0x0;
|
||||
#endif
|
||||
|
||||
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;
|
||||
int16_t PTerm, ITerm, PTermACC, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
|
||||
static int16_t lastGyro[3] = { 0, 0, 0 };
|
||||
static int16_t delta1[3], delta2[3];
|
||||
static int16_t errorGyroI[3] = { 0, 0, 0 };
|
||||
|
|
@ -265,6 +258,7 @@ void loop(void)
|
|||
static int16_t initialThrottleHold;
|
||||
static uint32_t loopTime;
|
||||
uint16_t auxState = 0;
|
||||
int16_t prop;
|
||||
|
||||
// this will return false if spektrum is disabled. shrug.
|
||||
if (spektrumFrameComplete())
|
||||
|
|
@ -402,19 +396,30 @@ void loop(void)
|
|||
rcOptions[i] = (auxState & cfg.activate[i]) > 0;
|
||||
|
||||
// note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
|
||||
if ((rcOptions[BOXACC] || (failsafeCnt > 5 * cfg.failsafe_delay)) && (sensors(SENSOR_ACC))) {
|
||||
if ((rcOptions[BOXANGLE] || (failsafeCnt > 5 * cfg.failsafe_delay)) && (sensors(SENSOR_ACC))) {
|
||||
// bumpless transfer to Level mode
|
||||
if (!f.ACC_MODE) {
|
||||
if (!f.ANGLE_MODE) {
|
||||
errorAngleI[ROLL] = 0;
|
||||
errorAngleI[PITCH] = 0;
|
||||
f.ACC_MODE = 1;
|
||||
f.ANGLE_MODE = 1;
|
||||
}
|
||||
} else
|
||||
f.ACC_MODE = 0; // failsave support
|
||||
} else {
|
||||
f.ANGLE_MODE = 0; // failsave support
|
||||
}
|
||||
|
||||
if (rcOptions[BOXHORIZON]) {
|
||||
if (!f.HORIZON_MODE) {
|
||||
errorAngleI[ROLL] = 0;
|
||||
errorAngleI[PITCH] = 0;
|
||||
f.HORIZON_MODE = 1;
|
||||
}
|
||||
} else {
|
||||
f.HORIZON_MODE = 0;
|
||||
}
|
||||
|
||||
if ((rcOptions[BOXARM]) == 0)
|
||||
f.OK_TO_ARM = 1;
|
||||
if (f.ACC_MODE) {
|
||||
if (f.ANGLE_MODE || f.HORIZON_MODE) {
|
||||
LED1_ON;
|
||||
} else {
|
||||
LED1_OFF;
|
||||
|
|
@ -584,42 +589,57 @@ void loop(void)
|
|||
}
|
||||
}
|
||||
}
|
||||
// **** PITCH & ROLL & YAW PID ****
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
if (f.ACC_MODE && axis < 2) { // LEVEL MODE
|
||||
// 50 degrees max inclination
|
||||
errorAngle = constrain(2 * rcCommand[axis] - GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis];
|
||||
#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)
|
||||
// **** PITCH & ROLL & YAW PID ****
|
||||
prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
|
||||
// 50 degrees max inclination
|
||||
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis];
|
||||
#ifdef LEVEL_PDF
|
||||
PTermACC = -(int32_t)angle[axis] * cfg.P8[PIDLEVEL] / 100;
|
||||
#else
|
||||
PTermACC = (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
|
||||
PTermACC = constrain(PTermACC, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
|
||||
|
||||
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
|
||||
ITermACC = ((int32_t)errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12;
|
||||
}
|
||||
if (!f.ANGLE_MODE || axis == 2) { // MODE relying on GYRO or YAW axis
|
||||
error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis];
|
||||
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
|
||||
PTermGYRO = rcCommand[axis];
|
||||
|
||||
delta = gyroData[axis] - lastGyro[axis]; //16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
|
||||
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
|
||||
if (abs(gyroData[axis]) > 640)
|
||||
errorGyroI[axis] = 0;
|
||||
ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6;
|
||||
}
|
||||
if (f.HORIZON_MODE && axis < 2) {
|
||||
PTerm = ((int32_t)PTermACC * (500 - prop) + (int32_t)PTermGYRO * prop) / 500;
|
||||
ITerm = ((int32_t)ITermACC * (500 - prop) + (int32_t)ITermGYRO * prop) / 500;
|
||||
} else {
|
||||
if (f.ANGLE_MODE && axis < 2) {
|
||||
PTerm = PTermACC;
|
||||
ITerm = ITermACC;
|
||||
} else {
|
||||
PTerm = PTermGYRO;
|
||||
ITerm = ITermGYRO;
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
DTerm = ((int32_t)deltaSum * dynD8[axis]) >> 5; // 32 bits is needed for calculation
|
||||
axisPID[axis] = PTerm + ITerm - DTerm;
|
||||
}
|
||||
|
||||
mixTable();
|
||||
|
|
|
|||
79
src/mw.h
79
src/mw.h
|
|
@ -3,7 +3,7 @@
|
|||
/* for VBAT monitoring frequency */
|
||||
#define VBATFREQ 6 // to read battery voltage - nth number of loop iterations
|
||||
|
||||
#define VERSION 210
|
||||
#define VERSION 211
|
||||
|
||||
#define LAT 0
|
||||
#define LON 1
|
||||
|
|
@ -49,40 +49,49 @@ typedef enum GimbalFlags {
|
|||
} GimbalFlags;
|
||||
|
||||
/*********** RC alias *****************/
|
||||
#define ROLL 0
|
||||
#define PITCH 1
|
||||
#define YAW 2
|
||||
#define THROTTLE 3
|
||||
#define AUX1 4
|
||||
#define AUX2 5
|
||||
#define AUX3 6
|
||||
#define AUX4 7
|
||||
enum {
|
||||
ROLL = 0,
|
||||
PITCH,
|
||||
YAW,
|
||||
THROTTLE,
|
||||
AUX1,
|
||||
AUX2,
|
||||
AUX3,
|
||||
AUX4
|
||||
};
|
||||
|
||||
#define PIDALT 3
|
||||
#define PIDPOS 4
|
||||
#define PIDPOSR 5
|
||||
#define PIDNAVR 6
|
||||
#define PIDLEVEL 7
|
||||
#define PIDMAG 8
|
||||
#define PIDVEL 9 // not used currently
|
||||
enum {
|
||||
PIDROLL,
|
||||
PIDPITCH,
|
||||
PIDYAW,
|
||||
PIDALT,
|
||||
PIDPOS,
|
||||
PIDPOSR,
|
||||
PIDNAVR,
|
||||
PIDLEVEL,
|
||||
PIDMAG,
|
||||
PIDVEL,
|
||||
PIDITEMS
|
||||
};
|
||||
|
||||
#define BOXACC 0
|
||||
#define BOXBARO 1
|
||||
#define BOXMAG 2
|
||||
#define BOXCAMSTAB 3
|
||||
#define BOXCAMTRIG 4
|
||||
#define BOXARM 5
|
||||
#define BOXGPSHOME 6
|
||||
#define BOXGPSHOLD 7
|
||||
#define BOXPASSTHRU 8
|
||||
#define BOXHEADFREE 9
|
||||
#define BOXBEEPERON 10
|
||||
#define BOXLEDMAX 11 // we want maximum illumination
|
||||
#define BOXLLIGHTS 12 // enable landing lights at any altitude
|
||||
#define BOXHEADADJ 13 // acquire heading for HEADFREE mode
|
||||
|
||||
#define PIDITEMS 10
|
||||
#define CHECKBOXITEMS 14
|
||||
enum {
|
||||
BOXANGLE = 0,
|
||||
BOXHORIZON,
|
||||
BOXBARO,
|
||||
BOXMAG,
|
||||
BOXCAMSTAB,
|
||||
BOXCAMTRIG,
|
||||
BOXARM,
|
||||
BOXGPSHOME,
|
||||
BOXGPSHOLD,
|
||||
BOXPASSTHRU,
|
||||
BOXHEADFREE,
|
||||
BOXBEEPERON,
|
||||
BOXLEDMAX,
|
||||
BOXLLIGHTS,
|
||||
BOXHEADADJ,
|
||||
CHECKBOXITEMS
|
||||
};
|
||||
|
||||
#define min(a, b) ((a) < (b) ? (a) : (b))
|
||||
#define max(a, b) ((a) > (b) ? (a) : (b))
|
||||
|
|
@ -201,9 +210,9 @@ typedef struct config_t {
|
|||
typedef struct flags_t {
|
||||
uint8_t OK_TO_ARM;
|
||||
uint8_t ARMED;
|
||||
uint8_t I2C_INIT_DONE; // For i2c gps we have to now when i2c init is done, so we can update parameters to the i2cgps from eeprom (at startup it is done in setup())
|
||||
uint8_t ACC_CALIBRATED;
|
||||
uint8_t ACC_MODE;
|
||||
uint8_t ANGLE_MODE;
|
||||
uint8_t HORIZON_MODE;
|
||||
uint8_t MAG_MODE;
|
||||
uint8_t BARO_MODE;
|
||||
uint8_t GPS_HOME_MODE;
|
||||
|
|
|
|||
|
|
@ -38,12 +38,14 @@
|
|||
|
||||
#define MSP_EEPROM_WRITE 250 //in message no param
|
||||
|
||||
#define MSP_DEBUGMSG 253 //out message debug string buffer
|
||||
#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
|
||||
|
||||
#define INBUF_SIZE 64
|
||||
|
||||
static const char boxnames[] =
|
||||
"ACC;"
|
||||
"ANGLE;"
|
||||
"HORIZON;"
|
||||
"BARO;"
|
||||
"MAG;"
|
||||
"CAMSTAB;"
|
||||
|
|
@ -225,7 +227,8 @@ static void evaluateCommand(void)
|
|||
serialize16(cycleTime);
|
||||
serialize16(i2cGetErrorCounter());
|
||||
serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
|
||||
serialize32(f.ACC_MODE << BOXACC | f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.ARMED << BOXARM | rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
|
||||
serialize32(f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON | f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.ARMED << BOXARM |
|
||||
rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
|
||||
f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD | f.HEADFREE_MODE << BOXHEADFREE | f.PASSTHRU_MODE << BOXPASSTHRU |
|
||||
rcOptions[BOXBEEPERON] << BOXBEEPERON | rcOptions[BOXLEDMAX] << BOXLEDMAX | rcOptions[BOXLLIGHTS] << BOXLLIGHTS | rcOptions[BOXHEADADJ] << BOXHEADADJ);
|
||||
break;
|
||||
|
|
|
|||
Loading…
Reference in New Issue