Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +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++) { for (axis = 0; axis < 3; axis++) {
if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC
// 50 degrees max inclination // 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 = 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); 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 // -----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 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 // 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) if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
AngleRateTmp = (((int32_t)(controlRateConfig->yawRate + 27) * rcCommand[2]) >> 5); 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_def trims;
} rollAndPitchTrims_t; } rollAndPitchTrims_t;
typedef struct angleInclinations_s { typedef struct rollAndPitchInclination_s {
// absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
int16_t rollDeciDegrees; int16_t rollDeciDegrees;
int16_t pitchDeciDegrees; int16_t pitchDeciDegrees;
} angleInclinations_t_def; } rollAndPitchInclination_t_def;
typedef union { typedef union {
int16_t raw[ANGLE_INDEX_COUNT]; int16_t rawAngles[ANGLE_INDEX_COUNT];
angleInclinations_t_def angles; rollAndPitchInclination_t_def angle;
} angleInclinations_t; } rollAndPitchInclination_t;
extern angleInclinations_t angle; extern rollAndPitchInclination_t inclination;
extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT]; extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT];
extern int16_t gyroZero[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 gyroData[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
int16_t gyroZero[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 float anglerad[2] = { 0.0f, 0.0f }; // absolute angle inclination in radians
static void getEstimatedAttitude(void); static void getEstimatedAttitude(void);
@ -324,8 +324,8 @@ static void getEstimatedAttitude(void)
// Attitude of the estimated vector // Attitude of the estimated vector
anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z); 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)); 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)); inclination.angle.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI));
angle.angles.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI)); inclination.angle.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI));
if (sensors(SENSOR_MAG)) if (sensors(SENSOR_MAG))
heading = calculateHeading(&EstM); heading = calculateHeading(&EstM);
@ -356,9 +356,9 @@ static void getEstimatedAttitude(void)
#define DEGREES_80_IN_DECIDEGREES 800 #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) int getEstimatedAltitude(void)
@ -425,7 +425,7 @@ int getEstimatedAltitude(void)
vario = applyDeadband(vel_tmp, 5); vario = applyDeadband(vel_tmp, 5);
// only calculate pid if the copters thrust is facing downwards // only calculate pid if the copters thrust is facing downwards
if (isThrustFacingDownwards(&angle)) { if (isThrustFacingDownwards(&inclination)) {
// Altitude P-Controller // Altitude P-Controller
error = constrain(AltHold - EstAlt, -500, 500); error = constrain(AltHold - EstAlt, -500, 500);
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position 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; break;
case MULTITYPE_GIMBAL: case MULTITYPE_GIMBAL:
servo[0] = (((int32_t)servoConf[0].rate * angle.angles.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(0); servo[0] = (((int32_t)servoConf[0].rate * inclination.angle.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(0);
servo[1] = (((int32_t)servoConf[1].rate * angle.angles.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(1); servo[1] = (((int32_t)servoConf[1].rate * inclination.angle.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(1);
break; break;
case MULTITYPE_AIRPLANE: case MULTITYPE_AIRPLANE:
@ -502,11 +502,11 @@ void mixTable(void)
if (rcOptions[BOXCAMSTAB]) { if (rcOptions[BOXCAMSTAB]) {
if (gimbalConfig->gimbal_flags & GIMBAL_MIXTILT) { 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[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) * angle.angles.pitchDeciDegrees / 50 + (int32_t)servoConf[1].rate * angle.angles.rollDeciDegrees / 50; servo[1] += (-(int32_t)servoConf[0].rate) * inclination.angle.pitchDeciDegrees / 50 + (int32_t)servoConf[1].rate * inclination.angle.rollDeciDegrees / 50;
} else { } else {
servo[0] += (int32_t)servoConf[0].rate * angle.angles.pitchDeciDegrees / 50; servo[0] += (int32_t)servoConf[0].rate * inclination.angle.pitchDeciDegrees / 50;
servo[1] += (int32_t)servoConf[1].rate * angle.angles.rollDeciDegrees / 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; static uint32_t LEDTime;
if ((int32_t)(currentTime - LEDTime) >= 0) { if ((int32_t)(currentTime - LEDTime) >= 0) {
LEDTime = currentTime + 50000; LEDTime = currentTime + 50000;
ledringState(f.ARMED, angle.angles.pitchDeciDegrees, angle.angles.rollDeciDegrees); ledringState(f.ARMED, inclination.angle.pitchDeciDegrees, inclination.angle.rollDeciDegrees);
} }
} }
#endif #endif

2
src/serial_msp.c
View file

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

14
test/flight_imu_unittest.cc
View file

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