Merge pull request #424 from avoid3d/test-inclination-angle
Test inclination angle
This commit is contained in:
commit
90b012bc06
|
@ -175,12 +175,16 @@ bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination)
|
|||
return ABS(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && ABS(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES;
|
||||
}
|
||||
|
||||
/*
|
||||
* This (poorly named) function merely returns whichever is higher, roll inclination or pitch inclination.
|
||||
* //TODO: Fix this up. We could either actually return the angle between 'down' and the normal of the craft
|
||||
* (my best interpretation of scalar 'tiltAngle') or rename the function.
|
||||
*/
|
||||
int16_t calculateTiltAngle(rollAndPitchInclination_t *inclination)
|
||||
{
|
||||
return MAX(ABS(inclination->values.rollDeciDegrees), ABS(inclination->values.pitchDeciDegrees));
|
||||
}
|
||||
|
||||
|
||||
int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old)
|
||||
{
|
||||
int32_t result = 0;
|
||||
|
|
|
@ -54,6 +54,7 @@ extern "C" {
|
|||
|
||||
extern "C" {
|
||||
bool isThrustFacingDownwards(rollAndPitchInclination_t *inclinations);
|
||||
uint16_t calculateTiltAngle(rollAndPitchInclination_t *inclinations);
|
||||
}
|
||||
|
||||
typedef struct inclinationExpectation_s {
|
||||
|
@ -90,6 +91,36 @@ TEST(AltitudeHoldTest, IsThrustFacingDownwards)
|
|||
}
|
||||
}
|
||||
|
||||
typedef struct inclinationAngleExpectations_s {
|
||||
rollAndPitchInclination_t inclination;
|
||||
uint16_t expected_angle;
|
||||
} inclinationAngleExpectations_t;
|
||||
|
||||
TEST(AltitudeHoldTest, TestCalculateTiltAngle)
|
||||
{
|
||||
inclinationAngleExpectations_t inclinationAngleExpectations[] = {
|
||||
{ {0, 0}, 0},
|
||||
{ {1, 0}, 1},
|
||||
{ {0, 1}, 1},
|
||||
{ {0, -1}, 1},
|
||||
{ {-1, 0}, 1},
|
||||
{ {-1, -2}, 2},
|
||||
{ {-2, -1}, 2},
|
||||
{ {1, 2}, 2},
|
||||
{ {2, 1}, 2}
|
||||
};
|
||||
|
||||
rollAndPitchInclination_t inclination = {0, 0};
|
||||
uint16_t tilt_angle = calculateTiltAngle(&inclination);
|
||||
EXPECT_EQ(tilt_angle, 0);
|
||||
|
||||
for (uint8_t i = 0; i < 9; i++) {
|
||||
inclinationAngleExpectations_t *expectation = &inclinationAngleExpectations[i];
|
||||
uint16_t result = calculateTiltAngle(&expectation->inclination);
|
||||
EXPECT_EQ(expectation->expected_angle, result);
|
||||
}
|
||||
}
|
||||
|
||||
// STUBS
|
||||
|
||||
extern "C" {
|
||||
|
|
Loading…
Reference in New Issue