englebert
/
inav-simplefc
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
master
timecop@gmail.com 12 years ago
parent
ad1939591f
commit
4ed57b2696
5 changed files with 2799 additions and 2749 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5370
      obj/baseflight.hex
  2. 2
      src/main.c
  3. 86
      src/mw.c
  4. 83
      src/mw.h
  5. 7
      src/serial.c

5370
obj/baseflight.hex
File diff suppressed because it is too large
View File

2
src/main.c
View File

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

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)
{
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 ****
prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
for (axis = 0; axis < 3; axis++) {
if (f.ACC_MODE && axis < 2) { // LEVEL MODE
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];
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;
PTermACC = -(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
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
PTerm = constrain(PTerm, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
PTermACC = constrain(PTermACC, -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)
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)
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
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
ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6;
}
PTerm -= (int32_t) gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
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
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();

83
src/mw.h
View File

@ -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
#define PIDALT 3
#define PIDPOS 4
#define PIDPOSR 5
#define PIDNAVR 6
#define PIDLEVEL 7
#define PIDMAG 8
#define PIDVEL 9 // not used currently
#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 {
ROLL = 0,
PITCH,
YAW,
THROTTLE,
AUX1,
AUX2,
AUX3,
AUX4
};
enum {
PIDROLL,
PIDPITCH,
PIDYAW,
PIDALT,
PIDPOS,
PIDPOSR,
PIDNAVR,
PIDLEVEL,
PIDMAG,
PIDVEL,
PIDITEMS
};
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;

7
src/serial.c
View File

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

Write Preview
Loading…
Cancel
Save