diff --git a/src/flight_common.c b/src/flight_common.c index 7c493e60a3..748198492c 100644 --- a/src/flight_common.c +++ b/src/flight_common.c @@ -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); diff --git a/src/flight_common.h b/src/flight_common.h index 1fae4d84b7..2f1c4fc161 100644 --- a/src/flight_common.h +++ b/src/flight_common.h @@ -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]; diff --git a/src/flight_imu.c b/src/flight_imu.c index ab3610928c..3cc4b4299b 100644 --- a/src/flight_imu.c +++ b/src/flight_imu.c @@ -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 diff --git a/src/flight_mixer.c b/src/flight_mixer.c index 183e7ef9be..04e512ffbb 100755 --- a/src/flight_mixer.c +++ b/src/flight_mixer.c @@ -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; } } } diff --git a/src/mw.c b/src/mw.c index 75527cabb3..2022228f01 100755 --- a/src/mw.c +++ b/src/mw.c @@ -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 diff --git a/src/serial_msp.c b/src/serial_msp.c index 856e03c3db..ccccbf1418 100755 --- a/src/serial_msp.c +++ b/src/serial_msp.c @@ -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: diff --git a/test/flight_imu_unittest.cc b/test/flight_imu_unittest.cc index 884e182ffc..2d7040785d 100644 --- a/test/flight_imu_unittest.cc +++ b/test/flight_imu_unittest.cc @@ -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);