Merge pull request #1551 from loodydo/loodydo-Compass-Fix

Update Screen.cpp
This commit is contained in:
Thomas Göttgens 2022-08-04 11:34:38 +02:00 committed by GitHub
commit a9ad314307
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23

View File

@ -64,6 +64,11 @@ namespace graphics
static FrameCallback normalFrames[MAX_NUM_NODES + NUM_EXTRA_FRAMES];
static uint32_t targetFramerate = IDLE_FRAMERATE;
static char btPIN[16] = "888888";
// This defines the layout of the compass.
// If true, North with remain static at the top of the compass.
// If false, your current heading is static at the top of the compass.
bool compassNorthTop = false;
// This image definition is here instead of images.h because it's modified dynamically by the drawBattery function
uint8_t imgBattery[16] = {0xFF, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0xE7, 0x3C};
@ -595,7 +600,7 @@ class Point
void rotate(float radian)
{
float cos = cosf(radian), sin = sinf(radian);
float rx = x * cos - y * sin, ry = x * sin + y * cos;
float rx = x * cos + y * sin, ry = -x * sin + y * cos;
x = rx;
y = ry;
@ -609,8 +614,10 @@ class Point
void scale(float f)
{
//We use -f here to counter the flip that happens
//on the y axis when drawing and rotating on screen
x *= f;
y *= f;
y *= -f;
}
};
@ -682,16 +689,21 @@ static void drawNodeHeading(OLEDDisplay *display, int16_t compassX, int16_t comp
drawLine(display, rightArrow, tip);
}
// Draw the compass heading
static void drawCompassHeading(OLEDDisplay *display, int16_t compassX, int16_t compassY, float myHeading)
// Draw north
static void drawCompassNorth(OLEDDisplay *display, int16_t compassX, int16_t compassY, float myHeading)
{
Point N1(-0.04f, -0.65f), N2(0.04f, -0.65f);
Point N3(-0.04f, -0.55f), N4(0.04f, -0.55f);
//If north is supposed to be at the top of the compass we want rotation to be +0
if(compassNorthTop)
myHeading = -0;
Point N1(-0.04f, 0.65f), N2(0.04f, 0.65f);
Point N3(-0.04f, 0.55f), N4(0.04f, 0.55f);
Point *rosePoints[] = {&N1, &N2, &N3, &N4};
for (int i = 0; i < 4; i++) {
rosePoints[i]->rotate(myHeading);
rosePoints[i]->scale(-1 * COMPASS_DIAM);
// North on compass will be negative of heading
rosePoints[i]->rotate(-myHeading);
rosePoints[i]->scale(COMPASS_DIAM);
rosePoints[i]->translate(compassX, compassY);
}
drawLine(display, N1, N3);
@ -762,7 +774,7 @@ static void drawNodeInfo(OLEDDisplay *display, OLEDDisplayUiState *state, int16_
if (ourNode && hasPosition(ourNode)) {
Position &op = ourNode->position;
float myHeading = estimatedHeading(DegD(op.latitude_i), DegD(op.longitude_i));
drawCompassHeading(display, compassX, compassY, myHeading);
drawCompassNorth(display, compassX, compassY, myHeading);
if (hasPosition(node)) {
// display direction toward node
@ -775,12 +787,13 @@ static void drawNodeInfo(OLEDDisplay *display, OLEDDisplayUiState *state, int16_
else
snprintf(distStr, sizeof(distStr), "%.1f km", d / 1000);
// FIXME, also keep the guess at the operators heading and add/substract
// it. currently we don't do this and instead draw north up only.
float bearingToOther =
GeoCoord::bearing(DegD(p.latitude_i), DegD(p.longitude_i), DegD(op.latitude_i), DegD(op.longitude_i));
float headingRadian = bearingToOther - myHeading;
drawNodeHeading(display, compassX, compassY, headingRadian);
GeoCoord::bearing(DegD(op.latitude_i), DegD(op.longitude_i), DegD(p.latitude_i), DegD(p.longitude_i));
// If the top of the compass is a static north then bearingToOther can be drawn on the compass directly
// If the top of the compass is not a static north we need adjust bearingToOther based on heading
if(!compassNorthTop)
bearingToOther -= myHeading;
drawNodeHeading(display, compassX, compassY, bearingToOther);
}
}
if (!hasNodeHeading)
@ -1667,4 +1680,5 @@ int Screen::handleUIFrameEvent(const UIFrameEvent *event)
}
} // namespace graphics
#endif // HAS_SCREEN
#endif // HAS_SCREEN