Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 19:40:31 +03:00
Code Issues Wiki Activity

fix for airplane pwm mode - variable wasn't initialized, resulting in non-working motors on some compilers.

Browse source 
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 2012-09-08 08:15:16 +00:00
parent ad1939591f
commit 4ed57b2696
5 changed files with 2805 additions and 2755 deletions
Download patch file Download diff file Expand all files Collapse all files

5370
obj/baseflight.hex
View file

File diff suppressed because it is too large Load diff

2
src/main.c
View file

@ -61,6 +61,8 @@ int main(void)
// when using airplane/wing mixer, servo/motor outputs are remapped // when using airplane/wing mixer, servo/motor outputs are remapped
if (cfg.mixerConfiguration == MULTITYPE_AIRPLANE || cfg.mixerConfiguration == MULTITYPE_FLYING_WING) if (cfg.mixerConfiguration == MULTITYPE_AIRPLANE || cfg.mixerConfiguration == MULTITYPE_FLYING_WING)
pwm_params.airplane = true; pwm_params.airplane = true;
else
pwm_params.airplane = false;
pwm_params.usePPM = feature(FEATURE_PPM); pwm_params.usePPM = feature(FEATURE_PPM);
pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum
pwm_params.useServos = useServo; pwm_params.useServos = useServo;

86
src/mw.c
View file

@ -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) 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 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; uint8_t axis, i;
int16_t error, errorAngle; int16_t error, errorAngle;
int16_t delta, deltaSum; 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 lastGyro[3] = { 0, 0, 0 };
static int16_t delta1[3], delta2[3]; static int16_t delta1[3], delta2[3];
static int16_t errorGyroI[3] = { 0, 0, 0 }; static int16_t errorGyroI[3] = { 0, 0, 0 };
@ -265,6 +258,7 @@ void loop(void)
static int16_t initialThrottleHold; static int16_t initialThrottleHold;
static uint32_t loopTime; static uint32_t loopTime;
uint16_t auxState = 0; uint16_t auxState = 0;
int16_t prop;
// this will return false if spektrum is disabled. shrug. // this will return false if spektrum is disabled. shrug.
if (spektrumFrameComplete()) if (spektrumFrameComplete())
@ -402,19 +396,30 @@ void loop(void)
rcOptions[i] = (auxState & cfg.activate[i]) > 0; rcOptions[i] = (auxState & cfg.activate[i]) > 0;
// 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 ((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 // bumpless transfer to Level mode
if (!f.ACC_MODE) { if (!f.ANGLE_MODE) {
errorAngleI[ROLL] = 0; errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0; errorAngleI[PITCH] = 0;
f.ACC_MODE = 1; f.ANGLE_MODE = 1;
}
} 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;
} }
} else
f.ACC_MODE = 0; // failsave support
if ((rcOptions[BOXARM]) == 0) if ((rcOptions[BOXARM]) == 0)
f.OK_TO_ARM = 1; f.OK_TO_ARM = 1;
if (f.ACC_MODE) { if (f.ANGLE_MODE || f.HORIZON_MODE) {
LED1_ON; LED1_ON;
} else { } else {
LED1_OFF; LED1_OFF;
@ -584,31 +589,47 @@ 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 // **** PITCH & ROLL & YAW PID ****
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 prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
} else { // ACRO MODE or YAW axis for (axis = 0; axis < 3; 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) 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]; error -= gyroData[axis];
PTerm = rcCommand[axis]; PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp // 16 bits is ok here errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if (abs(gyroData[axis]) > 640) if (abs(gyroData[axis]) > 640)
errorGyroI[axis] = 0; errorGyroI[axis] = 0;
ITerm = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 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 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 delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
@ -618,7 +639,6 @@ void loop(void)
delta1[axis] = delta; delta1[axis] = delta;
DTerm = ((int32_t)deltaSum * dynD8[axis]) >> 5; // 32 bits is needed for calculation DTerm = ((int32_t)deltaSum * dynD8[axis]) >> 5; // 32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm - DTerm; axisPID[axis] = PTerm + ITerm - DTerm;
} }

