X-Plane 針路バグとエレベータトリムの切り替え

#include <Encoder.h>
#include <Bounce.h>
  
enum pins {
  MyEnc_A = 7,
  MyEnc_B = 8,
  ModeSwitchPin = 20,
  HdgModeLED = 11,
  ElevTrimLED = 13
};
  
// Encoder things
Encoder myEnc(MyEnc_A, MyEnc_B);
short myEncPrev = 0;
  
// Mode things
Bounce modeSwitch = Bounce (ModeSwitchPin, 5);
enum Modes {
  Mode_Heading, // automatically = 0
  Mode_ElevTrim, // automatically = 1
  Mode_Count // automagically = 2, and we have 2 modes, that's convenient!
};
int mode = 0;
  
// Heading mode things
FlightSimFloat headingBug;
  
// Elev trim mode things
FlightSimFloat elevTrim;
FlightSimFloat elevMin;
FlightSimFloat elevMax;
float elevTrimScalar = 0.005;
  
void setup() {
  pinMode(ModeSwitchPin, INPUT_PULLUP);
  pinMode(HdgModeLED, OUTPUT);
  pinMode(ElevTrimLED, OUTPUT);
  
  headingBug = XPlaneRef("sim/cockpit2/autopilot/heading_dial_deg_mag_pilot");
  elevTrim = XPlaneRef("sim/cockpit2/controls/elevator_trim");
  elevMin = XPlaneRef("sim/aircraft/controls/acf_min_trim_elev");
  elevMax = XPlaneRef("sim/aircraft/controls/acf_max_trim_elev");
}
  
void loop() {
  FlightSim.update();
  modeSwitch.update();
  
  // change mode when switch pressed
  if(modeSwitch.fallingEdge()) {
    ++mode;
    if(mode >= Mode_Count)
      mode = 0;
  }
  
  //light status LEDs
  digitalWrite(HdgModeLED, (mode == Mode_Heading));
  digitalWrite(ElevTrimLED, (mode == Mode_ElevTrim));
  
  short myEncDiff = (myEnc.read() - myEncPrev) / 4;
  
  if (myEncDiff) {
    myEncPrev = 0;
    myEnc.write(0);
  
    if (mode == Mode_Heading) {
      float tmp = headingBug;
      tmp += myEncDiff;
      while (tmp < 0.0) tmp += 360.0;
      while (tmp >= 360.0) tmp -= 360.0;
      headingBug = tmp;
    }
  
    if (mode == Mode_ElevTrim) {
      float tmp = elevTrim;
      tmp += (myEncDiff * elevTrimScalar);
      while (tmp < -elevMin) tmp = -elevMin;
      while (tmp > elevMax) tmp = elevMax;
      elevTrim = tmp;
    }
  } //if encDiff
} // loop