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;
|
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)
|
int16_t calculateTiltAngle(rollAndPitchInclination_t *inclination)
|
||||||
{
|
{
|
||||||
return MAX(ABS(inclination->values.rollDeciDegrees), ABS(inclination->values.pitchDeciDegrees));
|
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 calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old)
|
||||||
{
|
{
|
||||||
int32_t result = 0;
|
int32_t result = 0;
|
||||||
|
|
|
@ -54,6 +54,7 @@ extern "C" {
|
||||||
|
|
||||||
extern "C" {
|
extern "C" {
|
||||||
bool isThrustFacingDownwards(rollAndPitchInclination_t *inclinations);
|
bool isThrustFacingDownwards(rollAndPitchInclination_t *inclinations);
|
||||||
|
uint16_t calculateTiltAngle(rollAndPitchInclination_t *inclinations);
|
||||||
}
|
}
|
||||||
|
|
||||||
typedef struct inclinationExpectation_s {
|
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
|
// STUBS
|
||||||
|
|
||||||
extern "C" {
|
extern "C" {
|
||||||
|
|
Loading…
Reference in New Issue