diff --git a/src/graphics/Screen.cpp b/src/graphics/Screen.cpp index 2359c4dc2..1fa27339d 100644 --- a/src/graphics/Screen.cpp +++ b/src/graphics/Screen.cpp @@ -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 \ No newline at end of file + +#endif // HAS_SCREEN