79
src/mw.h
View file

@ -3,7 +3,7 @@
/* for VBAT monitoring frequency */ /* for VBAT monitoring frequency */
#define VBATFREQ 6 // to read battery voltage - nth number of loop iterations #define VBATFREQ 6 // to read battery voltage - nth number of loop iterations
#define VERSION 210 #define VERSION 211
#define LAT 0 #define LAT 0
#define LON 1 #define LON 1
@ -49,40 +49,49 @@ typedef enum GimbalFlags {
} GimbalFlags; } GimbalFlags;
/*********** RC alias *****************/ /*********** RC alias *****************/
#define ROLL 0 enum {
#define PITCH 1 ROLL = 0,
#define YAW 2 PITCH,
#define THROTTLE 3 YAW,
#define AUX1 4 THROTTLE,
#define AUX2 5 AUX1,
#define AUX3 6 AUX2,
#define AUX4 7 AUX3,
AUX4
};
#define PIDALT 3 enum {
#define PIDPOS 4 PIDROLL,
#define PIDPOSR 5 PIDPITCH,
#define PIDNAVR 6 PIDYAW,
#define PIDLEVEL 7 PIDALT,
#define PIDMAG 8 PIDPOS,
#define PIDVEL 9 // not used currently PIDPOSR,
PIDNAVR,
PIDLEVEL,
PIDMAG,
PIDVEL,
PIDITEMS
};
#define BOXACC 0 enum {
#define BOXBARO 1 BOXANGLE = 0,
#define BOXMAG 2 BOXHORIZON,
#define BOXCAMSTAB 3 BOXBARO,
#define BOXCAMTRIG 4 BOXMAG,
#define BOXARM 5 BOXCAMSTAB,
#define BOXGPSHOME 6 BOXCAMTRIG,
#define BOXGPSHOLD 7 BOXARM,
#define BOXPASSTHRU 8 BOXGPSHOME,
#define BOXHEADFREE 9 BOXGPSHOLD,
#define BOXBEEPERON 10 BOXPASSTHRU,
#define BOXLEDMAX 11 // we want maximum illumination BOXHEADFREE,
#define BOXLLIGHTS 12 // enable landing lights at any altitude BOXBEEPERON,
#define BOXHEADADJ 13 // acquire heading for HEADFREE mode BOXLEDMAX,
BOXLLIGHTS,
#define PIDITEMS 10 BOXHEADADJ,
#define CHECKBOXITEMS 14 CHECKBOXITEMS
};
#define min(a, b) ((a) < (b) ? (a) : (b)) #define min(a, b) ((a) < (b) ? (a) : (b))
#define max(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 { typedef struct flags_t {
uint8_t OK_TO_ARM; uint8_t OK_TO_ARM;
uint8_t ARMED; 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_CALIBRATED;
uint8_t ACC_MODE; uint8_t ANGLE_MODE;
uint8_t HORIZON_MODE;
uint8_t MAG_MODE; uint8_t MAG_MODE;
uint8_t BARO_MODE; uint8_t BARO_MODE;
uint8_t GPS_HOME_MODE; uint8_t GPS_HOME_MODE;

7
src/serial.c
View file

@ -38,12 +38,14 @@
#define MSP_EEPROM_WRITE 250 //in message no param #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 MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
#define INBUF_SIZE 64 #define INBUF_SIZE 64
static const char boxnames[] = static const char boxnames[] =
"ACC;" "ANGLE;"
"HORIZON;"
"BARO;" "BARO;"
"MAG;" "MAG;"
"CAMSTAB;" "CAMSTAB;"
@ -225,7 +227,8 @@ static void evaluateCommand(void)
serialize16(cycleTime); serialize16(cycleTime);
serialize16(i2cGetErrorCounter()); serialize16(i2cGetErrorCounter());
serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); 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 | 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); rcOptions[BOXBEEPERON] << BOXBEEPERON | rcOptions[BOXLEDMAX] << BOXLEDMAX | rcOptions[BOXLLIGHTS] << BOXLLIGHTS | rcOptions[BOXHEADADJ] << BOXHEADADJ);
break; break;