Digital Rudder Heading Alarm

#include <Arduino.h>
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_BNO055.h>
#include <GxEPD2_BW.h>
#include <freertos/FreeRTOS.h>
#include <freertos/queue.h>
#include <freertos/task.h>
#include <math.h>
Adafruit_BNO055 bno(55,0x28);
GxEPD2_BW<GxEPD2_290_T94_V2,GxEPD2_290_T94_V2::HEIGHT> display(GxEPD2_290_T94_V2(27,26,25,33));
struct Frame { int heading,target,error,state; };
QueueHandle_t frames;
bool sensor=false,armed=false; float target=0;
float wrap(float a){a=fmodf(a+360,360);return a<0?a+360:a;}
float difference(float a,float b){return wrap(a-b+180)-180;}
struct Button {int pin;bool raw=true,stable=true;uint32_t changed=0;
 bool pressed(){bool v=digitalRead(pin);if(v!=raw){raw=v;changed=millis();}if(v!=stable && millis()-changed>=35){stable=v;return !v;}return false;}
};
Button buttons[]={{13},{14},{16}};
void screenTask(void*){
 SPI.begin(18,19,23,27); display.init(115200,true,2,false); display.setRotation(1); display.setTextColor(GxEPD_BLACK); display.setTextWrap(false);
 Frame f,last={-999,-999,-999,-999}; unsigned updates=0;
 for(;;){
 if(xQueueReceive(frames,&f,portMAX_DELAY)!=pdTRUE)continue;
 if(f.heading==last.heading&&f.target==last.target&&f.error==last.error&&f.state==last.state)continue;
 if(updates++%60==0)display.setFullWindow();else display.setPartialWindow(0,0,296,128);
 display.firstPage();do{
 display.fillScreen(GxEPD_WHITE);display.setTextSize(2);display.setCursor(4,3);display.print("TEKNE YONU");
 display.setCursor(4,25);display.setTextSize(5);
 if(f.state<2)display.print("---");else {char s[8];snprintf(s,sizeof(s),"%03d",f.heading);display.print(s);} display.setTextSize(2);display.print(" deg");
 display.setCursor(4,70);display.printf("HEDEF %03d  FARK %+d",f.target,f.error);
 display.setCursor(4,96);
 const char* text=f.state==0?"SENSOR HATASI":f.state==1?"KALIBRASYON GEREK":f.state==2?"SET: HEDEF KAYDET":f.state==4?"SAPMA >3 DERECE":"ROTA TAMAM";
 display.print(text);
 }while(display.nextPage());display.powerOff();last=f;
 vTaskDelay(pdMS_TO_TICKS(500));
 }
}
void setup(){
 Serial.begin(115200); for(auto &b:buttons)pinMode(b.pin,INPUT_PULLUP);pinMode(32,OUTPUT);noTone(32);
 Wire.begin(21,22);Wire.setClock(100000);Wire.setTimeOut(100);
 sensor=bno.begin();frames=xQueueCreate(1,sizeof(Frame));
 xTaskCreate(screenTask,"screen",6144,nullptr,1,nullptr);
}
void loop(){
 bool press[3];for(int i=0;i<3;i++)press[i]=buttons[i].pressed();
 uint8_t sys=0,g=0,a=0,m=0,status=0,test=0,err=0; float h=0;
 if(sensor){bno.getCalibration(&sys,&g,&a,&m);bno.getSystemStatus(&status,&test,&err);h=bno.getVector(Adafruit_BNO055::VECTOR_EULER).x();}
 bool valid=sensor&&status==5&&err==0&&sys==3&&g==3&&a==3&&m==3&&isfinite(h);
 if(!valid)armed=false;
 if(valid&&press[0]){target=wrap(h);armed=true;}
 if(valid&&press[1]){target=wrap(target-1);armed=true;}
 if(valid&&press[2]){target=wrap(target+1);armed=true;}
 float delta=difference(h,target);bool alarm=valid&&armed&&fabsf(delta)>3.0f;
 static bool sounding=false;bool beep=alarm&&(millis()%800<180);
 // Invalid readings use a distinct double chirp, never a misleading 'on course' silence.
 if(!valid)beep=(millis()%2500<100)||(millis()%2500>=200&&millis()%2500<300);
 if(beep!=sounding){if(beep)tone(32,4000);else noTone(32);sounding=beep;}
 Frame f={(int)lroundf(wrap(h))%360,(int)lroundf(target)%360,(int)lroundf(delta),!sensor||err?0:!valid?1:!armed?2:alarm?4:3};
 xQueueOverwrite(frames,&f);delay(50);
}