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

Renaming angleInclination_t to rollAndPitchInclination. renamed angle

Browse source 
to inclination.
This commit is contained in:
Dominic Clifton 2014-05-05 17:44:18 +01:00
parent a4b16f461c
commit d352c68c9b
7 changed files with 29 additions and 29 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/flight_common.c
View file

@ -68,7 +68,7 @@ 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) - angle.raw[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);
@ -127,7 +127,7 @@ 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) - angle.raw[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);

12
src/flight_common.h
View file

@ -80,19 +80,19 @@ typedef union {
rollAndPitchTrims_t_def trims;
} rollAndPitchTrims_t;
typedef struct angleInclinations_s {
typedef struct rollAndPitchInclination_s {
// absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
int16_t rollDeciDegrees;
int16_t pitchDeciDegrees;
} angleInclinations_t_def;
} rollAndPitchInclination_t_def;
typedef union {
int16_t raw[ANGLE_INDEX_COUNT];
angleInclinations_t_def angles;
} angleInclinations_t;
int16_t rawAngles[ANGLE_INDEX_COUNT];
rollAndPitchInclination_t_def angle;
} rollAndPitchInclination_t;
extern angleInclinations_t angle;
extern rollAndPitchInclination_t inclination;
extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT];
extern int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT];

12
src/flight_imu.c
View file

@ -68,7 +68,7 @@ float magneticDeclination = 0.0f; // calculated at startup from config
int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
angleInclinations_t angle = { { 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
rollAndPitchInclination_t inclination = { { 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
float anglerad[2] = { 0.0f, 0.0f }; // absolute angle inclination in radians
static void getEstimatedAttitude(void);
@ -324,8 +324,8 @@ static void getEstimatedAttitude(void)
// Attitude of the estimated vector
anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
anglerad[AI_PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
angle.angles.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI));
angle.angles.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI));
inclination.angle.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI));
inclination.angle.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI));
if (sensors(SENSOR_MAG))
heading = calculateHeading(&EstM);
@ -356,9 +356,9 @@ static void getEstimatedAttitude(void)
#define DEGREES_80_IN_DECIDEGREES 800
bool isThrustFacingDownwards(angleInclinations_t *angleInclinations)
bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination)
{
return abs(angleInclinations->angles.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && abs(angleInclinations->angles.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES;
return abs(inclination->angle.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && abs(inclination->angle.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES;
}
int getEstimatedAltitude(void)
@ -425,7 +425,7 @@ int getEstimatedAltitude(void)
vario = applyDeadband(vel_tmp, 5);
// only calculate pid if the copters thrust is facing downwards
if (isThrustFacingDownwards(&angle)) {
if (isThrustFacingDownwards(&inclination)) {
// Altitude P-Controller
error = constrain(AltHold - EstAlt, -500, 500);
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position

12
src/flight_mixer.c
View file

@ -448,8 +448,8 @@ void mixTable(void)
break;
case MULTITYPE_GIMBAL:
servo[0] = (((int32_t)servoConf[0].rate * angle.angles.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(0);
servo[1] = (((int32_t)servoConf[1].rate * angle.angles.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(1);
servo[0] = (((int32_t)servoConf[0].rate * inclination.angle.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(0);
servo[1] = (((int32_t)servoConf[1].rate * inclination.angle.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(1);
break;
case MULTITYPE_AIRPLANE:
@ -502,11 +502,11 @@ void mixTable(void)
if (rcOptions[BOXCAMSTAB]) {
if (gimbalConfig->gimbal_flags & GIMBAL_MIXTILT) {
servo[0] -= (-(int32_t)servoConf[0].rate) * angle.angles.pitchDeciDegrees / 50 - (int32_t)servoConf[1].rate * angle.angles.rollDeciDegrees / 50;
servo[1] += (-(int32_t)servoConf[0].rate) * angle.angles.pitchDeciDegrees / 50 + (int32_t)servoConf[1].rate * angle.angles.rollDeciDegrees / 50;
servo[0] -= (-(int32_t)servoConf[0].rate) * inclination.angle.pitchDeciDegrees / 50 - (int32_t)servoConf[1].rate * inclination.angle.rollDeciDegrees / 50;
servo[1] += (-(int32_t)servoConf[0].rate) * inclination.angle.pitchDeciDegrees / 50 + (int32_t)servoConf[1].rate * inclination.angle.rollDeciDegrees / 50;
} else {
servo[0] += (int32_t)servoConf[0].rate * angle.angles.pitchDeciDegrees / 50;
servo[1] += (int32_t)servoConf[1].rate * angle.angles.rollDeciDegrees / 50;
servo[0] += (int32_t)servoConf[0].rate * inclination.angle.pitchDeciDegrees / 50;
servo[1] += (int32_t)servoConf[1].rate * inclination.angle.rollDeciDegrees / 50;
}
}
}

2
src/mw.c
View file

@ -173,7 +173,7 @@ void annexCode(void)
static uint32_t LEDTime;
if ((int32_t)(currentTime - LEDTime) >= 0) {
LEDTime = currentTime + 50000;
ledringState(f.ARMED, angle.angles.pitchDeciDegrees, angle.angles.rollDeciDegrees);
ledringState(f.ARMED, inclination.angle.pitchDeciDegrees, inclination.angle.rollDeciDegrees);
}
}
#endif

2
src/serial_msp.c
View file

@ -521,7 +521,7 @@ static void evaluateCommand(void)
case MSP_ATTITUDE:
headSerialReply(6);
for (i = 0; i < 2; i++)
serialize16(angle.raw[i]);
serialize16(inclination.rawAngles[i]);
serialize16(heading);
break;
case MSP_ALTITUDE:

14
test/flight_imu_unittest.cc
View file

@ -48,18 +48,18 @@
#define UPWARDS_THRUST false
bool isThrustFacingDownwards(angleInclinations_t *angleInclinations);
bool isThrustFacingDownwards(rollAndPitchInclination_t *inclinations);
typedef struct angleInclinationExpectation_s {
angleInclinations_t inclination;
typedef struct inclinationExpectation_s {
rollAndPitchInclination_t inclination;
bool expectDownwardsThrust;
} angleInclinationExpectation_t;
} inclinationExpectation_t;
TEST(FlightImuTest, IsThrustFacingDownwards)
{
// given
angleInclinationExpectation_t angleInclinationExpectations[] = {
inclinationExpectation_t inclinationExpectations[] = {
{ { 0, 0 }, DOWNWARDS_THRUST },
{ { 799, 799 }, DOWNWARDS_THRUST },
{ { 800, 799 }, UPWARDS_THRUST },
@ -72,12 +72,12 @@ TEST(FlightImuTest, IsThrustFacingDownwards)
{ { -800, -800 }, UPWARDS_THRUST },
{ { -801, -801 }, UPWARDS_THRUST }
};
uint8_t testIterationCount = sizeof(angleInclinationExpectations) / sizeof(angleInclinationExpectation_t);
uint8_t testIterationCount = sizeof(inclinationExpectations) / sizeof(inclinationExpectation_t);
// expect
for (uint8_t index = 0; index < testIterationCount; index ++) {
angleInclinationExpectation_t *angleInclinationExpectation = &angleInclinationExpectations[index];
inclinationExpectation_t *angleInclinationExpectation = &inclinationExpectations[index];
printf("iteration: %d\n", index);
bool result = isThrustFacingDownwards(&angleInclinationExpectation->inclination);
EXPECT_EQ(angleInclinationExpectation->expectDownwardsThrust, result);