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

format code properly

Browse source 
match the comment from pullrequest about spacing
remains : some hand alignment for comment and wrong /** */ usage.
Conflicts:

	src/board.h
	src/buzzer.c
	src/config.c
	src/drivers/serial_common.h
	src/drivers/system_common.c
	src/drv_gpio.h
	src/drv_pwm.c
	src/drv_timer.c
	src/drv_uart.c
	src/flight_imu.c
	src/mw.c
	src/serial_cli.c
This commit is contained in:
treymarc 2014-05-08 00:36:19 +00:00 • committed by Dominic Clifton
parent ab2273f93e
commit cabc57774c
27 changed files with 141 additions and 135 deletions
Download patch file Download diff file Expand all files Collapse all files

34
src/flight_common.c
View file

@ -17,17 +17,17 @@ int16_t heading, magHold;
int16_t axisPID[3];
uint8_t dynP8[3], dynI8[3], dynD8[3];
static int32_t errorGyroI[3] = { 0, 0, 0 };
static int32_t errorAngleI[2] = { 0, 0 };
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim);
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim);
typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim); // pid controller function prototype
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim); // pid controller function prototype
pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
void mwDisarm(void)
{
if (f.ARMED)
@ -53,7 +53,8 @@ void resetErrorGyro(void)
errorGyroI[YAW] = 0;
}
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
{
int axis, prop;
int32_t error, errorAngle;
@ -68,7 +69,8 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
for (axis = 0; axis < 3; axis++) {
if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC
// 50 degrees max inclination
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis];
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis];
PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
@ -76,7 +78,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
ITermACC = (errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12;
}
if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == FD_YAW) { // MODE relying on GYRO or YAW axis
error = (int32_t)rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
error -= gyroData[axis];
PTermGYRO = rcCommand[axis];
@ -99,7 +101,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
}
}
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];
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
@ -112,7 +114,8 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
#define GYRO_I_MAX 256
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
rollAndPitchTrims_t *angleTrim)
{
int32_t errorAngle = 0;
int axis;
@ -127,13 +130,14 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// -----Get the desired angle rate depending on flight mode
if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) { // MODE relying on ACC
// calculate error and limit the angle to max configured inclination
errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis]; // 16 bits is ok here
errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis]; // 16 bits is ok here
}
if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
AngleRateTmp = (((int32_t)(controlRateConfig->yawRate + 27) * rcCommand[2]) >> 5);
} else {
} else {
if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRateTmp = ((int32_t) (controlRateConfig->rollPitchRate + 27) * rcCommand[axis]) >> 4;
AngleRateTmp = ((int32_t)(controlRateConfig->rollPitchRate + 27) * rcCommand[axis]) >> 4;
if (f.HORIZON_MODE) {
// mix up angle error to desired AngleRateTmp to add a little auto-level feel
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL]) >> 8;
@ -160,16 +164,16 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t)-GYRO_I_MAX << 13, (int32_t)+GYRO_I_MAX << 13);
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastError[axis] = RateError;
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.
delta = (delta * ((uint16_t)0xFFFF / (cycleTime >> 4))) >> 6;
delta = (delta * ((uint16_t) 0xFFFF / (cycleTime >> 4))) >> 6;
// add moving average here to reduce noise
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];