src/main/tracker/main.c

   1 /*
   2  * This file is part of u360gts, aka amv-open360tracker 32bits:
   3  * https://github.com/raul-ortega/amv-open360tracker-32bits
   4  *
   5  * The code by itself do nothing, it have to be combined with cleanflight
   6  * source code to be run on NAZE32 based boards for controlling a
   7  * DIY continuous 360 degree rotating antenna tracker system for FPV.
   8  *
   9  * Some functions are adaptations by Ra�l Ortega of the original code written by Samuel Brucksch:
  10  * https://github.com/SamuelBrucksch/open360tracker *
  11  *
  12  * u360gts is free software: you can redistribute it and/or modify
  13  * it under the terms of the GNU General Public License as published by
  14  * the Free Software Foundation, either version 3 of the License, or
  15  * (at your option) any later version.
  16  *
  17  * u360gts is distributed in the hope that it will be useful,
  18  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  19  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  20  * GNU General Public License for more details.
  21  *
  22  * You should have received a copy of the GNU General Public License
  23  * along with u360gts.  If not, see <http://www.gnu.org/licenses/>.
  24  *
  25  */
  26
  27 #include <stdbool.h>
  28 #include <stdint.h>
  29 #include <stdlib.h>
  30 #include <string.h>
  31
  32 #include "platform.h"
  33
  34 #include "common/axis.h"
  35 #include "common/color.h"
  36 #include "common/atomic.h"
  37 #include "common/maths.h"
  38 #include "common/printf.h"
  39 #include "common/utils.h"
  40
  41
  42 #include "drivers/nvic.h"
  43
  44 #include "drivers/sensor.h"
  45 #include "drivers/system.h"
  46 #include "drivers/gpio.h"
  47 #include "drivers/light_led.h"
  48 #include "drivers/sound_beeper.h"
  49 #include "drivers/timer.h"
  50 #include "drivers/serial.h"
  51 #include "drivers/serial_softserial.h"
  52 #include "drivers/serial_uart.h"
  53 #include "drivers/accgyro.h"
  54 #include "drivers/compass.h"
  55 #include "drivers/pwm_mapping.h"
  56 #include "drivers/pwm_rx.h"
  57 #include "drivers/adc.h"
  58 #include "drivers/bus_i2c.h"
  59 #include "drivers/bus_spi.h"
  60 #include "drivers/inverter.h"
  61 #include "drivers/flash_m25p16.h"
  62 #include "drivers/sonar_hcsr04.h"
  63 #include "rx/rx.h"
  64 #include "io/serial.h"
  65 #include "io/flashfs.h"
  66 #include "io/gps.h"
  67 #include "io/escservo.h"
  68 #include "io/rc_controls.h"
  69 #include "io/gimbal.h"
  70 #include "io/ledstrip.h"
  71 #include "io/display.h"
  72 #include "io/serial_cli.h"
  73 #include "io/beeper.h"
  74 #include "sensors/sensors.h"
  75 #include "sensors/sonar.h"
  76 #include "sensors/barometer.h"
  77 #include "sensors/compass.h"
  78 #include "sensors/acceleration.h"
  79 #include "sensors/gyro.h"
  80 #include "sensors/battery.h"
  81 #include "sensors/boardalignment.h"
  82 #include "sensors/initialisation.h"
  83
  84 #include "telemetry/telemetry.h"
  85 #include "blackbox/blackbox.h"
  86
  87 #include "flight/pid.h"
  88 #include "flight/imu.h"
  89 #include "flight/mixer.h"
  90 #include "flight/failsafe.h"
  91 #include "flight/navigation.h"
  92
  93 #include "config/runtime_config.h"
  94 #include "config/config.h"
  95 #include "config/config_profile.h"
  96 #include "config/config_master.h"
  97
  98 #include "tracker/defines.h"
  99 #include "tracker/config.h"
 100 #include "tracker/telemetry.h"
 101 #include "tracker/math.h"
 102 #include "tracker/TinyGPS.h"
 103 #include "tracker/servos.h"
 104 #include "tracker/gps_estimation.h"
 105 #include "tracker/interpolation.h"
 106 #include "tracker/protocol_detection.h"
 107
 108 void calcTilt(void);
 109 void getError(void);
 110 void calculatePID();
 111 void blinkLeds(uint8_t numblinks);
 112 int getHeading(void);
 113 //void calibrate_compass(void);
 114 void servo_tilt_update();
 115 void saveLastTilt(bool writteEeprom);
 116 void tracker_setup();
 117 void tracker_loop();
 118 void updateBatteryStatus(void);
 119 void updateServoTest(void);
 120 void updateDigitalButtons(void);
 121 void updateReadTelemetry(void);
 122 void updateTelemetryLost(void);
 123 void updateTargetPosition(void);
 124 void updateHeading(void);
 125 void updateFixedPages(void);
 126 void updateSetHomeByGPS(void);
 127 void updateMFD(void);
 128 void enterMenuMode(void);
 129 void exitMenuMode(void);
 130 void updateSetHomeButton(void);
 131 void updateMenuButton(void);
 132 void updateTracking(void);
 133 void proccessMenu(uint8_t menuButton);
 134 void processMenuRoot(void);
 135 void processMenuCalibrate(void);
 136 void processMenuTelemetry(void);
 137 void processMenuTelemetryProtocol(void);
 138 void processMenuTelemetryBaudrate(void);
 139 void processMenuFeature(uint16_t featureIndex);
 140 void offsetTrimIncrease(void);
 141 void offsetTrimDecrease(void);
 142 float map(long x, long in_min, long in_max, long out_min, long out_max);
 143 void calcEstimatedPosition();
 144 bool couldLolcalGpsSetHome(bool setByUser);
 145 bool couldTelemetrySetHome(void);
 146 void updateCalibratePan();
 147 uint16_t calculateDeltaHeading(uint16_t heading1, uint16_t heading2);
 148 void setEpsMode(void);
 149 int16_t getOffset(int16_t offset_master,int8_t offset_trim);
 150 void updateProtocolDetection(void);
 151 void protocolInit(uint16_t protocol);
 152 void trackingInit(void);
 153 void telemetryPortInit(void);
 154 void setHomeByLocalGps(positionVector_t *tracker, int32_t lat, int32_t lon, int16_t alt, bool home_updated, bool beep);
 155 void calcTelemetryFrequency(void);
 156 uint8_t filterTiltAngle(uint8_t target);
 157 void servosInit(void);
 158
 159 //EASING
 160 int16_t _lastTilt;
 161 int16_t tilt;
 162 bool _servo_tilt_has_arrived = true;
 163 uint8_t _tilt_pos = 0;
 164 float _servo_tilt_must_move = -1;
 165 float easingout = 0.0f;
 166 float tiltTarget;
 167
 168 //TIMER/CALIB
 169 unsigned long time;
 170 unsigned long calib_timer;
 171
 172 //HOME
 173 unsigned long home_timer;
 174 unsigned long home_timer_reset;
 175
 176 //PID stuff
 177 long Error[11];
 178 long Accumulator;
 179 long PID;
 180 long Dk;
 181 uint8_t Divider;
 182
 183 //SERVOS PWM
 184 uint16_t pwmPan;
 185 uint16_t pwmTilt;
 186 uint16_t _lastPwmTilt;
 187
 188 uint16_t minPwmPan;
 189 uint16_t maxPwmPan;
 190 int16_t maxDeltaHeading;
 191
 192 //OFFSET TRIM STATE
 193 typedef enum {
 194         PWMPAN0_UNKNOWN,
 195         FINDING_OUT_MIN_PWMPAN0,
 196         MIN_PWMPAN0_FOUND,
 197         FINDING_OUT_MAX_PWMPAN0,
 198         MAX_PWMPAN0_FOUND,
 199         PWMPAN0_CALCULATED_WITH_SUCCESS
 200 };
 201 uint8_t pwmPanState = PWMPAN0_UNKNOWN;
 202 //TARGET/TRACKER POSITION
 203 // The target position (lat/lon)
 204 positionVector_t targetPosition;
 205 // The tracker position (lat/lon)
 206 positionVector_t trackerPosition;
 207 float currentDistance = 0;
 208 float currentSpeed = 0;
 209 //uint16_t eps_frequency = 0;
 210
 211 //TELEMETRY VARS
 212 uint16_t distance;
 213 bool mfdTestMode;
 214 bool gotAlt;
 215 bool gotFix;
 216 bool settingHome;
 217 bool gotTelemetry=false;
 218 bool lostTelemetry=true;
 219
 220 //TRACKER STATE VARS
 221 bool homeSet;
 222 bool homeReset;
 223 bool trackingStarted;
 224 bool currentState;
 225 bool homeButtonCurrentState;
 226 bool previousState;
 227 bool homeButtonPreviousState;
 228 bool gotNewHeading;
 229 bool homeSet_BY_GPS = false;
 230
 231 //TIMERS
 232 unsigned long servoPanTimer = 0;
 233 unsigned long servoPanVarTime = 0;
 234 unsigned long minServoPanVarTime = 0;
 235 unsigned long servoPanTimerStart = 0;
 236 unsigned long debugTimer = 0;
 237 unsigned long easingTimer = 0;
 238 unsigned long lostTelemetry_timer = 0;
 239 unsigned long epsVectorTimer = 0;
 240 unsigned long currentTimeMillis = 0;
 241
 242 //RSSI
 243 extern uint16_t rssi;
 244
 245 //Display
 246 uint8_t displayPageIndex=0;
 247 extern uint8_t menuState;
 248 extern uint8_t indexMenuOption;
 249 uint8_t menuOption;
 250 bool detection_title_updated = false;
 251
 252 //COMMON VARS
 253 serialPort_t *trackerSerial;
 254
 255 //VBAT
 256 static uint32_t vbatLastServiced = 0;
 257 //static uint32_t ibatLastServiced = 0;
 258 /* VBAT monitoring interval (in microseconds) - 1s*/
 259 #define VBATINTERVAL (6 * 3500)
 260 /* IBat monitoring interval (in microseconds) - 6 default looptimes */
 261 #define IBATINTERVAL (6 * 3500)
 262
 263 //OFFSET TRIM STATE
 264 typedef enum {
 265         TRIM_STATE_DISABLED,
 266         TRIM_STATE_ENABLED,
 267         DISABLING_TRIM_STATE,
 268         TRIM_STATE_DISABLED_BY_USER
 269 };
 270
 271 uint8_t OFFSET_TRIM_STATE = TRIM_STATE_DISABLED;
 272 uint8_t EPS_MODE;
 273 uint16_t EPS_DISTANCE_GAIN;
 274 uint16_t EPS_FREQUENCY;
 275 //DEBUG VARS
 276 uint16_t debugIndex=0;
 277
 278 //LAT,LON esttimation
 279 extern epsVector_t targetLast;
 280 extern epsVector_t targetCurrent;
 281 extern epsVector_t targetEstimated;
 282
 283 int16_t  imuheading=0;
 284
 285
 286 extern uint32_t currentTime;
 287 extern uint32_t previousTime;
 288 extern uint16_t cycleTime;
 289
 290 extern float dT;
 291
 292 #ifdef SOFTSERIAL_LOOPBACK
 293 serialPort_t *loopbackPort;
 294 #endif
 295
 296 uint16_t homeButton=0;
 297 uint8_t homeButtonCount=0;
 298
 299 uint16_t calibrateButton=0;
 300 uint8_t calibrateButtonCount=0;
 301
 302 extern int16_t telemetry_sats;
 303
 304 int _contador=0;
 305
 306 extern const uint32_t baudRates[];
 307 extern uint8_t telemetry_provider;
 308 extern int32_t telemetry_lat;
 309 extern int32_t telemetry_lon;
 310
 311 extern uint8_t telemetry_frequency;
 312 uint8_t telemetry_fixes;
 313 uint32_t telemetry_millis;
 314
 315 void tracker_setup(void)
 316 {
 317
 318   protocolInit(masterConfig.telemetry_protocol);
 319
 320   if(feature(FEATURE_AUTODETECT))
 321           enableProtocolDetection();
 322
 323   trackingInit();
 324
 325   telemetryPortInit();
 326
 327   setPrintfSerialPort(trackerSerial);
 328
 329   LED0_OFF;
 330
 331   if(masterConfig.init_servos==1) {
 332           servosInit();
 333   }
 334
 335   if(feature(FEATURE_EASING)) {
 336           if(masterConfig.init_servos==1)
 337                   _lastTilt = 0;
 338           else {
 339                   _lastTilt = masterConfig.easing_last_tilt;
 340                   _lastPwmTilt = map(_lastTilt, 0, 90, masterConfig.tilt0, masterConfig.tilt90);
 341           }
 342   }
 343
 344
 345
 346   time = millis();
 347
 348   #ifdef TRACKER_DEBUG
 349           serialPrint(trackerSerial,"Setup finished\n");
 350   #endif
 351
 352   LED0_OFF;
 353   LED1_OFF;
 354
 355   blinkLeds(3);
 356
 357   epsVectorsInit(&targetLast,&targetCurrent,&targetEstimated,masterConfig.eps_interpolation,masterConfig.eps_interpolation_points);
 358
 359   targetPosition.home_alt = -32768;
 360  }
 361
 362 void telemetryPortInit(void){
 363         trackerSerial = openSerialPort(masterConfig.serialConfig.portConfigs[0].identifier, FUNCTION_NONE, NULL, baudRates[masterConfig.serialConfig.portConfigs[0].msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
 364 }
 365
 366 void trackingInit(void){
 367         setEpsMode();
 368         updateEPSParams();
 369
 370         OFFSET_TRIM = masterConfig.offset_trim;
 371         OFFSET = getOffset(masterConfig.offset,masterConfig.offset_trim);
 372
 373         gotAlt = false;
 374         gotFix = false;
 375
 376         homeSet = false;
 377         homeReset = false;
 378
 379         trackingStarted = false;
 380
 381         settingHome = false;
 382
 383         previousState = true;
 384         homeButtonPreviousState = true;
 385
 386         currentState = true;
 387         homeButtonCurrentState = true;
 388
 389         gotNewHeading = false;
 390
 391         menuState = 0;
 392
 393         telemetry_frequency = 0;
 394         telemetry_millis = millis();
 395 }
 396
 397 void tracker_loop(void)
 398 {
 399
 400         static uint32_t loopTime;
 401         currentTime = micros();
 402         if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
 403                 loopTime = currentTime + masterConfig.looptime;
 404
 405                 // Measure loop rate just after reading the sensors
 406                 currentTime = micros();
 407                 cycleTime = (int32_t)(currentTime - previousTime);
 408                 previousTime = currentTime;
 409
 410                 dT = (float)cycleTime * 0.000001f;
 411
 412                 updateCompass(&masterConfig.magZero);
 413
 414                 updateCalibratePan();
 415
 416                 handleSerial();
 417
 418                 updateDigitalButtons();
 419
 420                 updateBatteryStatus();
 421
 422                 updateServoTest();
 423
 424                 updateReadTelemetry();
 425
 426                 updateTelemetryLost();
 427
 428                 updateProtocolDetection();
 429
 430                 updateTargetPosition();
 431
 432                 updateHeading();
 433
 434                 updateMenuButton();
 435
 436                 updateSetHomeButton();
 437
 438                 updateSetHomeByGPS();
 439
 440                 updateMFD();
 441
 442                 updateTracking();
 443
 444                 //update RSSI every 50Hz
 445                 if (feature(FEATURE_RSSI_ADC) || (masterConfig.rxConfig.rssi_channel > 0)) {
 446                         updateRSSI(currentTime);
 447                 }
 448         }
 449
 450         beeperUpdate();
 451
 452
 453         //update display
 454     if (feature(FEATURE_DISPLAY)) {
 455         updateDisplay();
 456     }
 457
 458         if(feature(FEATURE_EASING)) {
 459                 if(millis()-easingTimer > masterConfig.easing_millis){
 460                   servo_tilt_update();
 461                   easingTimer = millis();
 462                 }
 463         }
 464
 465 #ifdef GPS
 466         if (feature(FEATURE_GPS)) {
 467             gpsThread();
 468         }
 469 #endif
 470
 471 #ifdef TELEMETRY
 472         if (!cliMode && feature(FEATURE_TELEMETRY)) {
 473                 telemetryProcess(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 474         }
 475 #endif
 476
 477         /*if(feature(FEATURE_DEBUG) && !cliMode) {
 478                 if(millis()-debugTimer>1000){
 479                         printf("c: %d H: %d\r\n",(BUTTON(MENU_BUTTON)>0),(BUTTON(HOME_BUTTON)>0));
 480                         debugTimer=millis();
 481                 }
 482         }*/
 483 }
 484
 485 //Tilt angle tiltTarget = atan(alt/dist)
 486 void calcTilt(void) {
 487
 488
 489   //this will fix an error where the tracker randomly points up when plane is lower than tracker
 490   if (targetPosition.alt < trackerPosition.alt){
 491     targetPosition.alt = trackerPosition.alt;
 492   }
 493
 494   //prevent division by 0
 495   if (targetPosition.distance == 0) {
 496     // in larger altitude 1m distance shouldnt mess up the calculation.
 497     //e.g. in 100m height and dist 1 the angle is 89.4° which is actually accurate enough
 498     targetPosition.distance = 1;
 499   }
 500
 501   tiltTarget = toDeg(atan((float)(targetPosition.alt - trackerPosition.alt) / targetPosition.distance));
 502
 503   if (tiltTarget < 0)
 504     tiltTarget = 0;
 505   else if (tiltTarget > 90)
 506     tiltTarget = 90;
 507
 508   tiltTarget = filterTiltAngle(tiltTarget);
 509
 510   if(feature(FEATURE_EASING)) {
 511         if(_servo_tilt_has_arrived){
 512                 _servo_tilt_must_move = tiltTarget;
 513                 _servo_tilt_has_arrived = false;
 514                 _tilt_pos = 0;
 515         }
 516   }
 517   else {
 518         pwmTilt = (uint16_t) map(tiltTarget,0,90,masterConfig.tilt0, masterConfig.tilt90);
 519     pwmWriteServo(masterConfig.tilt_pin,pwmTilt);
 520   }
 521 }
 522
 523 void getError(void)
 524 {
 525         int16_t delta;
 526
 527         if(feature(FEATURE_NOPID)){
 528                 delta = trackerPosition.heading - targetPosition.heading;
 529         } else {
 530                 delta = targetPosition.heading - trackerPosition.heading;
 531
 532                 for (uint8_t i = 0; i < 10; i++) {
 533                   Error[i + 1] = Error[i];
 534                 }
 535
 536         }
 537
 538
 539         if (delta > 1800)
 540                 delta -= 3600;
 541         else if (delta <= -1800)
 542                 delta += 3600;
 543         Error[0] = delta;
 544 }
 545
 546 void calculatePID(void)
 547 {
 548   int8_t panInverted = (masterConfig.pan_inverted == 0 ? 1 : -1);
 549   if(feature(FEATURE_NOPID)) {
 550         // Calculate pwmPan without usind PID control system
 551         int16_t PAN_SPEED;
 552         if(abs(Error[0]) >= masterConfig.nopid_map_angle * 10)
 553                 PAN_SPEED = panInverted * masterConfig.nopid_max_speed;
 554         else
 555                 PAN_SPEED = panInverted * map(abs(Error[0]), 0, masterConfig.nopid_map_angle * 10, masterConfig.min_pan_speed, masterConfig.nopid_max_speed);
 556
 557         if(abs(Error[0]) <= masterConfig.nopid_min_delta)
 558                 pwmPan = masterConfig.pan0;
 559
 560         if (Error[0] < 0)
 561                 pwmPan = masterConfig.pan0 + PAN_SPEED;
 562         else if(Error[0] > 0)
 563                 pwmPan = masterConfig.pan0 - PAN_SPEED;
 564
 565   } else {
 566         // Calculate pwmPan using PID control system
 567         Divider = masterConfig.max_pid_divider;
 568         PID = Error[0] * (panInverted * masterConfig.p);     // start with proportional gain
 569         Accumulator += Error[0];  // accumulator is sum of errors
 570         if (Accumulator > masterConfig.max_pid_accumulator)
 571                 Accumulator = masterConfig.max_pid_accumulator;
 572         if (Accumulator < -1 * masterConfig.max_pid_accumulator)
 573                 Accumulator = -1 * masterConfig.max_pid_accumulator;
 574         PID += (panInverted * masterConfig.i) * Accumulator; // add integral gain and error accumulation
 575         Dk = (panInverted * masterConfig.d) * (Error[0] - Error[10]);
 576         PID += Dk; // differential gain comes next
 577         PID = PID >> Divider; // scale PID down with divider
 578         // limit the PID to the resolution we have for the PWM variable
 579         if (PID >= masterConfig.max_pid_gain)
 580                 PID = masterConfig.max_pid_gain;
 581
 582         if (PID <= -1*masterConfig.max_pid_gain)
 583                 PID = -1*masterConfig.max_pid_gain;
 584
 585         if (Error[0] > masterConfig.max_pid_error) {
 586                 pwmPan = masterConfig.pan0 + PID + masterConfig.min_pan_speed;
 587         } else if (Error[0] < -1 * masterConfig.max_pid_error) {
 588                 pwmPan = masterConfig.pan0 + PID - masterConfig.min_pan_speed;
 589         } else {
 590                 pwmPan = masterConfig.pan0;
 591         }
 592   }
 593
 594
 595 }
 596
 597 void servo_tilt_update(){
 598   if(_servo_tilt_must_move < 0) _tilt_pos = 0;
 599   if(!_servo_tilt_has_arrived && _servo_tilt_must_move > -1){
 600         if(abs(_lastTilt-_servo_tilt_must_move) > masterConfig.easing_min_angle) {
 601           if(_tilt_pos < masterConfig.easing_steps){
 602                 if(_lastTilt <= _servo_tilt_must_move)
 603                   easingout = _lastTilt + easeTilt(_tilt_pos, 0, _servo_tilt_must_move - _lastTilt, masterConfig.easing_steps);
 604                 else
 605                         easingout = _lastTilt - easeTilt(_tilt_pos, 0, _lastTilt - _servo_tilt_must_move, masterConfig.easing_steps);
 606                         pwmTilt=(int16_t)map(easingout,0,90,masterConfig.tilt0,masterConfig.tilt90);
 607                         if(_lastPwmTilt != pwmTilt)
 608                                 pwmWriteServo(masterConfig.tilt_pin,pwmTilt);
 609                         _tilt_pos++;
 610           }
 611           else {
 612                 if(_tilt_pos == masterConfig.easing_steps){
 613                   pwmTilt = (uint16_t) map(_servo_tilt_must_move,0,90, masterConfig.tilt0, masterConfig.tilt90);
 614                   pwmWriteServo(masterConfig.tilt_pin,pwmTilt);
 615                   _lastTilt = _servo_tilt_must_move;
 616                   _tilt_pos=0;
 617                   _servo_tilt_has_arrived = true;
 618                   _servo_tilt_must_move = -1;
 619                 }
 620           }
 621           _lastPwmTilt = pwmTilt;
 622         }
 623         else {
 624                 _servo_tilt_has_arrived = true;
 625                 _servo_tilt_must_move = -1;
 626         }
 627   }
 628 }
 629
 630 void serialPrintln(void){
 631   serialPrint(trackerSerial, "\n");
 632 }
 633
 634 void blinkLeds(uint8_t numblinks){
 635
 636         for(int i=0;i<numblinks;i++) {
 637                   LED0_ON;
 638                   LED1_ON;
 639                   delay(250);
 640                   LED0_OFF;
 641                   LED1_OFF;
 642                   delay(250);
 643           }
 644 }
 645
 646
 647 int getHeading(void) {
 648         static t_fp_vector imuVector;
 649         imuVector.A[0]=magADC[0];
 650         imuVector.A[1]=magADC[1];
 651         imuVector.A[2]=magADC[2];
 652
 653         OFFSET_TRIM = masterConfig.offset_trim;
 654         OFFSET = getOffset(masterConfig.offset,masterConfig.offset_trim);
 655
 656         int imuheading = imuCalculateHeading(&imuVector);
 657
 658         return imuheading;
 659 }
 660
 661 int16_t getOffset(int16_t offset_master,int8_t offset_trim){
 662         return offset_master - (int16_t)offset_trim;
 663 }
 664
 665 void updateDigitalButtons(void) {
 666         //Home Button
 667         homeButton=GPIO_ReadInputDataBit(GPIOB, Pin_9);
 668         if(homeButton==0) {
 669                 homeButtonCount++;
 670         } else if (homeButton==1) {
 671                 homeButtonCount=0;
 672                 DISABLE_BUTTON(HOME_BUTTON);
 673         }
 674         if(homeButtonCount > masterConfig.min_logic_level) {
 675                 ENABLE_BUTTON(HOME_BUTTON);
 676         }
 677         //Calibrate Button
 678         calibrateButton=GPIO_ReadInputDataBit(GPIOB, Pin_8);
 679         if(calibrateButton==0) {
 680                 calibrateButtonCount++;
 681         } else if (calibrateButton==1) {
 682                 calibrateButtonCount=0;
 683                 DISABLE_BUTTON(MENU_BUTTON);
 684         }
 685         if(calibrateButtonCount > masterConfig.min_logic_level) ENABLE_BUTTON(MENU_BUTTON);
 686
 687 }
 688
 689 void setHomeByTelemetry(positionVector_t *tracker, positionVector_t *target) {
 690   tracker->lat = target->lat;
 691   tracker->lon = target->lon;
 692   tracker->alt = target->alt;
 693
 694   setTelemetryHome(target->lat,target->lon,target->alt);
 695
 696   if(feature(FEATURE_DEBUG)){
 697           tracker->lat = 47403583; tracker->lon = 8535850; tracker->alt = 474;
 698   }
 699
 700   tracker->alt = 0;
 701   tracker->home_alt = target->alt;
 702
 703   homeSet = true;
 704   homeSet_BY_GPS = false;
 705   homeReset = false;
 706   home_timer_reset = 0;
 707   epsVectorLoad(&targetLast,target->lat,target->lon,0,0,0);
 708   epsVectorLoad(&targetCurrent,target->lat,target->lon,0,0,millis());
 709 }
 710
 711 void setHomeByLocalGps(positionVector_t *tracker, int32_t lat, int32_t lon, int16_t alt, bool home_updated, bool beep) {
 712   tracker->lat = lat;
 713   tracker->lon = lon;
 714   tracker->alt = alt;
 715
 716   setTelemetryHome(lat,lon,alt);
 717
 718   if(feature(FEATURE_DEBUG)) {
 719           tracker->lat = 47403583; tracker->lon = 8535850; tracker->alt = 474;
 720   }
 721
 722   if(home_updated) {
 723           tracker->alt = alt - tracker->home_alt;
 724   } else {
 725           tracker->alt = 0;
 726           tracker->home_alt = alt;
 727   }
 728
 729   if(targetPosition.home_alt == -32768)
 730           targetPosition.alt = 0;
 731
 732   homeSet = true;
 733   homeSet_BY_GPS = true;
 734   homeReset = false;
 735   home_timer_reset = 0;
 736   if(!home_updated) {
 737           epsVectorLoad(&targetLast,lat,lon,0,0,0);
 738           epsVectorLoad(&targetCurrent,lat,lon,0,0,millis());
 739   }
 740 }
 741
 742 void updateBatteryStatus(void){
 743     if (feature(FEATURE_VBAT)) {
 744         if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
 745             vbatLastServiced = currentTime;
 746             updateBattery();
 747         }
 748     }
 749 }
 750
 751 void updateFixedPages(void){
 752
 753         displayPageIndex =
 754                         PAGE_GPS * (displayPageIndex == PAGE_TELEMETRY && feature(FEATURE_GPS) && !PROTOCOL(TP_MFD)) + \
 755                         PAGE_BATTERY * ((displayPageIndex == PAGE_TELEMETRY && !feature(FEATURE_GPS) && (feature(FEATURE_VBAT) || feature(FEATURE_RSSI_ADC) || (masterConfig.rxConfig.rssi_channel > 0))) || (displayPageIndex == PAGE_GPS && (feature(FEATURE_VBAT) || feature(FEATURE_RSSI_ADC) || (masterConfig.rxConfig.rssi_channel > 0)))) + \
 756                         PAGE_TELEMETRY * (displayPageIndex == 0);
 757         if(displayPageIndex !=0 ){
 758                 //Show fixed page
 759                 displayShowFixedPage(displayPageIndex);
 760         } else  {
 761                 //Activate cycling
 762                 displayResetPageCycling();
 763                 displayEnablePageCycling();
 764         }
 765 }
 766
 767 void updateServoTest(void){
 768         if(PROTOCOL(TP_SERVOTEST)){
 769                  if(SERVO(SERVOPAN_MOVE)) {
 770                          targetPosition.heading = SERVOTEST_HEADING * 10;
 771                          DISABLE_SERVO(SERVOPAN_MOVE);
 772                  }
 773
 774                  if(SERVO(SERVOTILT_MOVE)) {
 775                         SERVOTEST_TILT = filterTiltAngle(SERVOTEST_TILT);
 776                         if(feature(FEATURE_EASING) && masterConfig.easing > 0) {//#ifdef TILT_EASING
 777                                   _servo_tilt_must_move = SERVOTEST_TILT;
 778                                   _servo_tilt_has_arrived = false;
 779                         }
 780                         else
 781                         {
 782                                 tilt = map(SERVOTEST_TILT, 0, 90, masterConfig.tilt0, masterConfig.tilt90);
 783                                 pwmWriteServo(masterConfig.tilt_pin,tilt);
 784                         }
 785                         DISABLE_SERVO(SERVOTILT_MOVE);
 786                 }
 787         }
 788 }
 789
 790 void updateReadTelemetry(void){
 791         if (serialRxBytesWaiting(trackerSerial)>1){
 792                 uint8_t c = serialRead(trackerSerial);
 793
 794                 evaluateOtherData(trackerSerial,c);
 795
 796                 LED0_ON;
 797
 798                 if(!PROTOCOL(TP_SERVOTEST) && !cliMode)
 799                         encodeTargetData(c);
 800
 801                 if(!PROTOCOL(TP_SERVOTEST)) {
 802                         gotTelemetry = true;
 803                         lostTelemetry = false;
 804                         lostTelemetry_timer = 0;
 805                 }
 806
 807
 808         } else {
 809                 LED0_OFF;
 810                 gotTelemetry = false;
 811         }
 812 }
 813
 814 void updateTelemetryLost(void){
 815         if(lostTelemetry) {
 816                 gotFix = false;
 817                 gotAlt = false;
 818                 telemetry_frequency = 0;
 819                 telemetry_fixes = 0;
 820                 return;
 821         }
 822
 823         if (!gotTelemetry && lostTelemetry_timer == 0){
 824                 lostTelemetry_timer = millis();
 825                 return;
 826         }
 827
 828         if(!gotTelemetry && (millis() - lostTelemetry_timer > 3000)){
 829                 lostTelemetry = true;
 830                 if(feature(FEATURE_AUTODETECT)){
 831                         showAutodetectingTitle(0);
 832                         enableProtocolDetection();
 833                 }
 834         }
 835 }
 836
 837 void updateTargetPosition(void){
 838         if(!PROTOCOL(TP_SERVOTEST)){
 839                 if (gotAlt) {
 840
 841                         if(telemetry_sats >= masterConfig.telemetry_min_sats && targetPosition.home_alt == -32768)
 842                                 targetPosition.home_alt = getTargetAlt(0);
 843
 844                         targetPosition.alt = getTargetAlt(targetPosition.home_alt);
 845
 846                         if(PROTOCOL(TP_MFD)){
 847                                 distance = getDistance();
 848                                 targetPosition.heading = getAzimuth() * 10;
 849                         }
 850                         else{
 851                                   //Do nothing
 852                         }
 853
 854                         gotAlt = false;
 855                 }
 856
 857                 if(!PROTOCOL(TP_MFD)){
 858                         if (gotFix) {
 859
 860                                 calcTelemetryFrequency();
 861
 862                                 targetPosition.lat = getTargetLat();
 863                                 targetPosition.lon = getTargetLon();
 864                                 currentDistance = distance_between(targetLast.lat / TELEMETRY_LATLON_DIVIDER_F,targetLast.lon / TELEMETRY_LATLON_DIVIDER_F,targetPosition.lat / TELEMETRY_LATLON_DIVIDER_F,targetPosition.lon / TELEMETRY_LATLON_DIVIDER_F);
 865                                 currentTimeMillis = millis();
 866                                 currentSpeed = epsVectorSpeed(targetLast.time,currentTimeMillis,currentDistance);
 867                                 if(masterConfig.max_speed_filter == 0 || targetCurrent.speed < masterConfig.max_speed_filter){
 868                                         epsVectorLoad(&targetCurrent,targetPosition.lat,targetPosition.lon,currentDistance,targetLast.time,currentTimeMillis);//epsVectorSpeed(targetLast.time,currentTimeMillis,currentDistance);
 869                                         if(homeSet) {
 870                                                 if(feature(FEATURE_EPS)) {
 871                                                         targetCurrent.heading = course_to(targetLast.lat / TELEMETRY_LATLON_DIVIDER_F,targetLast.lon / TELEMETRY_LATLON_DIVIDER_F,targetCurrent.lat / TELEMETRY_LATLON_DIVIDER_F,targetCurrent.lon / TELEMETRY_LATLON_DIVIDER_F);
 872                                                         if(masterConfig.eps > 1){
 873                                                                 if(!pvFull())
 874                                                                         pvPut(&targetCurrent,1);
 875
 876                                                                 if(masterConfig.eps_interpolation) {
 877                                                                         epsVectorAddPoint(&targetLast,&targetCurrent);
 878                                                                 }
 879                                                         }
 880                                                         if(masterConfig.eps == 1 || masterConfig.eps == 3) {
 881                                                                 calcEstimatedPosition();
 882                                                                 epsVectorTimer = millis();
 883                                                         }
 884                                                 } else {
 885                                                         targetPosition.distance = distance_between(trackerPosition.lat / TELEMETRY_LATLON_DIVIDER_F, trackerPosition.lon / TELEMETRY_LATLON_DIVIDER_F, targetPosition.lat / TELEMETRY_LATLON_DIVIDER_F, targetPosition.lon / TELEMETRY_LATLON_DIVIDER_F);
 886                                                         targetPosition.heading = course_to(trackerPosition.lat / TELEMETRY_LATLON_DIVIDER_F, trackerPosition.lon / TELEMETRY_LATLON_DIVIDER_F, targetPosition.lat / TELEMETRY_LATLON_DIVIDER_F, targetPosition.lon / TELEMETRY_LATLON_DIVIDER_F) * 10.0f;
 887                                                 }
 888                                         }
 889                                         epsVectorCurrentToLast(&targetCurrent,&targetLast);
 890                                 }
 891                                 gotFix = false;
 892                         }
 893                         if(feature(FEATURE_EPS) && masterConfig.eps > 1)
 894                                 calcEstimatedPosition();
 895                 }
 896         }
 897 }
 898
 899 void calcEstimatedPosition(){
 900         if(homeSet && lostTelemetry == false) {
 901                 if(masterConfig.eps == 1 || (millis()- epsVectorTimer > masterConfig.eps_frequency && masterConfig.eps > 1)){
 902                         epsVectorEstimate(&targetLast,&targetCurrent,&targetEstimated,masterConfig.eps_gain,masterConfig.eps_frequency,masterConfig.eps);
 903                         targetPosition.distance = distance_between(trackerPosition.lat / TELEMETRY_LATLON_DIVIDER_F, trackerPosition.lon / TELEMETRY_LATLON_DIVIDER_F, targetPosition.lat / TELEMETRY_LATLON_DIVIDER_F, targetPosition.lon / TELEMETRY_LATLON_DIVIDER_F);
 904                         targetPosition.heading = course_to(trackerPosition.lat / TELEMETRY_LATLON_DIVIDER_F, trackerPosition.lon / TELEMETRY_LATLON_DIVIDER_F, targetEstimated.lat / TELEMETRY_LATLON_DIVIDER_F, targetEstimated.lon / TELEMETRY_LATLON_DIVIDER_F) * 10.0f;
 905                         epsVectorTimer = millis();
 906                 }
 907         }
 908 }
 909
 910 void updateHeading(void){
 911         // we update the heading every 14ms to get as many samples into the smooth array as possible
 912         if (millis() > time) {
 913                 time = millis() + 14;
 914                 trackerPosition.heading = getHeading();
 915                 gotNewHeading = true;
 916         }
 917 }
 918
 919 void offsetTrimIncrease(void){
 920         masterConfig.offset_trim++;
 921         if(masterConfig.offset_trim > 20)
 922                 masterConfig.offset_trim = 20;
 923         else
 924                 writeEEPROM();
 925
 926 }
 927 void offsetTrimDecrease(void){
 928         masterConfig.offset_trim--;
 929         if(masterConfig.offset_trim < -20)
 930                 masterConfig.offset_trim = -20;
 931         else
 932                 writeEEPROM();
 933 }
 934
 935 void updateMenuButton(void){
 936         currentState = !BUTTON(MENU_BUTTON);
 937         if (currentState != previousState) {
 938                 LED1_TOGGLE;
 939                 // When trim offset mode is disabled this button acts as menu browser
 940                 if(OFFSET_TRIM_STATE == TRIM_STATE_DISABLED || OFFSET_TRIM_STATE == TRIM_STATE_DISABLED_BY_USER) {
 941                         /*
 942                          *      SI EL OFFSET EST� MODIFICADO, RESETEARLO
 943                          */
 944                         if(menuState && !cliMode) {
 945                                 //MENU MODE
 946                                 if (!currentState && calib_timer == 0) {
 947                                         calib_timer = millis();
 948                                 } else if (currentState && millis() - calib_timer < 1000) {
 949                                         proccessMenu(MENU_BUTTON);
 950                                         calib_timer = 0;
 951                                 } else if (currentState && millis() - calib_timer < 1500) {
 952                                         //button not pressed long enough
 953                                         calib_timer = 0;
 954                                 } else if (currentState && millis() - calib_timer > 1500) {
 955                                         calib_timer = 0;
 956                                         exitMenuMode();
 957                                 }
 958                         } else if(!menuState){
 959                                 //TELEMETRY MODE
 960                                 if (!currentState && calib_timer == 0) {
 961                                         calib_timer = millis();
 962                                 } else if (currentState && millis() - calib_timer < 1000) {
 963                                         updateFixedPages();
 964                                         calib_timer = 0;
 965                                 } else if (currentState && millis() - calib_timer < 1500) {
 966                                         //button not pressed long enough
 967                                         calib_timer = 0;
 968                                 } else if (currentState && millis() - calib_timer > 1500) {
 969                                         //start calibration routine if button pressed > 4s and released
 970                                         //ENABLE_STATE(CALIBRATE_MAG);
 971                                         calib_timer = 0;
 972                                         home_timer = 0;
 973                                         enterMenuMode();
 974                                 }
 975                         }
 976                 // When trim offset is enabled, this button decrease the offset one unit
 977                 } else if(OFFSET_TRIM_STATE == TRIM_STATE_ENABLED){
 978                         if (!currentState && calib_timer == 0) {
 979                                 calib_timer = millis();
 980                         } else if (currentState && millis() - calib_timer < 1000) {
 981                                 offsetTrimDecrease();
 982                                 calib_timer = 0;
 983                         } else if (currentState && millis() - calib_timer > 2000) {
 984                                 calib_timer = 0;
 985                                 //FIX ME: RESET OFFSET TRIM
 986                         }
 987                 }
 988
 989                 previousState = currentState;
 990         }
 991 }
 992
 993
 994 void updateSetHomeButton(void){
 995         //If disabling offset trim, wait untill button is released
 996         if(OFFSET_TRIM_STATE == DISABLING_TRIM_STATE) {
 997                 if(!BUTTON(HOME_BUTTON)) {
 998                         OFFSET_TRIM_STATE = TRIM_STATE_DISABLED_BY_USER;
 999                         home_timer = 0;
1000                         home_timer_reset = 0;
1001                 }
1002                 return;
1003         }
1004
1005         homeButtonCurrentState = !BUTTON(HOME_BUTTON);
1006         if (homeButtonCurrentState != homeButtonPreviousState) {
1007                 LED1_TOGGLE;
1008                 // Modo NO trimado
1009                 if(OFFSET_TRIM_STATE == TRIM_STATE_DISABLED || OFFSET_TRIM_STATE == TRIM_STATE_DISABLED_BY_USER) {
1010                         // Modo MENU
1011                         if(menuState) {
1012                                 if (!homeButtonCurrentState && home_timer == 0) {
1013                                         home_timer = millis();
1014                                 } else if (homeButtonCurrentState && (millis() - home_timer < 1000)) {
1015                                         proccessMenu(HOME_BUTTON);
1016                                         home_timer = 0;
1017                                 }
1018                         // Modo HOME
1019                         } else {
1020                                 if (!homeButtonCurrentState && home_timer == 0) {
1021                                         home_timer = millis();
1022                                 // SET HOME
1023                                 } else if (homeButtonCurrentState && (millis() - home_timer < 1000) && !PROTOCOL(TP_MFD)) {
1024                                         home_timer = 0;
1025                                         // By telemetry if has enought sats
1026                                         if(!homeSet && telemetry_sats >= masterConfig.telemetry_min_sats)
1027                                                 setHomeByTelemetry(&trackerPosition, &targetPosition);
1028                                         // By local GPS because telemetry hasn't got enought sats and we don't want wait more time.
1029                                         else if(!homeSet && couldLolcalGpsSetHome(true)){
1030                                                 setHomeByLocalGps(&trackerPosition,GPS_coord[LAT]/10,GPS_coord[LON]/10,GPS_altitude,false,true);
1031                                         }
1032                                 // RESET HOME
1033                                 } else if (homeButtonCurrentState && (millis() - home_timer > 2000) && !PROTOCOL(TP_MFD)) {
1034                                         if(homeSet && !homeReset && !trackingStarted){
1035                                                 homeSet = false;
1036                                                 homeReset = true;
1037                                                 home_timer = 0;
1038                                                 home_timer_reset = 0;
1039                                         }
1040                                 }
1041                         }
1042                 // Modo TRIMADO OFFSET
1043                 } else if(OFFSET_TRIM_STATE == TRIM_STATE_ENABLED){
1044                         if (!homeButtonCurrentState && home_timer == 0) {
1045                                 home_timer = millis();
1046                         // INCREMENTO
1047                         } else if (homeButtonCurrentState && millis() - home_timer < 1000) {
1048                                 offsetTrimIncrease();
1049                                 home_timer = 0;
1050                         // DECREMENTO
1051                         } else if (homeButtonCurrentState && millis() - home_timer > 2000) {
1052                                 home_timer = 0;
1053                                 OFFSET_TRIM_STATE = DISABLING_TRIM_STATE;
1054                         }
1055                 }
1056
1057                 homeButtonPreviousState = homeButtonCurrentState;
1058         }
1059
1060 }
1061
1062 void updateSetHomeByGPS(void){
1063         if(PROTOCOL(TP_SERVOTEST) || PROTOCOL(TP_MFD) || PROTOCOL(TP_CALIBRATING_MAG) || PROTOCOL(TP_CALIBRATING_PAN0) || PROTOCOL(TP_CALIBRATING_MAXPAN))
1064                 return;
1065         if(homeReset && lostTelemetry){
1066                         telemetry_lat = 0;
1067                         telemetry_lon = 0;
1068                         telemetry_sats = 0;
1069         }
1070         if(homeReset && home_timer_reset == 0 ) {
1071                 home_timer_reset = millis();
1072         } else if(homeReset && millis()- home_timer_reset < 5000 ) {
1073                 return;         // We have 5 seconds to set home manually by telemetry
1074         } else if(homeReset && millis()- home_timer_reset >= 5000 && couldLolcalGpsSetHome(false)){
1075                 homeReset = false;
1076                 home_timer_reset = 0;
1077                 //if((!homeSet || (homeSet && homeSet_BY_GPS)) && feature(FEATURE_GPS) && STATE(GPS_FIX) && (GPS_numSat >= masterConfig.home_min_sats)) // || GPS_numSat>3))
1078                 setHomeByLocalGps(&trackerPosition,GPS_coord[LAT]/10,GPS_coord[LON]/10,GPS_altitude,false,true);
1079                 if(masterConfig.gpsConfig.homeBeeper)
1080                           beeper(BEEPER_ARMING_GPS_FIX);
1081         } else if(!homeSet && couldLolcalGpsSetHome(false)) {
1082                 homeReset = true;
1083                 home_timer_reset = 0;
1084         } else if(masterConfig.update_home_by_local_gps == 1 && homeSet && couldLolcalGpsSetHome(false)){
1085                 setHomeByLocalGps(&trackerPosition,GPS_coord[LAT]/10,GPS_coord[LON]/10,GPS_altitude,true,false);
1086         } else if(!homeSet && couldTelemetrySetHome()){
1087                 setHomeByTelemetry(&trackerPosition, &targetPosition);
1088                 if(masterConfig.gpsConfig.homeBeeper)
1089                                           beeper(BEEPER_ARMING_GPS_FIX);
1090         }
1091 }
1092
1093 bool couldLolcalGpsSetHome(bool setByUser){
1094         if(feature(FEATURE_DEBUG)){
1095                   return true;
1096         }
1097         return ((setByUser && GPS_numSat >= 4) || (!setByUser && GPS_numSat >= masterConfig.gps_min_sats)) && (feature(FEATURE_GPS) && STATE(GPS_FIX));
1098 }
1099
1100 bool couldTelemetrySetHome(void){
1101         return (!feature(FEATURE_GPS) && masterConfig.telemetry_home == 1 && telemetry_sats >= masterConfig.telemetry_min_sats);
1102 }
1103
1104 void updateMFD(void){
1105         if(PROTOCOL(TP_MFD)){
1106
1107                 homeSet = true;
1108                 settingHome = 0;
1109
1110                 if (mfdTestMode || (homeSet && gotFix)) {
1111                         targetPosition.distance = getDistance();
1112                         targetPosition.alt = getTargetAlt(0);
1113                         targetPosition.heading = getAzimuth() * 10;
1114                         gotFix = false;
1115                 }
1116
1117                 if ((mfdTestMode || homeSet) && gotNewHeading) {
1118                         getError();
1119                         calculatePID();
1120                         pwmWriteServo(masterConfig.pan_pin,pwmPan);
1121                         calcTilt();
1122                         gotNewHeading = false;
1123                 }
1124
1125                 if(homeSet && lostTelemetry == true && !cliMode){
1126                         pwmWriteServo(masterConfig.pan_pin,masterConfig.pan0);
1127                         return;
1128                 }
1129         }
1130 }
1131
1132 void updateTracking(void){
1133         if(!PROTOCOL(TP_MFD) && !PROTOCOL(TP_CALIBRATING_MAG) && !PROTOCOL(TP_CALIBRATING_PAN0) && !PROTOCOL(TP_CALIBRATING_MAXPAN) && masterConfig.pan0_calibrated==1) {
1134                 if(PROTOCOL(TP_SERVOTEST)) {
1135                         homeSet = true;
1136                         trackingStarted = true;
1137                 } else {
1138                         trackingStarted = (homeSet && ((targetPosition.distance >= masterConfig.start_tracking_distance) || \
1139                                         (targetPosition.distance < masterConfig.start_tracking_distance && (targetPosition.alt - trackerPosition.alt) >= masterConfig.start_tracking_altitude) && targetPosition.home_alt != -32768));
1140                 }
1141
1142                 if(trackingStarted)  {
1143                         if(lostTelemetry == true && !cliMode){
1144                                 pwmWriteServo(masterConfig.pan_pin,masterConfig.pan0);
1145                                 return;
1146                         }
1147                         if(!PROTOCOL(TP_SERVOTEST))
1148                                 calcTilt();
1149
1150                         if (gotNewHeading) {
1151                                   getError();
1152                                   calculatePID();
1153                                   pwmWriteServo(masterConfig.pan_pin,pwmPan);
1154                                   gotNewHeading = false;
1155                         }
1156
1157                         if(!(OFFSET_TRIM_STATE == TRIM_STATE_DISABLED_BY_USER))
1158                                 OFFSET_TRIM_STATE = TRIM_STATE_ENABLED;
1159
1160                 } else {
1161                         OFFSET_TRIM_STATE = TRIM_STATE_DISABLED;
1162                         if(homeSet && !cliMode )
1163                                 pwmWriteServo(masterConfig.pan_pin,masterConfig.pan0);
1164                 }
1165         }
1166 }
1167
1168 void enterMenuMode(void){
1169         saveLastTilt(false);
1170         menuState = MENU_ROOT;
1171         indexMenuOption=0;
1172         displayShowFixedPage(PAGE_MENU);
1173 }
1174 void exitMenuMode(void) {
1175         menuState = MENU_IDEL;
1176         displayResetPageCycling();
1177         displayEnablePageCycling();
1178 }
1179
1180 void processMenuRoot(void){
1181         menuOption = indexMenuOption % (OP_EXIT+1);
1182         if(menuOption == OP_EXIT) {
1183                 writeEEPROM();
1184                 systemReset();
1185         } else if(menuOption == OP_VBAT)
1186                 menuState = MENU_BATTERY;
1187         else if(menuOption == OP_CALIBRATE)
1188                 processMenuCalibrate();
1189         else if(menuOption == OP_GPS)
1190                 menuState = MENU_GPS;
1191         else if(menuOption == OP_EPS)
1192                 menuState = MENU_EPS;
1193         else if(menuOption == OP_EASING)
1194                 menuState = MENU_EASING;
1195         else if(menuOption == OP_TELEMETRY)
1196                 menuState = MENU_TELEMETRY;
1197         else
1198                 menuState = MENU_ROOT;
1199 }
1200
1201 void processMenuBattery(void){
1202         processMenuFeature(FEATURE_VBAT);
1203 }
1204
1205 void processMenuGPS(void){
1206         processMenuFeature(FEATURE_GPS);
1207 }
1208
1209 void processMenuEPS(void){
1210         /*processMenuFeature(FEATURE_EPS);*/
1211                 menuOption = indexMenuOption % (OP_EPS_EXIT+1);
1212         if(menuOption == OP_EPS_EXIT){
1213                 menuState = MENU_ROOT;
1214                 indexMenuOption = OP_EXIT;
1215                 return;
1216         }
1217         if(menuOption == OP_EPS_SAVE) {
1218                 writeEEPROM();
1219                 systemReset();
1220         } else if(menuOption == OP_MODE)
1221                 menuState = MENU_EPS_MODE;
1222         else if(menuOption == OP_DISTANCEGAIN)
1223                 menuState = MENU_EPS_DISTANCEGAIN;
1224         else if(menuOption == OP_FREQUENCY)
1225                 menuState = MENU_EPS_FREQUENCY;
1226         else
1227                 menuState = MENU_EPS;
1228 }
1229
1230 void processMenuEPSMode(void){
1231         menuOption = indexMenuOption % (OP_EPS_MODE_EXIT+1);
1232         if(menuOption == OP_EPS_MODE_EXIT)
1233                 menuState = MENU_EPS;
1234         else if(menuOption >= OP_DISABLED && menuOption <= OP_MODE3){
1235                 masterConfig.eps = menuOption;
1236                 setEpsMode();
1237                 if(masterConfig.eps == OP_DISABLED)
1238                         featureClear(FEATURE_EPS);
1239                 else
1240                         featureSet(FEATURE_EPS);
1241                 menuState = MENU_EPS;
1242         } else
1243                 menuState = MENU_EPS;
1244         indexMenuOption = OP_EPS_SAVE;
1245 }
1246
1247 void processMenuEPSIncreasDecreaseParamValue(uint16_t *param){
1248         menuOption = indexMenuOption % (OP_INCREASEDECREASE_EXIT+1);
1249         if(menuOption == OP_INCREASEDECREASE_EXIT)
1250                 menuState = MENU_EPS;
1251         else {
1252                 switch(menuOption){
1253                 case OP_INCREASE:
1254                         *param +=10;
1255                         break;
1256                 case OP_DECREASE:
1257                         *param -=10;
1258                         break;
1259                 }
1260                 if(*param < 1)
1261                         *param = 1;
1262                 if(*param > 1000)
1263                         *param = 1000;
1264                 updateEPSParams();
1265                 //menuState = MENU_EPS;
1266         }
1267         indexMenuOption = menuOption; //OP_EPS_SAVE;
1268 }
1269
1270 void processMenuEASING(void){
1271         processMenuFeature(FEATURE_EASING);
1272 }
1273
1274 void processMenuFeature(uint16_t featureIndex){
1275         menuOption = indexMenuOption % (OP_ENABLEDISABLE_EXIT+1);
1276         if(menuOption == OP_ENABLE)
1277                 featureSet(featureIndex);
1278         else if(menuOption == OP_DISABLE)
1279                 featureClear(featureIndex);
1280         menuState=MENU_ROOT;
1281         indexMenuOption = OP_EXIT;
1282 }
1283
1284 void processMenuCalibrate(void){
1285         exitMenuMode();
1286         pwmPan0 = masterConfig.pan0;
1287         pwmPanCalibrationPulse = masterConfig.pan_calibration_pulse;
1288         ENABLE_STATE(CALIBRATE_MAG);
1289 }
1290
1291 void processMenuTelemetry(void){
1292         menuOption = indexMenuOption % (OP_TELMETRY_EXIT+1);
1293         if(menuOption == OP_TELMETRY_EXIT){
1294                 menuState = MENU_ROOT;
1295                 indexMenuOption = OP_EXIT;
1296                 return;
1297         }
1298         if(menuOption == OP_TELMETRY_SAVE) {
1299                 writeEEPROM();
1300                 systemReset();
1301         } else if(menuOption == OP_PROTOCOL)
1302                 menuState = MENU_TELEMETRY_PROTOCOL;
1303         else if(menuOption == OP_BAUDRATE)
1304                 menuState = MENU_TELEMETRY_BAUDRATE;
1305         else
1306                 menuState = MENU_TELEMETRY;
1307 }
1308
1309 void processMenuTelemetryProtocol(void){
1310         menuOption = indexMenuOption % (OP_TELEMETRY_PROTOCOL_EXIT+1);
1311         if(menuOption == OP_TELEMETRY_PROTOCOL_EXIT)
1312                 menuState = MENU_TELEMETRY;
1313         else if(menuOption == OP_AUTODETECT){
1314                 featureSet(FEATURE_AUTODETECT);
1315                 menuState = MENU_TELEMETRY;
1316         } else {
1317                 featureClear(FEATURE_AUTODETECT);
1318                 masterConfig.telemetry_protocol = (1 << ( 2 + menuOption));
1319                 menuState = MENU_TELEMETRY;
1320         }
1321         indexMenuOption = OP_TELMETRY_SAVE;
1322 }
1323
1324 void processMenuTelemetryBaudrate(void){
1325         menuOption = indexMenuOption % (OP_TELEMETRY_BAUDRATE_EXIT+1);
1326         if(menuOption == OP_TELEMETRY_BAUDRATE_EXIT)
1327                 menuState = MENU_TELEMETRY;
1328         else {
1329                 masterConfig.serialConfig.portConfigs[0].msp_baudrateIndex = menuOption;//(1 << 2+menuOption);
1330                 menuState = MENU_TELEMETRY;
1331         }
1332         indexMenuOption = OP_TELMETRY_SAVE;
1333 }
1334
1335 void proccessMenu(uint8_t menuButton) {
1336
1337         if(menuButton == MENU_BUTTON) {
1338                 indexMenuOption++;
1339                 showMainMenuPage();
1340         } else if(menuButton == HOME_BUTTON) {
1341                 // root
1342                 if(menuState == MENU_ROOT) {
1343                         processMenuRoot();
1344                 } else if(menuState == MENU_BATTERY) {
1345                         processMenuBattery();
1346                 } else if(menuState == MENU_GPS) {
1347                         processMenuGPS();
1348                 } else if(menuState == MENU_EPS) {
1349                         processMenuEPS();
1350                 } else if(menuState == MENU_EPS_MODE) {
1351                         processMenuEPSMode();
1352                 } else if(menuState == MENU_EPS_DISTANCEGAIN) {
1353                         processMenuEPSIncreasDecreaseParamValue(&masterConfig.eps_gain.distance);
1354                 } else if(menuState == MENU_EPS_FREQUENCY) {
1355                         processMenuEPSIncreasDecreaseParamValue(&masterConfig.eps_frequency);
1356                 }else if(menuState == MENU_EASING) {
1357                         processMenuEASING();
1358                 // Telemetry
1359                 } else if(menuState == MENU_TELEMETRY) {
1360                         processMenuTelemetry();
1361                 // Telemetry Protocol
1362                 } else if(menuState == MENU_TELEMETRY_PROTOCOL) {
1363                         processMenuTelemetryProtocol();
1364                 // Telemetry Baudrate
1365                 } else if (menuState == MENU_TELEMETRY_BAUDRATE) {
1366                         processMenuTelemetryBaudrate();
1367                 }
1368                 if(menuState != MENU_EPS_DISTANCEGAIN && menuState != MENU_EPS_FREQUENCY)
1369                         indexMenuOption=0;
1370                 displayShowFixedPage(PAGE_MENU);
1371         }
1372
1373 }
1374
1375 float map(long x, long in_min, long in_max, long out_min, long out_max)
1376 {
1377   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
1378 }
1379
1380 void saveLastTilt(bool writteEeprom){
1381         if(feature(FEATURE_EASING)) {
1382                 masterConfig.easing_last_tilt = _lastTilt;
1383                 if(writteEeprom) writeEEPROM();
1384         }
1385 }
1386
1387 void updateCalibratePan()
1388 {
1389         uint16_t deltaHeading;
1390         float slope;
1391         // ENABLE CALIBRATING PAN0 PROCCESS
1392     if (STATE(CALIBRATE_PAN)) {
1393         servoPanTimer = millis();
1394         targetPosition.heading = 0;
1395         DISABLE_STATE(CALIBRATE_PAN);
1396         ENABLE_PROTOCOL(TP_CALIBRATING_PAN0);
1397         pwmPan = masterConfig.pan_calibration_pulse;
1398         pwmWriteServo(masterConfig.pan_pin, pwmPan);
1399         masterConfig.pan0_calibrated = 0;
1400         minPwmPan = 1500;
1401         maxPwmPan = 1500;
1402         maxDeltaHeading = 0;
1403         if(masterConfig.mag_calibrated == 0) {
1404                         ENABLE_STATE(CALIBRATE_MAG);
1405         }
1406         pwmPanState = FINDING_OUT_MIN_PWMPAN0;
1407         return;
1408      }
1409
1410     //CALIBRATING PWMPAN0
1411     if(PROTOCOL(TP_CALIBRATING_PAN0) && !PROTOCOL(TP_CALIBRATING_MAG)){
1412         if(masterConfig.pan0_calibrated == 0){
1413                         // CALCULATING MIN AND MAX PAN0
1414                         if(millis() - servoPanTimer > 100 && (pwmPanState == FINDING_OUT_MIN_PWMPAN0 || pwmPanState == FINDING_OUT_MAX_PWMPAN0)){
1415                                 trackerPosition.heading = getHeading();
1416                                 deltaHeading = calculateDeltaHeading(trackerPosition.heading,targetPosition.heading);
1417                                 if (deltaHeading > 2){
1418                                         // SERVO IS STILL MOVING
1419                                         targetPosition.heading = trackerPosition.heading;
1420                                         if(pwmPanState == FINDING_OUT_MIN_PWMPAN0) {
1421                                                 pwmPan++;
1422                                                 if(pwmPan > maxPwmPan + 100)
1423                                                         pwmPan = minPwmPan - 100;
1424                                         } else if(pwmPanState == FINDING_OUT_MAX_PWMPAN0){
1425                                                 pwmPan--;
1426                                                 if(pwmPan < minPwmPan - 100)
1427                                                         pwmPan = maxPwmPan + 100;
1428                                         }
1429                                         pwmWriteServo(masterConfig.pan_pin, pwmPan);
1430                                 } else {
1431                                         // SERVO SEEMS TO BE STOPPED
1432                                         if(pwmPanState == FINDING_OUT_MIN_PWMPAN0){
1433                                                 pwmPanState = MIN_PWMPAN0_FOUND;
1434                                                 minPwmPan = pwmPan;
1435                                                 printf("min %d\n", minPwmPan);
1436                                         } else if(pwmPanState == FINDING_OUT_MAX_PWMPAN0){
1437                                                 pwmPanState = MAX_PWMPAN0_FOUND;
1438                                                 maxPwmPan = pwmPan;
1439                                                 if(maxPwmPan < minPwmPan)
1440                                                         maxPwmPan = minPwmPan;
1441                                                 printf("max %d\n", maxPwmPan);
1442                                         }
1443                                 }
1444                                 servoPanTimer = millis();
1445                         }
1446                         //CHECK IF MIN AND MAX PAN0 HAS BEEN WELL CALIBRATED 3 SECONDS LATER
1447                         if(pwmPanState == MIN_PWMPAN0_FOUND || pwmPanState == MAX_PWMPAN0_FOUND) {
1448                                 if(millis() - servoPanTimer > 3000){
1449                                         trackerPosition.heading = getHeading();
1450                                         // due to interference the magnetometer could oscillate while the servo is stopped
1451                                         deltaHeading = calculateDeltaHeading(trackerPosition.heading,targetPosition.heading);
1452                                         if (deltaHeading > 50){
1453                                                 // SERVO IS STILL MOVING
1454                                                 targetPosition.heading = trackerPosition.heading;
1455                                                 if(pwmPanState == MIN_PWMPAN0_FOUND) {
1456                                                         pwmPanState = FINDING_OUT_MIN_PWMPAN0;
1457                                                         pwmPan = masterConfig.pan_calibration_pulse;
1458                                                 } else if(pwmPanState == MAX_PWMPAN0_FOUND){
1459                                                         pwmPanState = FINDING_OUT_MAX_PWMPAN0;
1460                                                         pwmPan = 1500 + (1500 - masterConfig.pan_calibration_pulse);
1461                                                 }
1462                                                 pwmWriteServo(masterConfig.pan_pin, pwmPan);
1463                                         }
1464                                         else {
1465                                                 // SERVO IS STOPED
1466                                                 if(pwmPanState == MIN_PWMPAN0_FOUND){
1467                                                         //targetPosition.heading = trackerPosition.heading;
1468                                                         pwmPanState = FINDING_OUT_MAX_PWMPAN0;
1469                                                         pwmPan = 1500 + (1500 - masterConfig.pan_calibration_pulse);
1470                                                         pwmWriteServo(masterConfig.pan_pin, pwmPan);
1471                                                 } else if(pwmPanState == MAX_PWMPAN0_FOUND) {
1472                                                         // CALIBRATION FIHISHED WITH SUCCESS
1473                                                         pwmPanState = PWMPAN0_CALCULATED_WITH_SUCCESS;
1474                                                         masterConfig.mag_calibrated = 1;
1475                                                         masterConfig.pan0_calibrated = 1;
1476                                                         DISABLE_PROTOCOL(TP_CALIBRATING_PAN0);
1477                                                         masterConfig.min_pan_speed = (uint16_t)(maxPwmPan - minPwmPan)/2.0f;
1478                                                         masterConfig.pan0 = minPwmPan + masterConfig.min_pan_speed;
1479                                                         printf("Calibration has finished with success:\n");
1480                                                         printf("  set pan0=%d\n", masterConfig.pan0);
1481                                                         printf("  set min_pan_speed=%d\n", masterConfig.min_pan_speed);
1482                                                         printf("  set pan0_calibrated=%d\n", masterConfig.mag_calibrated);
1483                                                         saveConfigAndNotify();
1484                                                         // ACTIVATE MAX PWMPAN CALCULATION
1485                                                         /*ENABLE_PROTOCOL(TP_CALIBRATING_MAXPAN);
1486                                                         pwmPan = masterConfig.pan0 - 600;
1487                                                         pwmWriteServo(masterConfig.pan_pin, pwmPan);*/
1488                                                         trackerPosition.heading = getHeading();
1489                                                         targetPosition.heading = trackerPosition.heading;
1490                                                 }
1491                                         }
1492                                         servoPanTimer = millis();
1493                                 }
1494
1495                         }
1496         }
1497
1498
1499
1500     }
1501
1502
1503     /*// CALIBRATE MAX PAN
1504         if(PROTOCOL(TP_CALIBRATING_MAXPAN) && !PROTOCOL(TP_CALIBRATING_MAG)) {
1505                 trackerPosition.heading = getHeading();
1506                 deltaHeading = calculateDeltaHeading(trackerPosition.heading,targetPosition.heading);
1507                 if(deltaHeading > 450){
1508                         servoPanVarTime = millis() - servoPanTimer;
1509                         slope = (servoPanVarTime/10.0f);
1510                         if(slope > 0.0f)
1511                                 printf("%d, %d,%d\r\n", pwmPan,servoPanVarTime,deltaHeading);
1512                         // Enviar nuevo pulso
1513                         pwmPan += 10;
1514                         if(pwmPan > masterConfig.pan0 + 600) {
1515                                 //Calcular m�ximo y finalizar
1516                                 masterConfig.max_pid_gain = maxPwmPan;
1517                                 pwmWriteServo(masterConfig.pan_pin, masterConfig.pan0);
1518                                 DISABLE_PROTOCOL(TP_CALIBRATING_MAXPAN);
1519                                 saveConfigAndNotify();
1520                         } else {
1521                                 pwmWriteServo(masterConfig.pan_pin, pwmPan);
1522                                 trackerPosition.heading = getHeading();
1523                                 targetPosition.heading = trackerPosition.heading;
1524                                 servoPanTimer = millis();
1525                         }
1526                 }
1527         }*/
1528 }
1529
1530 uint16_t calculateDeltaHeading(uint16_t heading1, uint16_t heading2){
1531
1532         int32_t deltaHeading;
1533
1534         deltaHeading = heading1 - heading2;
1535
1536         if(heading1 < heading2)
1537                 deltaHeading = deltaHeading + 3600;
1538
1539         if (deltaHeading > 1800)
1540                 deltaHeading -= 3600;
1541         else if (deltaHeading < -1800)
1542                 deltaHeading += 3600;
1543
1544         return (uint16_t) abs(deltaHeading);
1545 }
1546
1547 void setEpsMode(void){
1548         EPS_MODE =  masterConfig.eps;
1549 }
1550
1551 void updateEPSParams(){
1552         EPS_DISTANCE_GAIN = masterConfig.eps_gain.distance;
1553         EPS_FREQUENCY = masterConfig.eps_frequency;
1554 }
1555
1556 void updateProtocolDetection(void){
1557         uint16_t detected_protocol;
1558
1559         if(!feature(FEATURE_AUTODETECT) || cliMode)
1560                 return;
1561
1562         detected_protocol = getProtocol();
1563
1564         if(detected_protocol == 0 && !detection_title_updated ){
1565                 detection_title_updated = true;
1566                 updateDisplayProtocolTitle(detected_protocol);
1567                 return;
1568         }
1569
1570
1571         if(detected_protocol > 0 && isProtocolDetectionEnabled()){
1572             updateDisplayProtocolTitle(detected_protocol);
1573             detection_title_updated = false;
1574             protocolInit(detected_protocol);
1575             if(!homeSet){
1576             trackingInit();
1577             if(PROTOCOL(TP_MFD))
1578                 settingHome = true;
1579         }
1580         }
1581 }
1582
1583 void protocolInit(uint16_t protocol){
1584         DISABLE_PROTOCOL(0b1111111111111);
1585         switch(protocol) {
1586           case TP_SERVOTEST:
1587                 ENABLE_PROTOCOL(TP_SERVOTEST);
1588                 break;
1589           case TP_MFD:
1590                 ENABLE_PROTOCOL(TP_MFD);
1591                 featureClear(FEATURE_EPS);
1592                 featureClear(FEATURE_GPS);
1593                 mfdTestMode = false;
1594                 break;
1595           case TP_GPS_TELEMETRY:
1596                 ENABLE_PROTOCOL(TP_GPS_TELEMETRY);
1597                 break;
1598           case TP_MAVLINK:
1599                 ENABLE_PROTOCOL(TP_MAVLINK);
1600                 break;
1601           case TP_RVOSD:
1602                 ENABLE_PROTOCOL(TP_RVOSD);
1603                 break;
1604           case TP_FRSKY_D:
1605                 ENABLE_PROTOCOL(TP_FRSKY_D);
1606                 telemetry_provider = masterConfig.telemetry_provider;
1607                 break;
1608           case TP_FRSKY_X:
1609                 ENABLE_PROTOCOL(TP_FRSKY_X);
1610                 telemetry_provider = masterConfig.telemetry_provider;
1611                 break;
1612           case TP_LTM:
1613                 ENABLE_PROTOCOL(TP_LTM);
1614                 break;
1615           case TP_CROSSFIRE:
1616                 ENABLE_PROTOCOL(TP_CROSSFIRE);
1617                 break;
1618           case TP_PITLAB:
1619                 ENABLE_PROTOCOL(TP_PITLAB);
1620                 break;
1621         }
1622 }
1623
1624 uint8_t filterTiltAngle(uint8_t target){
1625         return (masterConfig.tilt_max_angle > 0 && target > masterConfig.tilt_max_angle)?masterConfig.tilt_max_angle:target;
1626 }
1627
1628 void calcTelemetryFrequency(void){
1629
1630         telemetry_fixes++;
1631
1632         if(millis() - telemetry_millis >= 5000){
1633                 telemetry_frequency = (uint8_t) (telemetry_fixes / 5.0);
1634                 telemetry_fixes = 0;
1635                 telemetry_millis = millis();
1636         }
1637 }
1638
1639 void servosInit(void)
1640 {
1641         pwmWriteServo(masterConfig.pan_pin, masterConfig.pan0);
1642         pwmWriteServo(masterConfig.tilt_pin, masterConfig.tilt0);
1643 }