updated to r780. added cells/vario and current sensor
updated to r780. added cells/vario and current sensor

--- a/ArduCAM_OSD.ino
+++ b/ArduCAM_OSD.ino
@@ -70,7 +70,7 @@
 #include "wiring.h"
 #endif
 #include <EEPROM.h>
-#include <SimpleTimer.h>
+//#include <SimpleTimer.h>
 #include <GCS_MAVLink.h>
 
 #ifdef membug
@@ -97,7 +97,7 @@
 FastSerialPort0(Serial);
 OSD osd; //OSD object 
 
-SimpleTimer  mavlinkTimer;
+//SimpleTimer  mavlinkTimer;
 
 
 /* **********************************************/
@@ -159,11 +159,11 @@
 delay(2000);
 Serial.flush(); 
     // Startup MAVLink timers  
-    mavlinkTimer.Set(&OnMavlinkTimer, 120);
+    //mavlinkTimer.Set(&OnMavlinkTimer, 120);
 
     // House cleaning, clear display and enable timers
-    osd.clear();
-    mavlinkTimer.Enable();
+//    osd.clear();
+    //mavlinkTimer.Enable();
 
 } // END of setup();
 
@@ -177,7 +177,7 @@
 void loop() 
 {
 
-    if(enable_mav_request == 1){//Request rate control
+    /*if(enable_mav_request == 1){//Request rate control
         //osd.clear();
         //osd.setPanel(3,10);
         //osd.openPanel();
@@ -192,10 +192,15 @@
         osd.clear();
         waitingMAVBeats = 0;
         lastMAVBeat = millis();//Preventing error from delay sensing
+    }*/
+    
+    //Run "timer" every 120 miliseconds
+    if(millis() > mavLinkTimer + 120){
+      mavLinkTimer = millis();
+      OnMavlinkTimer();
     }
-
     read_mavlink();
-    mavlinkTimer.Run();
+    //mavlinkTimer.Run();
 }
 
 /* *********************************************** */
@@ -212,8 +217,9 @@
     writePanels();       // writing enabled panels (check OSD_Panels Tab)
     
     setFdataVars();
+    
+    checkModellType();
 }
-
 
 void unplugSlaves(){
     //Unplug list of SPI
@@ -221,5 +227,5 @@
     digitalWrite(10,  HIGH); // unplug USB HOST: ArduCam Only
 #endif
     digitalWrite(MAX7456_SELECT,  HIGH); // unplug OSD
-}
-
+}

+

file:a/Font.ino -> file:b/Font.ino
--- a/Font.ino
+++ b/Font.ino
@@ -1,4 +1,4 @@
-
+/*
 void uploadFont()
 {
     uint16_t byte_count = 0;
@@ -17,7 +17,7 @@
 //    osd.closePanel();
 
 
-    Serial.printf_P(PSTR("Ready for Font\n"));
+    Serial.printf_P("Ready for Font\n");
 
     while(font_count < 256) { 
         int8_t incomingByte = Serial.read();
@@ -84,12 +84,13 @@
             osd.write_NVM(font_count, character_bitmap);    
             byte_count = 0;
             font_count++;
-            Serial.printf_P(PSTR("Char Done\n"));
+            Serial.printf_P("Char Done\n");
         }
     }
 
     //  character_bitmap[]
 }
 
-

 
+*/

+

--- a/MAVLink.ino
+++ b/MAVLink.ino
@@ -2,8 +2,6 @@
 #include "../GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink.h"
 
 #ifdef FRSKY
-#ifndef SPort_H
-#define SPort_H
 
 #define FRSKY_RX_PACKET_SIZE 0x09
 
@@ -44,10 +42,19 @@
 #define CURR_LAST_ID       0x020f
 #define VFAS_FIRST_ID      0x0210
 #define VFAS_LAST_ID       0x021f
-#define GPS_SPEED_FIRST_ID 0x0830
-#define GPS_SPEED_LAST_ID  0x083f
 #define CELLS_FIRST_ID     0x0300
 #define CELLS_LAST_ID      0x030f
+
+#define GPS_LONG_LATI_FIRST_ID  0x0800
+#define GPS_LONG_LATI_LAST_ID   0x080f
+#define GPS_ALT_FIRST_ID        0x0820
+#define GPS_ALT_LAST_ID         0x082f
+#define GPS_SPEED_FIRST_ID      0x0830
+#define GPS_SPEED_LAST_ID       0x083f
+#define GPS_COURS_FIRST_ID      0x0840
+#define GPS_COURS_LAST_ID       0x084f
+#define GPS_TIME_DATE_FIRST_ID  0x0850
+#define GPS_TIME_DATE_LAST_ID   0x085f
 
 #define SPORT_DATA_U8(packet)   (buffer[4])
 #define SPORT_DATA_S32(packet)  (*((int32_t *)(buffer+4)))
@@ -56,17 +63,16 @@
 long          _varioSpeed;
 long          _varioAltitude;
 unsigned long _altitudeOffset;
+unsigned long _baroAltitude;
 int           _vfasVoltage;
 int           _vfasCurrent;
 unsigned long _vfasConsumption;
 unsigned long _uptime;
-int           mySerialStarted;
-
-static byte buffer[FRSKY_RX_PACKET_SIZE];
-static byte bufferIndex = 0;
-static byte dataState = STATE_DATA_IDLE;
-
-#endif
+unsigned int   osd_swr;
+//unsigned int   osd_rssi;
+float          _cellVoltage;
+static float   osd_analog_batt = 0;                 // Battery A voltage in milivolt
+
 #endif
 
 // true when we have received at least 1 MAVLink packet
@@ -97,6 +103,10 @@
     mavlink_message_t msg; 
     mavlink_status_t status;
 
+    static byte buffer[FRSKY_RX_PACKET_SIZE];
+    static byte bufferIndex = 0;
+    static byte dataState = STATE_DATA_IDLE;
+
 #ifdef FRSKY_DEBUG
     if ( Serial.available() ) {
       osd.setPanel(5,9);
@@ -111,9 +121,11 @@
     }
 #endif
 
+
     //grabing data 
-    while(Serial.available() > 0) {
+    while(Serial.available() > 0) { 
 #ifdef FRSKY
+
         byte data = Serial.read();
     
         /* allow CLI to be started by hitting enter 3 times, if no
@@ -125,29 +137,31 @@
                 crlf_count = 0;
             }
             if (crlf_count == 3) {
-                uploadFont();
+                //uploadFont();
             }
-        } else {
-            //mavlink_active = 1;
         }
-    
+        
         if (data == START_STOP) {
+          mavlink_active = 1;
           dataState = STATE_DATA_IN_FRAME;
           bufferIndex = 0;
           
-          mavbeat = 1;
-          //apm_mav_system    = msg.sysid;
-          //apm_mav_component = msg.compid;
-          osd_mode = 1;
-          //Mode (arducoper armed/disarmed)
-          //base_mode = mavlink_msg_heartbeat_get_base_mode(&msg);
-          motor_armed = 1;
-    
-          osd_nav_mode = 0;
-          lastMAVBeat = millis();
-          if(waitingMAVBeats == 1){
-              enable_mav_request = 1;
-          }
+                    mavbeat = 1;
+                    apm_mav_system    = 0;
+                    apm_mav_component = 0;
+//                    apm_mav_type      = mavlink_msg_heartbeat_get_type(&msg);            
+                 //   osd_mode = mavlink_msg_heartbeat_get_custom_mode(&msg);
+                    //osd_mode = (uint8_t)mavlink_msg_heartbeat_get_custom_mode(&msg);
+                    //Mode (arducoper armed/disarmed)
+                    //base_mode = mavlink_msg_heartbeat_get_base_mode(&msg);
+//                    if(getBit(base_mode,7)) motor_armed = 1;
+//                    else motor_armed = 0;
+
+                    osd_nav_mode = 0;          
+                    /*lastMAVBeat = millis();
+                    if(waitingMAVBeats == 1){
+                        enable_mav_request = 1;
+                    }*/
           
         } else {
           switch (dataState) {
@@ -165,7 +179,6 @@
         }
     
         if (bufferIndex == FRSKY_RX_PACKET_SIZE) {
-          mavlink_active = 1;
           dataState = STATE_DATA_IDLE;
     
           short crc = 0;
@@ -178,42 +191,141 @@
           }
           if (crc == 0x00ff) {
             byte packetType = buffer[1];
+            
+            
             switch (packetType) {
               case DATA_FRAME:
                 unsigned int appId = *((unsigned int *)(buffer+2));
     
+                if (appId == RSSI_ID) {
+                  osd_rssi = SPORT_DATA_U8(buffer);
+                  //Serial.println ("osd_rssi");
+                  //Serial.println (osd_rssi);
+                }
+                if (appId == SWR_ID) {
+                  osd_swr = SPORT_DATA_U8(buffer);
+                  //Serial.println ("osd_swr");
+                  //Serial.println (osd_swr);                  
+                }
+                if (appId == BATT_ID) {
+                  osd_analog_batt = SPORT_DATA_U8(buffer);
+                  //Serial.println ("osd_analog_batt");
+                  //Serial.println (osd_analog_batt);                                    
+                }
+    
                 if (appId >= VARIO_FIRST_ID && appId <= VARIO_LAST_ID) {
-                  osd_groundspeed = SPORT_DATA_S32(buffer);
-    
+                  osd_airspeed = SPORT_DATA_S32(buffer);
+                  //Serial.println ("osd_airspeed");
+                  //Serial.println (osd_airspeed);
+                  
                 } else if (appId >= ALT_FIRST_ID && appId <= ALT_LAST_ID) {
-                    if (_altitudeOffset == 0)
-                      _altitudeOffset = -SPORT_DATA_S32(buffer);
-    
-                  osd_alt = SPORT_DATA_S32(buffer) + _altitudeOffset;
-    
+                  //if (_altitudeOffset == 0) _altitudeOffset = -SPORT_DATA_S32(buffer);
+                  
+                  setBaroAltitude( SPORT_DATA_S32(buffer) );
+                  //Serial.println ("osd_alt");
+                  //Serial.println (_baroAltitude/100, 2);
+                  osd_alt = float(_baroAltitude/100);
                 } else if (appId >= VFAS_FIRST_ID && appId <= VFAS_LAST_ID) {
-                  osd_vbat_A = SPORT_DATA_S32(buffer) * 10;
-    
+                  osd_vbat_A = SPORT_DATA_U32(buffer);
+                  //Serial.println ("osd_vbat_A");
+                  //Serial.println (float(osd_vbat_A/100));
+                  osd_vbat_A = float(osd_vbat_A/100);
+                  
                 } else if (appId >= CURR_FIRST_ID && appId <= CURR_LAST_ID) {
                   osd_curr_A = SPORT_DATA_U32(buffer);
-    
+                  osd_curr_A = float(osd_curr_A/100);
                   unsigned long now = micros();
-                  osd_battery_remaining_A += (long)osd_curr_A * 1000 / (3600000000 /
-                                                                  (now - _uptime));
+                  //osd_battery_remaining_A += (long)osd_curr_A * 1000 / (3600000000 / (now - _uptime));
+                  //Serial.println ("osd_curr_A");
+                  //Serial.println (osd_curr_A);
+                  
                   _uptime = now;
-    
-                } else if (appId == RSSI_ID) {
-                  osd_rssi = SPORT_DATA_U32(buffer);
+                } else if (appId >= CELLS_FIRST_ID && appId <= CELLS_LAST_ID) {
+                  uint32_t cells = SPORT_DATA_U32(buffer);
+                  uint8_t battnumber = cells & 0xF;
+                  //Serial.println ("battnumber");
+                  //Serial.println (battnumber);
+                  _cellVoltage = ((((cells & 0x000FFF00) >> 8) / 10)*2);
+                  //Serial.println ("cellVoltage");
+                  //Serial.println ( cellVoltage/100, 2 );
+                   osd_battery_remaining_A = float(_cellVoltage/100);
+                  
+                } else if (appId >= GPS_SPEED_FIRST_ID && appId <= GPS_SPEED_LAST_ID) {
+                   osd_groundspeed = SPORT_DATA_U32(buffer);
+                   osd_groundspeed = (osd_groundspeed * 46) / 25 / 1000;
+                   //Serial.println ("osd_groundspeed");
+                   //Serial.println (osd_groundspeed);                   
+                   
+                } else if (appId >= GPS_COURS_FIRST_ID && appId <= GPS_COURS_LAST_ID) {
+                   osd_heading = SPORT_DATA_U32(buffer);
+                   //frskyData.hub.gpsCourse_bp = course / 100;
+                   //frskyData.hub.gpsCourse_ap = course % 100;  
+                   //Serial.println ("osd_heading");
+                   //Serial.println (osd_heading);                                      
+                   
+                } else if (appId >= GPS_TIME_DATE_FIRST_ID && appId <= GPS_TIME_DATE_LAST_ID) {
+                    uint32_t gps_time_date = SPORT_DATA_U32(buffer);
+                    //Serial.println ("gps_time_date");
+                    //Serial.println (gps_time_date);                                                          
+                    
+                    if (gps_time_date & 0x000000ff) {
+                      //frskyData.hub.year = (uint16_t) ((gps_time_date & 0xff000000) >> 24);
+                      //frskyData.hub.month = (uint8_t) ((gps_time_date & 0x00ff0000) >> 16);
+                      //frskyData.hub.day = (uint8_t) ((gps_time_date & 0x0000ff00) >> 8);
+                    }
+                    else {
+                      //frskyData.hub.hour = (uint8_t) ((gps_time_date & 0xff000000) >> 24);
+                      //frskyData.hub.min = (uint8_t) ((gps_time_date & 0x00ff0000) >> 16);
+                      //frskyData.hub.sec = (uint16_t) ((gps_time_date & 0x0000ff00) >> 8);
+                      //frskyData.hub.hour = ((uint8_t) (frskyData.hub.hour + g_eeGeneral.timezone + 24)) % 24;
+                    }
+                   
+                } else if (appId >= GPS_ALT_FIRST_ID && appId <= GPS_ALT_LAST_ID) {
+                   //osd_alt = SPORT_DATA_S32(buffer);   
+                   
+                } else if (appId >= GPS_LONG_LATI_FIRST_ID && appId <= GPS_LONG_LATI_LAST_ID) {
+                    uint32_t gps_long_lati_data = SPORT_DATA_U32(buffer);
+                    uint32_t gps_long_lati_b1w, gps_long_lati_a1w;
+                    gps_long_lati_b1w = (gps_long_lati_data & 0x3fffffff) / 10000;
+                    gps_long_lati_a1w = (gps_long_lati_data & 0x3fffffff) % 10000;
+                    osd_heading = 0;
+                    switch ((gps_long_lati_data & 0xc0000000) >> 30) {
+                      case 0:
+                        osd_lat = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
+                        //osd_lat = gps_long_lati_a1w;
+                        //frskyData.hub.gpsLatitudeNS = 'N';
+                        osd_heading = 0;
+                        break;
+                      case 1:
+                        osd_lat = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
+                        //osd_lat = gps_long_lati_a1w;
+                        //frskyData.hub.gpsLatitudeNS = 'S';
+                        osd_heading = 90;
+                        break;
+                      case 2:
+                        osd_lon = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
+                        //osd_lon = gps_long_lati_a1w;
+                        //frskyData.hub.gpsLongitudeEW = 'E';
+                        osd_heading = 180;
+                        break;
+                      case 3:
+                        osd_lon = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
+                        //osd_lon = gps_long_lati_a1w;
+                        //frskyData.hub.gpsLongitudeEW = 'W';
+                        osd_heading = 360;
+                        break;
+                    }                   
                 }
                 break;
             }
+            
+            
           }
         }
-#endif
-
-#ifndef FRSKY
+
+#else
         uint8_t c = Serial.read();
-        
+
         /* allow CLI to be started by hitting enter 3 times, if no
         heartbeat packets have been received */
         if (mavlink_active == 0 && millis() < 20000 && millis() > 5000) {
@@ -222,13 +334,14 @@
             } else {
                 crlf_count = 0;
             }
-            if (crlf_count == 3) {
-                uploadFont();
-            }
-        }        
-        
+//            if (crlf_count == 3) {
+//                uploadFont();
+//            }
+        }
+
         //trying to grab msg  
         if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
+            lastMAVBeat = millis();
             mavlink_active = 1;
             //handle msg
             switch(msg.msgid) {
@@ -242,14 +355,14 @@
                     osd_mode = (uint8_t)mavlink_msg_heartbeat_get_custom_mode(&msg);
                     //Mode (arducoper armed/disarmed)
                     base_mode = mavlink_msg_heartbeat_get_base_mode(&msg);
-                    if(getBit(base_mode,7)) motor_armed = 1;
-                    else motor_armed = 0;
+//                    if(getBit(base_mode,7)) motor_armed = 1;
+//                    else motor_armed = 0;
 
                     osd_nav_mode = 0;          
-                    lastMAVBeat = millis();
+                    /*lastMAVBeat = millis();
                     if(waitingMAVBeats == 1){
                         enable_mav_request = 1;
-                    }
+                    }*/
                 }
                 break;
             case MAVLINK_MSG_ID_SYS_STATUS:
@@ -269,6 +382,8 @@
                     osd_lon = mavlink_msg_gps_raw_int_get_lon(&msg) / 10000000.0f;
                     osd_fix_type = mavlink_msg_gps_raw_int_get_fix_type(&msg);
                     osd_satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(&msg);
+                    osd_cog = mavlink_msg_gps_raw_int_get_cog(&msg);
+                    eph = mavlink_msg_gps_raw_int_get_eph(&msg);
                 }
                 break; 
             case MAVLINK_MSG_ID_VFR_HUD:
@@ -295,8 +410,8 @@
 //                  nav_bearing = mavlink_msg_nav_controller_output_get_nav_bearing(&msg);
                   wp_target_bearing = mavlink_msg_nav_controller_output_get_target_bearing(&msg);
                   wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(&msg);
-//                  alt_error = mavlink_msg_nav_controller_output_get_alt_error(&msg);
-//                  aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(&msg);
+                  alt_error = mavlink_msg_nav_controller_output_get_alt_error(&msg);
+                  aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(&msg);
                   xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(&msg);
                 }
                 break;
@@ -320,14 +435,21 @@
                 break;           
             case MAVLINK_MSG_ID_WIND:
                 {
-                    osd_winddirection = abs(mavlink_msg_wind_get_direction(&msg)); // 0..360 deg, 0=north
-                    osd_windspeed = mavlink_msg_wind_get_speed(&msg); //m/s
-//                    osd_windspeedz = mavlink_msg_wind_get_speed_z(&msg); //m/s
+//                  if (osd_climb < 1.66 && osd_climb > -1.66){
+                  osd_winddirection = mavlink_msg_wind_get_direction(&msg); // 0..360 deg, 0=north
+                  osd_windspeed = mavlink_msg_wind_get_speed(&msg); //m/s
+//                  osd_windspeedz = mavlink_msg_wind_get_speed_z(&msg); //m/s
+//                  }
                 }
                 break;
             case MAVLINK_MSG_ID_SCALED_PRESSURE:
                 {
                     temperature = mavlink_msg_scaled_pressure_get_temperature(&msg);
+                }
+                break;
+            case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
+                {
+                    osd_home_alt = osd_alt - (mavlink_msg_global_position_int_get_relative_alt(&msg)*0.001);
                 }
                 break;
             default:
@@ -336,6 +458,7 @@
             }
         }
 #endif
+        
         delayMicroseconds(138);
         //next one
     }
@@ -343,5 +466,19 @@
     packet_drops += status.packet_rx_drop_count;
     parse_error += status.parse_error;
 
-}

-
+}
+
+#ifdef FRSKY
+void setBaroAltitude(float baroAltitude)
+{
+  if (!_altitudeOffset)
+    _altitudeOffset = -baroAltitude;
+
+  baroAltitude += _altitudeOffset;
+  _baroAltitude = baroAltitude;
+  if (_baroAltitude < 0 || _baroAltitude > 1000000) {
+    _baroAltitude = 0;
+  }
+}
+#endif

+

--- a/OSD_Config.h
+++ b/OSD_Config.h
@@ -49,7 +49,7 @@
 #define Off_BIT        1
 #define WindS_BIT      2
 #define Climb_BIT      3
-//#define Tune_BIT       4
+#define Tune_BIT       4
 #define CALLSIGN_BIT   5
 #define RSSI_BIT       6
 #define Eff_BIT        7
@@ -163,9 +163,9 @@
 #define panClimb_en_ADDR 182
 #define panClimb_x_ADDR 184
 #define panClimb_y_ADDR 186
-//#define panTune_en_ADDR 188
-//#define panTune_x_ADDR 190
-//#define panTune_y_ADDR 192
+#define panTune_en_ADDR 188
+#define panTune_x_ADDR 190
+#define panTune_y_ADDR 192
 #define panEff_en_ADDR 194
 #define panEff_x_ADDR 196
 #define panEff_y_ADDR 198
@@ -185,6 +185,12 @@
 #define panDistance_x_ADDR 226
 #define panDistance_y_ADDR 228
 
+#define SIGN_MSL_ON_ADDR 876
+#define SIGN_HA_ON_ADDR 878
+#define SIGN_GS_ON_ADDR 880
+#define SIGN_AS_ON_ADDR 882 
+#define MODELL_TYPE_ADD 884 
+#define AUTO_SCREEN_SWITC_ADD 886
 #define OSD_BATT_SHOW_PERCENT_ADDR 888
 #define measure_ADDR 890
 #define overspeed_ADDR 892
@@ -206,6 +212,12 @@
 
 #define OSD_CALL_SIGN_ADDR 920
 #define OSD_CALL_SIGN_TOTAL 8
+#define FW_VERSION1_ADDR 930
+#define FW_VERSION2_ADDR 932
+#define FW_VERSION3_ADDR 934
+#define CS_VERSION1_ADDR 936
+#define CS_VERSION2_ADDR 938
+#define CS_VERSION3_ADDR 940
 
 #define CHK1 1000
 #define CHK2 1006

--- a/OSD_Config_Func.ino
+++ b/OSD_Config_Func.ino
@@ -45,10 +45,10 @@
     writeEEPROM(VER-42,CHK2);
     for(panel = 0; panel < npanels; panel++) writeSettings();
 
-    osd.setPanel(4,9);
-    osd.openPanel();
-    osd.printf_P(PSTR("OSD Initialized, reboot")); 
-    osd.closePanel();
+//    osd.setPanel(4,9);
+//    osd.openPanel();
+//    osd.printf_P(PSTR("OSD Initialized, reboot")); 
+//    osd.closePanel();
 
     // run for ever so user resets 
     for(;;) {}
@@ -156,9 +156,9 @@
     writeEEPROM(on, panClimb_en_ADDR + offset);
     writeEEPROM(1,  panClimb_x_ADDR + offset);
     writeEEPROM(8,  panClimb_y_ADDR + offset);
-//    writeEEPROM(on, panTune_en_ADDR + offset);
-//    writeEEPROM(10, panTune_x_ADDR + offset);
-//    writeEEPROM(4,  panTune_y_ADDR + offset);
+    writeEEPROM(on, panTune_en_ADDR + offset);
+    writeEEPROM(10, panTune_x_ADDR + offset);
+    writeEEPROM(4,  panTune_y_ADDR + offset);
     writeEEPROM(on, panEff_en_ADDR + offset);
     writeEEPROM(14, panEff_x_ADDR + offset);
     writeEEPROM(13, panEff_y_ADDR + offset);
@@ -186,6 +186,7 @@
     stall = EEPROM.read(stall_ADDR);
     battv = EEPROM.read(battv_ADDR);
     switch_mode = EEPROM.read(switch_mode_ADDR);
+    panel_auto_switch = EEPROM.read(AUTO_SCREEN_SWITC_ADD);
 //    if (EEPROM.read(ch_toggle_ADDR) < 4 || EEPROM.read(ch_toggle_ADDR) > 8){
 //     	EEPROM.write(ch_toggle_ADDR, 5);
 //	}
@@ -335,9 +336,9 @@
     panClimb_XY[0][panel] = readEEPROM(panClimb_x_ADDR + offset);
     panClimb_XY[1][panel] = checkPAL(readEEPROM(panClimb_y_ADDR + offset));
 
-//    setBit(panD_REG[panel], Tune_BIT, readEEPROM(panTune_en_ADDR + offset));
-//    panTune_XY[0][panel] = readEEPROM(panTune_x_ADDR + offset);
-//    panTune_XY[1][panel] = checkPAL(readEEPROM(panTune_y_ADDR + offset));
+    setBit(panD_REG[panel], Tune_BIT, readEEPROM(panTune_en_ADDR + offset));
+    panTune_XY[0][panel] = readEEPROM(panTune_x_ADDR + offset);
+    panTune_XY[1][panel] = checkPAL(readEEPROM(panTune_y_ADDR + offset));
 
     setBit(panD_REG[panel], RSSI_BIT, readEEPROM(panRSSI_en_ADDR + offset));
     panRSSI_XY[0][panel] = readEEPROM(panRSSI_x_ADDR + offset);

file:a/OSD_Func.h -> file:b/OSD_Func.h
--- a/OSD_Func.h
+++ b/OSD_Func.h
@@ -1,127 +1,135 @@
-
-//------------------ Heading and Compass ----------------------------------------
-
-static char buf_show[12];
-const char buf_Rule[36] = {0x82,0x80,0x81,0x80,0x81,0x80,
-                           0x84,0x80,0x81,0x80,0x81,0x80,
-                           0x83,0x80,0x81,0x80,0x81,0x80,
-                           0x85,0x80,0x81,0x80,0x81,0x80};
-void setHeadingPatern()
-{
-  int start;
-  start = round((osd_heading * 24)/360);
-  start -= 3;
-  if(start < 0) start += 24;
-  for(int x=0; x <= 10; x++){
-    buf_show[x] = buf_Rule[start];
-    if(++start > 23) start = 0;
-  }
-  buf_show[7] = '\0';
-}
-
-//------------------ Battery Remaining Picture ----------------------------------
-
-char setBatteryPic(uint16_t bat_level)
-{
-  if(bat_level <= 100){
-    return 0xb4;
-  }
-  else if(bat_level <= 300){
-    return 0xb5;
-  }
-  else if(bat_level <= 400){
-    return 0xb6;
-  }
-  else if(bat_level <= 500){
-    return 0xb7;
-  }
-  else if(bat_level <= 800){
-    return 0xb8;
-  }
-  else return 0xb9;
-}
-
-//------------------ Home Distance and Direction Calculation ----------------------------------
-
-void setHomeVars(OSD &osd)
-{
-  float dstlon, dstlat;
-  long bearing;
-  
-  if(osd_throttle > 3 && takeoff_heading == -400)
-    takeoff_heading = osd_heading;
-  osd_alt_to_home = (osd_alt - osd_home_alt);
-  if(osd_got_home == 0 && osd_fix_type > 1){
-    osd_home_lat = osd_lat;
-    osd_home_lon = osd_lon;
-    //osd_home_alt = osd_alt;
-    osd_got_home = 1;
-  }
-  else if(osd_got_home == 1){
-    // JRChange: osd_home_alt: check for stable osd_alt (must be stable for 25*120ms = 3s)
-    if(osd_alt_cnt < 25){
-      if(fabs(osd_alt_prev - osd_alt) > 0.5){
-        osd_alt_cnt = 0;
-        osd_alt_prev = osd_alt;
-      }
-      else
-      {
-        if(++osd_alt_cnt >= 25){
-          osd_home_alt = osd_alt;  // take this stable osd_alt as osd_home_alt
-          haltset = 1;
-        }
-      }
-    }
-    // shrinking factor for longitude going to poles direction
-    float rads = fabs(osd_home_lat) * 0.0174532925;
-    double scaleLongDown = cos(rads);
-    double scaleLongUp   = 1.0f/cos(rads);
-
-    //DST to Home
-    dstlat = fabs(osd_home_lat - osd_lat) * 111319.5;
-    dstlon = fabs(osd_home_lon - osd_lon) * 111319.5 * scaleLongDown;
-    osd_home_distance = sqrt(sq(dstlat) + sq(dstlon));
-
-    //DIR to Home
-    dstlon = (osd_home_lon - osd_lon); //OffSet_X
-    dstlat = (osd_home_lat - osd_lat) * scaleLongUp; //OffSet Y
-    bearing = 90 + (atan2(dstlat, -dstlon) * 57.295775); //absolut home direction
-    if(bearing < 0) bearing += 360;//normalization
-    bearing = bearing - 180;//absolut return direction
-    if(bearing < 0) bearing += 360;//normalization
-    bearing = bearing - osd_heading;//relative home direction
-    if(bearing < 0) bearing += 360; //normalization
-    osd_home_direction = round((float)(bearing/360.0f) * 16.0f) + 1;//array of arrows =)
-    if(osd_home_direction > 16) osd_home_direction = 0;
-
-  }
-
-}
-
-void setFdataVars(){
-
-  if (haltset == 1 && takeofftime == 0 && osd_alt_to_home > 5 && osd_throttle > 10){
-    takeofftime = 1;
-    tdistance = 0;
-    FTime = (millis()/1000);
-  }
-  
-//  if ((millis() - dt) >= 1000){
-//    if (osd_groundspeed > 1.0) tdistance = tdistance + (((millis() - dt) / 1000) * osd_groundspeed); 
-//  dt = millis();
-//  }
-
-  if (osd_groundspeed > 1.0) tdistance += (osd_groundspeed * (millis() - runt) / 1000.0);
-  mah_used += (osd_curr_A * 10.0 * (millis() - runt) / 3600000.0);
-  runt = millis();
-    
-  if (takeofftime == 1){
-    if (osd_home_distance > max_home_distance) max_home_distance = osd_home_distance;
-    if (osd_airspeed > max_osd_airspeed) max_osd_airspeed = osd_airspeed;
-    if (osd_groundspeed > max_osd_groundspeed) max_osd_groundspeed = osd_groundspeed;
-    if (osd_alt_to_home > max_osd_home_alt) max_osd_home_alt = osd_alt_to_home;
-    if (osd_windspeed > max_osd_windspeed) max_osd_windspeed = osd_windspeed;
-  }
-}
+

+//------------------ Heading and Compass ----------------------------------------

+

+static char buf_show[12];

+const char buf_Rule[36] = {0x82,0x80,0x81,0x80,0x81,0x80,

+                           0x84,0x80,0x81,0x80,0x81,0x80,

+                           0x83,0x80,0x81,0x80,0x81,0x80,

+                           0x85,0x80,0x81,0x80,0x81,0x80};

+void setHeadingPatern()

+{

+  int start;

+  start = round((osd_heading * 24)/360);

+  start -= 3;

+  if(start < 0) start += 24;

+  for(int x=0; x <= 10; x++){

+    buf_show[x] = buf_Rule[start];

+    if(++start > 23) start = 0;

+  }

+  buf_show[7] = '\0';

+}

+

+//------------------ Battery Remaining Picture ----------------------------------

+

+char setBatteryPic(uint16_t bat_level)

+{

+  if(bat_level <= 100){

+    return 0xb4;

+  }

+  else if(bat_level <= 300){

+    return 0xb5;

+  }

+  else if(bat_level <= 400){

+    return 0xb6;

+  }

+  else if(bat_level <= 500){

+    return 0xb7;

+  }

+  else if(bat_level <= 800){

+    return 0xb8;

+  }

+  else return 0xb9;

+}

+

+//------------------ Home Distance and Direction Calculation ----------------------------------

+

+void setHomeVars(OSD &osd)

+{

+  float dstlon, dstlat;

+  long bearing;

+  

+//  if(osd_throttle > 3 && takeoff_heading == -400)

+//    takeoff_heading = osd_heading;

+  osd_alt_to_home = (osd_alt - osd_home_alt);

+  if(osd_got_home == 0 && osd_fix_type == 3){

+    osd_home_lat = osd_lat;

+    osd_home_lon = osd_lon;

+    //osd_home_alt = osd_alt;

+    osd_got_home = 1;

+  }

+  else if(osd_got_home == 1){

+    // JRChange: osd_home_alt: check for stable osd_alt (must be stable for 25*120ms = 3s)

+//    if(osd_alt_cnt < 25){

+//      if(fabs(osd_alt_prev - osd_alt) > 0.5){

+//        osd_alt_cnt = 0;

+//        osd_alt_prev = osd_alt;

+//      }

+//      else

+//      {

+//        if(++osd_alt_cnt >= 25){

+//          osd_home_alt = osd_alt;  // take this stable osd_alt as osd_home_alt

+//          haltset = 1;

+//        }

+//      }

+//    }

+    // shrinking factor for longitude going to poles direction

+    float rads = fabs(osd_home_lat) * 0.0174532925;

+    double scaleLongDown = cos(rads);

+    double scaleLongUp   = 1.0f/cos(rads);

+

+    //DST to Home

+    dstlat = fabs(osd_home_lat - osd_lat) * 111319.5;

+    dstlon = fabs(osd_home_lon - osd_lon) * 111319.5 * scaleLongDown;

+    osd_home_distance = sqrt(sq(dstlat) + sq(dstlon));

+

+    //DIR to Home

+    dstlon = (osd_home_lon - osd_lon); //OffSet_X

+    dstlat = (osd_home_lat - osd_lat) * scaleLongUp; //OffSet Y

+    bearing = 90 + (atan2(dstlat, -dstlon) * 57.295775); //absolut home direction

+    if(bearing < 0) bearing += 360;//normalization

+    bearing = bearing - 180;//absolut return direction

+    if(bearing < 0) bearing += 360;//normalization

+    bearing = bearing - osd_heading;//relative home direction

+    if(bearing < 0) bearing += 360; //normalization

+    osd_home_direction = round((float)(bearing/360.0f) * 16.0f) + 1;//array of arrows =)

+    if(osd_home_direction > 16) osd_home_direction = 0;

+

+  }

+

+}

+

+void setFdataVars(){

+

+  if (takeofftime == 0 && osd_alt_to_home > 5 && osd_throttle > 10){

+    takeofftime = 1;

+    tdistance = 0;

+    FTime = (millis()/1000);

+  }

+  

+//  if ((millis() - dt) >= 1000){

+//    if (osd_groundspeed > 1.0) tdistance = tdistance + (((millis() - dt) / 1000) * osd_groundspeed); 

+//  dt = millis();

+//  }

+

+  if (osd_groundspeed > 1.0) tdistance += (osd_groundspeed * (millis() - runt) / 1000.0);

+  mah_used += (osd_curr_A * 10.0 * (millis() - runt) / 3600000.0);

+  runt = millis();

+    

+  if (takeofftime == 1){

+    if (osd_home_distance > max_home_distance) max_home_distance = osd_home_distance;

+    if (osd_airspeed > max_osd_airspeed) max_osd_airspeed = osd_airspeed;

+    if (osd_groundspeed > max_osd_groundspeed) max_osd_groundspeed = osd_groundspeed;

+    if (osd_alt_to_home > max_osd_home_alt) max_osd_home_alt = osd_alt_to_home;

+    if (osd_windspeed > max_osd_windspeed) max_osd_windspeed = osd_windspeed;

+  }

+

+}

+

+void checkModellType(){

+if (EEPROM.read(MODELL_TYPE_ADD) != 0) EEPROM.write(MODELL_TYPE_ADD, 0);

+if (EEPROM.read(FW_VERSION1_ADDR) != 2) EEPROM.write(FW_VERSION1_ADDR, 2);

+if (EEPROM.read(FW_VERSION2_ADDR) != 4) EEPROM.write(FW_VERSION2_ADDR, 4);

+if (EEPROM.read(FW_VERSION3_ADDR) != 1) EEPROM.write(FW_VERSION3_ADDR, 1);

+}

 

 

--- a/OSD_Panels.ino
+++ b/OSD_Panels.ino
@@ -1,8 +1,6 @@
 /******* STARTUP PANEL *******/
 
 void startPanels(){
-//    osd.clear();
-    //osd_clear = 3;
     panLogo(); // Display our logo  
     do_converts(); // load the unit conversion preferences
 }
@@ -12,7 +10,7 @@
 void panLogo(){
     osd.setPanel(5, 5);
     osd.openPanel();
-    osd.printf_P(PSTR("\xb0\xb1\xb2\xb3\xb4|\xb5\xb6\xb7\xb8\xb9|MinimOSD-Extra 2.4|Plane r628"));
+    osd.printf_P(PSTR("MinimOSD-FrSky 2.4|r780"));
     osd.closePanel();
 }
 
@@ -20,103 +18,95 @@
 /******* PANELS - POSITION *******/
 
 void writePanels(){ 
-
-  if(millis() < (lastMAVBeat + 2200)){
-  
-    if (osd_alt_to_home <= 10 && osd_groundspeed <= 1 && osd_throttle <= 1 && takeofftime == 1 && osd_home_distance <= 100) {
-      landing = 1;
-          }else{
-          landed = millis();
-          }
-    if (landing == 1 && (millis() - 7000) > landed){ 
-       if (osd_clear == 0){
-         osd.clear(); 
-         osd_clear = 1;
-         panFdata();
-       }
-//        if ((millis() - 10000) > landing) panFdata();     
-        }else{
-//         landed = millis();
-         
-         if (osd_clear == 1){
-         osd.clear(); 
-         osd_clear = 0;                    
-       }
-      if(ch_toggle > 3) panOff(); // This must be first so you can always toggle
-            if(panel != npanels)
-            {
-                
-                //Testing bits from 8 bit register A 
- //               if(ISa(panel,Cen_BIT)) panCenter(panCenter_XY[0][panel], panCenter_XY[1][panel]);   //4x2
-                if(ISd(panel,Warn_BIT)) panWarn(panWarn_XY[0][panel], panWarn_XY[1][panel]); // this must be here so warnings are always checked
-                if(ISa(panel,Pit_BIT)) panPitch(panPitch_XY[0][panel], panPitch_XY[1][panel]); //5x1
-                if(ISa(panel,Rol_BIT)) panRoll(panRoll_XY[0][panel], panRoll_XY[1][panel]); //5x1
-                if(ISa(panel,BatA_BIT)) panBatt_A(panBatt_A_XY[0][panel], panBatt_A_XY[1][panel]); //7x1
-                //  if(ISa(panel,BatB_BIT)) panBatt_B(panBatt_B_XY[0], panBatt_B_XY[1][panel]); //7x1
-                if(ISa(panel,GPSats_BIT)) panGPSats(panGPSats_XY[0][panel], panGPSats_XY[1][panel]); //5x1
-                if(ISa(panel,GPS_BIT)) panGPS(panGPS_XY[0][panel], panGPS_XY[1][panel]); //12x3
-                if(ISa(panel,Bp_BIT)) panBatteryPercent(panBatteryPercent_XY[0][panel], panBatteryPercent_XY[1][panel]); //
-                if(ISa(panel,COG_BIT)) panCOG(panCOG_XY[0][panel], panCOG_XY[1][panel]); //
-
-                //Testing bits from 8 bit register B
-                if(ISb(panel,Rose_BIT)) panRose(panRose_XY[0][panel], panRose_XY[1][panel]);        //13x3
-                if(ISb(panel,Head_BIT)) panHeading(panHeading_XY[0][panel], panHeading_XY[1][panel]); //13x3
- //               if(ISb(panel,MavB_BIT)) panMavBeat(panMavBeat_XY[0][panel], panMavBeat_XY[1][panel]); //13x3
-
-                if(osd_got_home == 1){
-                    if(ISb(panel,HDis_BIT)) panHomeDis(panHomeDis_XY[0][panel], panHomeDis_XY[1][panel]); //13x3
-                    if(ISb(panel,HDir_BIT)) panHomeDir(panHomeDir_XY[0][panel], panHomeDir_XY[1][panel]); //13x3
-                }
-
-                if(ISb(panel,Time_BIT)) panTime(panTime_XY[0][panel], panTime_XY[1][panel]);
- //               if(ISb(panel,WDir_BIT)) panWPDir(panWPDir_XY[0][panel], panWPDir_XY[1][panel]); //??x??
-                if(wp_number > 0){
-                if(ISb(panel,WDis_BIT)) panWPDis(panWPDis_XY[0][panel], panWPDis_XY[1][panel]); //??x??
-                }
-                //Testing bits from 8 bit register C 
-                //if(osd_got_home == 1){
-                if(ISc(panel,Alt_BIT)) panAlt(panAlt_XY[0][panel], panAlt_XY[1][panel]); //
-                if(ISc(panel,Halt_BIT)) panHomeAlt(panHomeAlt_XY[0][panel], panHomeAlt_XY[1][panel]); //
-                if(ISc(panel,Vel_BIT)) panVel(panVel_XY[0][panel], panVel_XY[1][panel]); //
-                if(ISc(panel,As_BIT)) panAirSpeed(panAirSpeed_XY[0][panel], panAirSpeed_XY[1][panel]); //
-
-                //}
-                if(ISc(panel,Thr_BIT)) panThr(panThr_XY[0][panel], panThr_XY[1][panel]); //
-                if(ISc(panel,FMod_BIT)) panFlightMode(panFMod_XY[0][panel], panFMod_XY[1][panel]);  //
-                if(ISc(panel,Hor_BIT)) panHorizon(panHorizon_XY[0][panel], panHorizon_XY[1][panel]); //14x5
-                if(ISc(panel,CurA_BIT)) panCur_A(panCur_A_XY[0][panel], panCur_A_XY[1][panel]);
-
-                //Testing bits from 8 bit register D 
-                //if(ISd(Off_BIT)) panOff(panOff_XY[0], panOff_XY[1]);
-                if(ISd(panel,WindS_BIT)) panWindSpeed(panWindSpeed_XY[0][panel], panWindSpeed_XY[1][panel]);
-                if(ISd(panel,Climb_BIT)) panClimb(panClimb_XY[0][panel], panClimb_XY[1][panel]);
-//                if(ISd(panel,Tune_BIT)) panTune(panTune_XY[0][panel], panTune_XY[1][panel]);
-                if(ISd(panel,RSSI_BIT)) panRSSI(panRSSI_XY[0][panel], panRSSI_XY[1][panel]); //??x??
-                if(ISd(panel,Eff_BIT)) panEff(panEff_XY[0][panel], panEff_XY[1][panel]);
-                if(ISd(panel,CALLSIGN_BIT)) panCALLSIGN(panCALLSIGN_XY[0][panel], panCALLSIGN_XY[1][panel]);
-                if(ISe(panel,TEMP_BIT)) panTemp(panTemp_XY[0][panel], panTemp_XY[1][panel]);
-//                if(ISe(panel,Ch_BIT)) panCh(panCh_XY[0][panel], panCh_XY[1][panel]);
-                if(ISe(panel,DIST_BIT)) panDistance(panDistance_XY[0][panel], panDistance_XY[1][panel]);
-            } else { //panel == npanels
-//                if(ISd(0,Warn_BIT)) panWarn(panWarn_XY[0][0], panWarn_XY[1][0]); // this must be here so warnings are always checked
-                if(ISd(0,CALLSIGN_BIT)) panCALLSIGN(panCALLSIGN_XY[0][panel], panCALLSIGN_XY[1][panel]); //call sign even in off panel
-            }
- //       } else { // if (osd_on > 0)
- //           panSetup();
- //       }
-        }
-    //}
-   } else { // if no mavlink update for 2 secs
-    
-        // this could be replaced with a No Mavlink warning so the last seen values still show
-
-        osd.clear();
-        waitingMAVBeats = 1;
-        // Display our logo and wait... 
-    //    panWaitMAVBeats(5,10); //Waiting for MAVBeats...
-    panLogo();
+  if ((takeofftime == 1) && (osd_alt_to_home > 10 || osd_groundspeed > 1 || osd_throttle > 1 || osd_home_distance > 100)){
+    landed = millis();
   }
-  
+
+  //Base panel selection
+  //No mavlink data available panel
+  if(millis() > (lastMAVBeat + 2200)){
+    if (currentBasePanel != 2){
+      osd.clear();
+      currentBasePanel = 2;
+    }   
+    //panLogo();
+    //waitingMAVBeats = 1;
+    //Display our logo and wait... 
+    panWaitMAVBeats(5,10); //Waiting for MAVBeats...
+  }
+  //Flight summary panel
+  //Only show flight summary 7 seconds after landing
+  else if ((millis() - 7000) > landed){ 
+    if (currentBasePanel != 1){
+      osd.clear();
+      currentBasePanel = 1;
+    }
+    panFdata(); 
+  }
+  //Normal osd panel
+  else{
+    if(ISd(0,Warn_BIT)) panWarn(panWarn_XY[0][0], panWarn_XY[1][0]); // this must be here so warnings are always checked
+    //Check for panel toggle
+    if(ch_toggle > 3) panOff(); // This must be first so you can always toggle
+    if ((osd_clear == 1) || (currentBasePanel != 0)){
+      osd.clear();
+      osd_clear = 0;
+      currentBasePanel = 0;
+    }
+    if(panel != npanels)
+    {
+      //Testing bits from 8 bit register A 
+      //if(ISa(panel,Cen_BIT)) panCenter(panCenter_XY[0][panel], panCenter_XY[1][panel]);   //4x2
+      //if(ISd(panel,Warn_BIT)) panWarn(panWarn_XY[0][panel], panWarn_XY[1][panel]); // this must be here so warnings are always checked
+      if(ISa(panel,Pit_BIT)) panPitch(panPitch_XY[0][panel], panPitch_XY[1][panel]); //5x1
+      if(ISa(panel,Rol_BIT)) panRoll(panRoll_XY[0][panel], panRoll_XY[1][panel]); //5x1
+      if(ISa(panel,BatA_BIT)) panBatt_A(panBatt_A_XY[0][panel], panBatt_A_XY[1][panel]); //7x1
+      //if(ISa(panel,BatB_BIT)) panBatt_B(panBatt_B_XY[0], panBatt_B_XY[1][panel]); //7x1
+      if(ISa(panel,GPSats_BIT)) panGPSats(panGPSats_XY[0][panel], panGPSats_XY[1][panel]); //5x1
+      if(ISa(panel,GPS_BIT)) panGPS(panGPS_XY[0][panel], panGPS_XY[1][panel]); //12x3
+      if(ISa(panel,Bp_BIT)) panBatteryPercent(panBatteryPercent_XY[0][panel], panBatteryPercent_XY[1][panel]); //
+      if(ISa(panel,COG_BIT)) panCOG(panCOG_XY[0][panel], panCOG_XY[1][panel]); //
+
+      //Testing bits from 8 bit register B
+      if(ISb(panel,Rose_BIT)) panRose(panRose_XY[0][panel], panRose_XY[1][panel]);        //13x3
+      if(ISb(panel,Head_BIT)) panHeading(panHeading_XY[0][panel], panHeading_XY[1][panel]); //13x3
+      //if(ISb(panel,MavB_BIT)) panMavBeat(panMavBeat_XY[0][panel], panMavBeat_XY[1][panel]); //13x3
+
+      if(osd_got_home == 1){
+        if(ISb(panel,HDis_BIT)) panHomeDis(panHomeDis_XY[0][panel], panHomeDis_XY[1][panel]); //13x3
+        if(ISb(panel,HDir_BIT)) panHomeDir(panHomeDir_XY[0][panel], panHomeDir_XY[1][panel]); //13x3
+      }
+      
+      if(ISb(panel,Time_BIT)) panTime(panTime_XY[0][panel], panTime_XY[1][panel]);
+      //if(ISb(panel,WDir_BIT)) panWPDir(panWPDir_XY[0][panel], panWPDir_XY[1][panel]); //??x??
+      if(wp_number > 0){
+        if(ISb(panel,WDis_BIT)) panWPDis(panWPDis_XY[0][panel], panWPDis_XY[1][panel]); //??x??
+      }
+      //Testing bits from 8 bit register C 
+      if(ISc(panel,Alt_BIT)) panAlt(panAlt_XY[0][panel], panAlt_XY[1][panel]); //
+      if(ISc(panel,Halt_BIT)) panHomeAlt(panHomeAlt_XY[0][panel], panHomeAlt_XY[1][panel]); //
+      if(ISc(panel,Vel_BIT)) panVel(panVel_XY[0][panel], panVel_XY[1][panel]); //
+      if(ISc(panel,As_BIT)) panAirSpeed(panAirSpeed_XY[0][panel], panAirSpeed_XY[1][panel]); //
+
+      if(ISc(panel,Thr_BIT)) panThr(panThr_XY[0][panel], panThr_XY[1][panel]); //
+      if(ISc(panel,FMod_BIT)) panFlightMode(panFMod_XY[0][panel], panFMod_XY[1][panel]);  //
+      if(ISc(panel,Hor_BIT)) panHorizon(panHorizon_XY[0][panel], panHorizon_XY[1][panel]); //14x5
+      if(ISc(panel,CurA_BIT)) panCur_A(panCur_A_XY[0][panel], panCur_A_XY[1][panel]);
+
+      //Testing bits from 8 bit register D 
+      if(ISd(panel,WindS_BIT)) panWindSpeed(panWindSpeed_XY[0][panel], panWindSpeed_XY[1][panel]);
+      if(ISd(panel,Climb_BIT)) panClimb(panClimb_XY[0][panel], panClimb_XY[1][panel]);
+      if(ISd(panel,Tune_BIT)) panTune(panTune_XY[0][panel], panTune_XY[1][panel]);
+      if(ISd(panel,RSSI_BIT)) panRSSI(panRSSI_XY[0][panel], panRSSI_XY[1][panel]); //??x??
+      if(ISd(panel,Eff_BIT)) panEff(panEff_XY[0][panel], panEff_XY[1][panel]);
+      if(ISd(panel,CALLSIGN_BIT)) panCALLSIGN(panCALLSIGN_XY[0][panel], panCALLSIGN_XY[1][panel]);
+      if(ISe(panel,TEMP_BIT)) panTemp(panTemp_XY[0][panel], panTemp_XY[1][panel]);
+      //if(ISe(panel,Ch_BIT)) panCh(panCh_XY[0][panel], panCh_XY[1][panel]);
+      if(ISe(panel,DIST_BIT)) panDistance(panDistance_XY[0][panel], panDistance_XY[1][panel]);
+    }
+  }
+  if(ISd(0,CALLSIGN_BIT)) panCALLSIGN(panCALLSIGN_XY[0][panel], panCALLSIGN_XY[1][panel]); //call sign even in off panel
+  timers();
     // OSD debug for development (Shown on top-middle panels) 
 #ifdef membug
     osd.setPanel(13,4);
@@ -130,11 +120,13 @@
 /******* PANELS - DEFINITION *******/
 
 /* **************************************************************** */
-// Panel  : COG Course Over Ground
+
+ // Panel  : COG Course Over Ground
 // Needs  : X, Y locations
 // Output : 
 // Size   : 
 // Staus  : done
+
 
 void panCOG(int first_col, int first_line){
     osd.setPanel(first_col, first_line);
@@ -143,9 +135,9 @@
 //    osd_COG_arrow_rotate_int = (((int)osd_cog / 100 - (int)osd_heading)/360 * 16 + 16) % 16 + 1; // [1, 16]
 //    osd_COG_arrow_rotate_int = (osd_cog / 100 - osd_heading) / 360 * 16 + 1;
     osd_COG_arrow_rotate_int = round(((osd_cog / 100) - osd_heading)/360.0 * 16.0 +1); //Convert to int 1-16 
-    if(osd_COG_arrow_rotate_int < 0 ) osd_COG_arrow_rotate_int += 16;
-    if(osd_COG_arrow_rotate_int == 0) osd_COG_arrow_rotate_int = 16;    
-    if(osd_COG_arrow_rotate_int == 17) osd_COG_arrow_rotate_int = 1;
+    if(osd_COG_arrow_rotate_int < 1 ) osd_COG_arrow_rotate_int += 16;
+//    if(osd_COG_arrow_rotate_int == 0) osd_COG_arrow_rotate_int = 16;    
+    else if(osd_COG_arrow_rotate_int > 16) osd_COG_arrow_rotate_int -= 16;
     
     if (((osd_cog / 100) - osd_heading) > 180){
        off_course = (osd_cog / 100 - osd_heading) - 360;
@@ -157,12 +149,13 @@
     
     showArrow((uint8_t)osd_COG_arrow_rotate_int,2);
 
-    osd.closePanel();
-}
-
-
-/* **************************************************************** */
-// Panel  : ODO
+
+    osd.closePanel();
+}
+
+
+/* **************************************************************** */
+// Panel  : panDistance
 // Needs  : X, Y locations
 // Output : 
 // Size   : 1 x 7Hea  (rows x chars)
@@ -172,10 +165,10 @@
     osd.setPanel(first_col, first_line);
     osd.openPanel();
     //do_converts();
-    if ((tdistance * converth) > 1000.0) {
-    osd.printf("%c%5.2f%c", 0x8f, ((tdistance * converth) / distconv), distchar);
+    if ((tdistance * converth) > 9999.0) {
+      osd.printf("%c%5.2f%c", 0x8f, ((tdistance * converth) / distconv), distchar);
     }else{
-    osd.printf("%c%5.0f%c", 0x8f, (tdistance * converth), high);
+      osd.printf("%c%5.0f%c", 0x8f, (tdistance * converth), high);
     }
     osd.closePanel();
 }
@@ -190,7 +183,7 @@
      osd.setPanel(11, 3);
     osd.openPanel();                          
 //    osd.printf("%c%3i%c%02i|%c%5.0f%c|%c%5.0f%c|%c%5.0f%c|%c%5.0f%c|%c%5.0f%c|%c%5.0f%c", 0x08,((int)start_Time/60)%60,0x3A,(int)start_Time%60, 0x0b, ((max_home_distance) * converth), high, 0x1b, ((tdistance) * converth), high, 0x13,(max_osd_airspeed * converts), spe,0x14,(max_osd_groundspeed * converts),spe,0x12, (max_osd_home_alt * converth), high,0x1d,(max_osd_windspeed * converts),spe);
-    osd.printf("%c%3i%c%02i|%c%5i%c|%c%5i%c|%c%5i%c|%c%5i%c|%c%5i%c|%c%5i%c|%c%5.0f%c|%c%10.6f|%c%10.6f", 0x08,((int)start_Time/60)%60,0x3A,(int)start_Time%60, 0x0b, (int)((max_home_distance) * converth), high, 0x8f, (int)((tdistance) * converth), high, 0x13,(int)(max_osd_airspeed * converts), spe,0x14,(int)(max_osd_groundspeed * converts),spe,0x12, (int)(max_osd_home_alt * converth), high,0x1d,(int)(max_osd_windspeed * converts),spe, 0x17, mah_used, 0x01, 0x03, (double)osd_lat, 0x04, (double)osd_lon);
+    osd.printf("%c%3i%c%02i|%c%5.0f%c|%c%5.0f%c|%c%5i%c|%c%5i%c|%c%5i%c|%c%5i%c|%c%5.0f%c|%c%11.6f|%c%11.6f", 0x08,((int)(start_Time/60)),0x3A,(int)start_Time%60, 0x0b, ((max_home_distance) * converth), high, 0x8f, (tdistance * converth), high, 0x13,(int)(max_osd_airspeed * converts), spe,0x14,(int)(max_osd_groundspeed * converts),spe,0x12, (int)(max_osd_home_alt * converth), high,0x1d,(int)(max_osd_windspeed * converts),spe, 0x17, mah_used, 0x01, 0x03, (double)osd_lat, 0x04, (double)osd_lon);
     osd.closePanel();
 }
 
@@ -204,7 +197,8 @@
 void panTemp(int first_col, int first_line){
     osd.setPanel(first_col, first_line);
     osd.openPanel();
-    osd.printf("%c%5.1f%c", 0x0a, (float(temperature * tempconv + tempconvAdd) / 100), temps);
+//    osd.printf("%c%5.1f%c", 0x0a, (float(temperature * tempconv + tempconvAdd) / 100), temps);
+    osd.printf("%5.1f%c", (float(temperature * tempconv + tempconvAdd) / 1000), temps);
     osd.closePanel();
 }
 
@@ -222,12 +216,12 @@
       if (ma == 0) {
               ma = 1;
             }
-        if (osd_groundspeed != 0) eff = (float(osd_curr_A * 10) / (osd_groundspeed * converts))* 0.5 + eff * 0.5;
+        if (osd_groundspeed != 0) eff = (float(osd_curr_A * 10.0) / (osd_groundspeed * converts))* 0.1 + eff * 0.9;
 //        eff = eff * 0.2 + eff * 0.8;
           if (eff > 0 && eff <= 9999) {
-            osd.printf("%c%4.0f%c", 0x16, (double)eff, 0x01);
+            osd.printf("%c%4.0f%c", 0x16, eff, 0x01);
           }else{
-          osd.printf_P(PSTR("\x16\x20\x20\x20\x20\x20")); 
+          osd.printf_P(PSTR("\x16\x20\x20\x20\x20\x20"));
           }
           
     }else{
@@ -242,13 +236,13 @@
           }
             if (osd_climb < -0.05){ 
 //            glide = ((osd_alt_to_home / (palt - osd_alt_to_home)) * ((millis() - descendt) / 1000)) * osd_groundspeed;
-            glide = (osd_alt_to_home / (palt - osd_alt_to_home)) * (tdistance - ddistance);
+            glide = ((osd_alt_to_home / (palt - osd_alt_to_home)) * (tdistance - ddistance)) * converth;
             if (glide > 9999) glide = 9999;
              if (glide != 'inf' && glide > -0){
-            osd.printf("%c%4.0f%c", 0x18, glide, 0x0c);
+            osd.printf("%c%4.0f%c", 0x18, glide, high);
              }
             }
-            else if (osd_climb > 0.0 && osd_pitch <= 0) {
+            else if (osd_climb >= -0.05 && osd_pitch < 0) {
               osd.printf_P(PSTR("\x18\x20\x20\x90\x91\x20"));   
             }else{
               osd.printf_P(PSTR("\x18\x20\x20\x20\x20\x20")); 
@@ -285,11 +279,14 @@
 void panRSSI(int first_col, int first_line){
     osd.setPanel(first_col, first_line);
     osd.openPanel();
-    rssi = (int16_t)osd_rssi;
-    //if (rssi > rssical) rssi = rssical;
-    //else if (rssi < rssipersent) rssi = rssipersent;
-
-    if(!rssiraw_on) rssi = (int16_t)((float)(rssi - rssipersent)/(float)(rssical-rssipersent)*100.0f);
+    
+    if(rssiraw_on == 0) rssi = (int16_t)((float)((int16_t)osd_rssi - rssipersent)/(float)(rssical-rssipersent)*100.0f);
+    if(rssiraw_on == 1) rssi = (int16_t)osd_rssi;
+    
+    if(rssiraw_on == 8) rssi = (int16_t)((float)(chan8_raw / 10 - rssipersent)/(float)(rssical-rssipersent)*100.0f);
+    if(rssiraw_on == 9) rssi = chan8_raw;
+
+    
 //    if (rssi < -99) rssi = -99;
     osd.printf("%c%3i%c", 0x09, rssi, 0x25);
 //    osd.printf("%c%3i%c", 0x09, osd_clear, 0x25); 
@@ -306,25 +303,14 @@
 void panCALLSIGN(int first_col, int first_line){
     osd.setPanel(first_col, first_line);
     osd.openPanel();
-    //osd.printf("%c%c%c%c%c%c", char_call[0], char_call[1], char_call[2], char_call[3], char_call[4], char_call[5]); 
-    //During the first 1000 miliseconds of each minute show callsign
-//    if(millis() <= 1000){
-//      osd.printf("%s", char_call); 
-//    }else{
-//    osd.printf_P(PSTR("\x20\x20\x20\x20\x20\x20\x20\x20"));
-//      osd.printf("%s",strclear);
-    
-//    if(millis() % 60000
-    if ((millis() - 60000) > CallSignBlink){
-      if (millis() - 61000 > CallSignBlink){
-        CallSignBlink = millis();
-          }
-      osd.printf("%s", char_call); 
+    if(((millis() / 1000) % 60) < 2){
+      osd.printf("%s", char_call);
     }else{
-//    osd.printf_P(PSTR("\x20\x20\x20\x20\x20\x20\x20\x20"));
       osd.printf("%s",strclear);
-    }
-    osd.closePanel();
+//osd.printf_P(PSTR("\x20\x20\x20\x20\x20\x20\x20\x20"));
+    }
+    osd.closePanel();
+
 }
 
 /* **************************************************************** */
@@ -428,7 +414,7 @@
     osd_wind_arrow_rotate_int = round((osd_winddirection - osd_heading)/360.0 * 16.0) + 9; //Convert to int 1-16
     }
     if(osd_wind_arrow_rotate_int > 16 ) osd_wind_arrow_rotate_int -= 16; //normalize
-    if(osd_wind_arrow_rotate_int < 1 ) osd_wind_arrow_rotate_int += 16; //normalize
+    else if(osd_wind_arrow_rotate_int < 1 ) osd_wind_arrow_rotate_int += 16; //normalize
     nor_osd_windspeed = osd_windspeed * 0.010 + nor_osd_windspeed * 0.990;    
     
     showArrow((uint8_t)osd_wind_arrow_rotate_int,1); //print data to OSD
@@ -443,91 +429,77 @@
 // Staus  : done
 
 void panOff(){
+  bool rotatePanel = 0;
+
+  if(ch_toggle == 5) ch_raw = chan5_raw;
+  else if(ch_toggle == 6) ch_raw = chan6_raw;
+  else if(ch_toggle == 7) ch_raw = chan7_raw;
+  else if(ch_toggle == 8) ch_raw = chan8_raw;
+
+  //If there is a warning force switch to panel 0
+  if(canswitch == 0){
+    if(panel != panel_auto_switch){
+      //osd.clear();
+      osd_clear = 1;
+    }
+    panel = panel_auto_switch; 
+  }
+  else{
+    //Flight mode switching
     if (ch_toggle == 4){
-        if ((osd_mode != 11) && (osd_mode != 1)){
-            if (osd_off_switch != osd_mode){ 
-                osd_off_switch = osd_mode;
-                osd_switch_time = millis();
-
-                if (osd_off_switch == osd_switch_last){
-                    if(panel == 0)
-                        {
-                            panel = 1;                                                        
-//                            if (millis() <= 60000){
-//                                osd_set = 1;
-//                            }else{
-//                                osd_set = 0;
-//                            }                            
-                        }
-                    else if(panel == 1)
-                        {
-                            panel = npanels;
-//                            osd_set = 0;                            
-                        }
-                    else if(panel == npanels)
-                        {
-                            panel = 0;
-                        }
-                    osd.clear();
-                }
-            }
-            if ((millis() - osd_switch_time) > 2000){
-                osd_switch_last = osd_mode;
+      if ((osd_mode != 6) && (osd_mode != 7)){
+        if (osd_off_switch != osd_mode){ 
+          osd_off_switch = osd_mode;
+            osd_switch_time = millis();
+            if (osd_off_switch == osd_switch_last){
+              rotatePanel = 1;
             }
         }
+        if ((millis() - osd_switch_time) > 2000){
+          osd_switch_last = osd_mode;
+        }
+      }
     }
     else {
-        if(ch_toggle == 5) ch_raw = chan5_raw;
-        else if(ch_toggle == 6) ch_raw = chan6_raw;
-        else if(ch_toggle == 7) ch_raw = chan7_raw;
-        else if(ch_toggle == 8) ch_raw = chan8_raw;
-
-        if (switch_mode == 0){
-            if (ch_raw > 1800 && warning != 1) {
-//                if (millis() <= 60000){
-//                    osd_set = 1;
-//                }
-//                else if (osd_set != 1 && warning != 1){
-//              if (warning != 1){
-                osd.clear();
-//                }
-                panel = npanels; //off panel
-            }
-            else if (ch_raw < 1200 && panel != 0) { //first panel
- //               osd_set = 0;
-                osd.clear();
-                panel = 0;
-            }    
-
- // else if (ch_raw >= 1200 && ch_raw <= 1800 && setup_menu != 6 && panel != 1 && warning != 1) { //second panel
-    else if (ch_raw >= 1200 && ch_raw <= 1800 && panel != 1 && warning != 1) { //second panel
- //               osd_set = 0;
-                osd.clear();
-                panel = 1;
-            }        
-        } else {
-
-            if (ch_raw > 1200)
- //               if (millis() <= 60000 && osd_set != 1){
- //                   if (osd_switch_time + 1000 < millis()){
- //                       osd_set = 1;
- //                       osd_switch_time = millis();
- //                   }
- //               } else {
-                    if (osd_switch_time + 1000 < millis()){
- //                       osd_set = 0;
-                        osd.clear();
-                        if (panel == npanels) {
-                            panel = 0;
-                        } else {
-                            panel++;
-                        }
-                        if (panel > 1) panel = npanels;
-                        osd_switch_time = millis();
-                    }
-  //              }
-        }    
-    }
+      
+      //Switch mode by value
+      if (switch_mode == 0){
+        //First panel
+        if (ch_raw < 1233 && panel != 0) {
+          osd_clear = 1;
+          //osd.clear();
+          panel = 0;
+        }
+        //Second panel
+        else if (ch_raw >= 1233 && ch_raw <= 1467 && panel != 1) { //second panel
+          osd_clear = 1;
+          //osd.clear();
+          panel = 1;
+        }
+        //Panel off
+        else if (ch_raw > 1467 && panel != npanels) {
+          osd_clear = 1;
+          //osd.clear();
+          panel = npanels; //off panel
+        }
+      }
+      //Rotation switch
+      else{
+        if (ch_raw > 1200)
+          if (osd_switch_time + 1000 < millis()){
+            rotatePanel = 1;
+            osd_switch_time = millis();
+        }
+      }    
+    }
+    if(rotatePanel == 1){
+      osd_clear = 1;
+      //osd.clear();
+      panel++;
+      if (panel > npanels)
+        panel = 0;
+    }
+  }
 }
 //* **************************************************************** */
 // Panel  : panTune
@@ -536,14 +508,14 @@
 // Size   : 1 x 7Hea  (rows x chars)
 // Staus  : done
     
-//  void panTune(int first_col, int first_line){
-//  osd.setPanel(first_col, first_line);
-//  osd.openPanel();
-
-//  osd.printf("%c%c%2.0f%c|%c%c%2.0f%c|%c%c%4.0i%c|%c%c%4.0i%c|%c%c%3.0f%c|%c%c%3.0f%c|%c%c%3.0f%c", 0x4E, 0x52, (nav_roll), 0x05, 0x4E, 0x50, (nav_pitch), 0x05, 0x4E, 0x48, (nav_bearing), 0x05, 0x54, 0x42, (wp_target_bearing), 0x05, 0x41, 0x45, (alt_error * converth), high, 0x58, 0x45, (xtrack_error), 0x6D, 0x41, 0x45, ((aspd_error / 100.0) * converts), spe);
-
-//  osd.closePanel();
-//}
+  void panTune(int first_col, int first_line){
+  osd.setPanel(first_col, first_line);
+  osd.openPanel();
+
+  osd.printf("%c%3.0f%c%c|%c%3.0f%c%c", 0xb0, (alt_error * converth), high, 0x20, 0xb1, ((aspd_error / 100.0) * converts), spe, 0x20);
+
+  osd.closePanel();
+}
 
 /* **************************************************************** */
 // Panel  : panCur_A
@@ -555,7 +527,12 @@
 void panCur_A(int first_col, int first_line){
     osd.setPanel(first_col, first_line);
     osd.openPanel();
-    osd.printf("%c%5.2f%c", 0xbd, (float(osd_curr_A) * 0.01), 0x0e);
+//    osd.printf("%c%5.2f%c", 0xbd, (float(osd_curr_A) * 0.01), 0x0e);
+#ifdef FRSKY
+    osd.printf("%c%5.2f%c", 0xbd, (double)osd_curr_A, 0x0e);
+#else
+    osd.printf("%5.2f%c", (float(osd_curr_A) * 0.01), 0x0e);
+#endif
     osd.closePanel();
 }
 
@@ -569,8 +546,14 @@
 void panAlt(int first_col, int first_line){
     osd.setPanel(first_col, first_line);
     osd.openPanel();
-    //osd.printf("%c%5.0f%c",0x85, (double)osd_alt_to_home, 0x0c);
-    osd.printf("%c%5.0f%c",0x11, (double)(osd_alt * converth), high);
+//    osd.printf("%c%5.0f%c",0x11, (double)(osd_alt * converth), high);
+//    if (iconMSL == 1) 
+    if(EEPROM.read(SIGN_MSL_ON_ADDR) != 0) osd.printf_P(PSTR("\x11"));
+#ifdef FRSKY    
+    osd.printf("%c%5.0f%c",0x11, (double)(osd_alt), high);
+#else
+    osd.printf("%5.0f%c", (double)(osd_alt * converth), high);
+#endif
     osd.closePanel();
 }
 
@@ -585,9 +568,8 @@
     osd.setPanel(first_col, first_line);
     osd.openPanel();
     vs = (osd_climb * converth * 60) * 0.1 + vs * 0.9;
-    osd.printf("%c%4.0f%c%c",0x15, int(vs / 10.0) * 10.0, climbchar, 0x20);
-    osd.closePanel();
-    
+    osd.printf("%c%4.0f%c%c", 0x15, int(vs / 10.0) * 10.0, climbchar, 0x20);
+    osd.closePanel();
 }
 
 /* **************************************************************** */
@@ -600,7 +582,10 @@
 void panHomeAlt(int first_col, int first_line){
     osd.setPanel(first_col, first_line);
     osd.openPanel();   
-    osd.printf("%c%5.0f%c",0x12, (double)(osd_alt_to_home * converth), high);
+//    osd.printf("%c%5.0f%c",0x12, (double)(osd_alt_to_home * converth), high);
+//    if (iconHA == 1) 
+    if(EEPROM.read(SIGN_HA_ON_ADDR) != 0) osd.printf_P(PSTR("\x12"));
+    osd.printf("%5.0f%c", (double)(osd_alt_to_home * converth), high);
     osd.closePanel();
 }
 
@@ -614,8 +599,10 @@
 void panVel(int first_col, int first_line){
     osd.setPanel(first_col, first_line);
     osd.openPanel();
-    
-    osd.printf("%c%3.0f%c",0x14,(double)(osd_groundspeed * converts),spe);
+//    osd.printf("%c%3.0f%c",0x14,(double)(osd_groundspeed * converts),spe);
+//    if (iconGS == 1) 
+    if(EEPROM.read(SIGN_GS_ON_ADDR) != 0) osd.printf_P(PSTR("\x14"));
+    osd.printf("%3.0f%c",(double)(osd_groundspeed * converts),spe);
     osd.closePanel();
 }
 
@@ -629,7 +616,10 @@
 void panAirSpeed(int first_col, int first_line){
     osd.setPanel(first_col, first_line);
     osd.openPanel();
-    osd.printf("%c%3.0f%c", 0x13, (double)(osd_airspeed * converts), spe);
+//    osd.printf("%c%3.0f%c", 0x13, (double)(osd_airspeed * converts), spe);
+//    if (iconAS == 1) 
+    if(EEPROM.read(SIGN_AS_ON_ADDR) != 0) osd.printf_P(PSTR("\x13"));        
+    osd.printf("%3.0f%c", (double)(osd_airspeed * converts), spe); 
     osd.closePanel();
 }
 
@@ -643,55 +633,99 @@
 void panWarn(int first_col, int first_line){
     osd.setPanel(first_col, first_line);
     osd.openPanel();
-
-    if (millis() > text_timer){ // if the text has been shown for a while
-        if (warning_type != 0) {
-            last_warning = warning_type; // save the warning type for cycling
-            warning_type = 0; // blank the text
-            warning = 1;
-            warning_timer = millis();            
-        } else {
-            if ((millis() - 10000) > warning_timer ) warning = 0;
-
-            int x = last_warning; // start the warning checks where we left it last time
-            while (warning_type == 0) { // cycle through the warning checks
-                x++;
-                if (x > 5) x = 1; // change the 6 if you add more warning types
-                if(x == 1) {if ((osd_fix_type) < 2) warning_type = 1;} // No GPS Fix
-                else if(x == 2) {if (osd_airspeed * converts < stall && osd_airspeed > 1.12) warning_type = 2;}
-                else if(x == 3) {if ((osd_airspeed * converts) > (float)overspeed) warning_type = 3;}
-                else if(x == 4) {if (osd_vbat_A < float(battv)/10.0 || (osd_battery_remaining_A < batt_warn_level && batt_warn_level != 0)) warning_type = 4;}
-                else if(x == 5) {if (rssi < rssi_warn_level && rssi != -99 && !rssiraw_on) warning_type = 5;}
-                if (x == last_warning) break; // if we've done a full cycle then there mustn't be any warnings
-            }
-        }
-
-        text_timer = millis() + 1000; // blink every 1 secs
-        if (warning == 1){ 
-            if (panel == 1) osd.clear();
-            panel = 0; // turn OSD on if there is a warning                  
-        }
-        char* warning_string;
-//        if (motor_armed == 0){
-//            warning_string = "\x20\x20\x44\x49\x53\x41\x52\x4d\x45\x44\x20\x20";      
-//        }else{
-            if (warning_type == 0) warning_string = "\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20";
-            else if (warning_type == 1) warning_string = "\x20\x4E\x6F\x20\x47\x50\x53\x20\x66\x69\x78\x21";
-            else if (warning_type == 2) warning_string = "\x20\x20\x20\x53\x74\x61\x6c\x6c\x21\x20\x20\x20";
-            else if (warning_type == 3) warning_string = "\x20\x4f\x76\x65\x72\x53\x70\x65\x65\x64\x21\x20";
-            else if (warning_type == 4) warning_string = "\x42\x61\x74\x74\x65\x72\x79\x20\x4c\x6f\x77\x21";
-            else if (warning_type == 5) warning_string = "\x20\x20\x4c\x6f\x77\x20\x52\x73\x73\x69\x20\x20";
-                //        case 6:
-                //osd.printf_P(PSTR("\x42\x61\x74\x74\x65\x72\x79\x20\x4c\x6f\x77\x21"));
-                //            warning_string = "\x20\x20\x44\x49\x53\x41\x52\x4d\x45\x44\x20\x20";
-                //            break;
-//            }
-        osd.printf("%s",warning_string);
-    }
-    osd.closePanel();
-}
-
+                
+  if (one_sec_timer_switch == 1){
+
+    boolean warning[]={0,0,0,0,0,0}; // Make and clear the array
+
+
+                // check all warnings at once
+                if ((osd_fix_type) < 2) {
+                  warning[1] = 1; 
+                  warning[0] = 1;
+                  }
+                if (osd_airspeed * converts < stall && takeofftime == 1) {
+                  warning[2] = 1; 
+                  warning[0] = 1;
+                  }
+                if ((osd_airspeed * converts) > (float)overspeed) {
+                  warning[3] = 1; 
+                  warning[0] = 1;
+                  }
+                if (osd_vbat_A < float(battv)/10.0 || (osd_battery_remaining_A < batt_warn_level && batt_warn_level != 0)) {
+                  warning[4] = 1; 
+                  warning[0] = 1;
+                  }
+                if (rssi < rssi_warn_level && rssi != -99 && !rssiraw_on) {
+                  warning[5] = 1; 
+                  warning[0] = 1;
+                  }
+//                if (eph > 150){  
+//                  warning[6] = 1;
+//                  warning[0] = 1;
+//                  }
+                  
   
+
+            // Prepare for printf in rotation
+            if (rotation == 0) if (warning[0] == 0 || warning[0] + warning[1] + warning[2] + warning[3] + warning[4] + warning[5] == 2) {
+                warning_string = "\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20\x20";
+              }else{
+                  rotation = 1; 
+              } 
+              
+            if (rotation == 1) if (warning[1] == 1) {
+                warning_string = "\x20\x4E\x6F\x20\x47\x50\x53\x20\x66\x69\x78\x21";
+              }else{
+                  rotation = 2; 
+              }
+              
+            if (rotation == 2) if (warning[2] == 1) {
+                warning_string = "\x20\x20\x20\x53\x74\x61\x6c\x6c\x21\x20\x20\x20";
+              }else{
+                  rotation = 3; 
+              }
+              
+            if (rotation == 3) if (warning[3] == 1) {
+                warning_string = "\x20\x4f\x76\x65\x72\x53\x70\x65\x65\x64\x21\x20";
+              }else{
+                  rotation = 4; 
+              }
+              
+            if (rotation == 4) if (warning[4] == 1) {
+                warning_string = "\x42\x61\x74\x74\x65\x72\x79\x20\x4c\x6f\x77\x21";
+              }else{
+                  rotation = 5; 
+              }
+              
+            if (rotation == 5) if (warning[5] == 1) {
+                warning_string = "\x20\x20\x4c\x6f\x77\x20\x52\x73\x73\x69\x20\x20";
+//                  rotation = 6;
+              }
+            
+//            if (rotation == 6) if (warning[6] == 1) {
+//                warning_string = "\x20\x20\x4c\x6f\x77\x20\x48\x44\x4f\x50\x20\x20";            
+//              }
+            rotation++;
+          
+          // Auto switch decesion
+          if (warning[0] == 1 && panel_auto_switch < 3){
+          canswitch = 0;  
+          }else if (ch_raw < 1200) {
+          canswitch = 1;
+          }
+ if (rotation > 5) rotation = 0;
+#ifdef FRSKY
+  if (warning[4] == 1 || warning[0] == 0) {
+        osd.printf("%s",warning_string);  
+  }
+#else
+ osd.printf("%s",warning_string);
+#endif
+ 
+  }
+osd.closePanel();
+}  
 /* **************************************************************** */
 // Panel  : panThr
 // Needs  : X, Y locations
@@ -702,7 +736,8 @@
 void panThr(int first_col, int first_line){
     osd.setPanel(first_col, first_line);
     osd.openPanel();
-    osd.printf("%c%3.0i%c",0x02,osd_throttle,0x25);
+//    osd.printf("%c%3.0i%c",0x14,osd_throttle,0x25);
+    osd.printf("%3.0i%c",osd_throttle,0x25);
     osd.closePanel();
 }
 
@@ -718,9 +753,13 @@
     osd.openPanel();
     
     if (EEPROM.read(OSD_BATT_SHOW_PERCENT_ADDR) == 1){
-    osd.printf("%c%3.0i%c", 0x17, osd_battery_remaining_A, 0x25);
+#ifdef FRSKY
+        osd.printf("%c%5.2f%c", 0x17, (double)osd_battery_remaining_A, 0x25);
+#else
+        osd.printf("%c%3.0i%c", 0x17, osd_battery_remaining_A, 0x25);
+#endif        
     }else{
-    osd.printf("%c%5.0f%c",0x17, mah_used, 0x01);
+        osd.printf("%c%4.0f%c", 0x17, mah_used, 0x01);
     }
     osd.closePanel();
 }
@@ -737,7 +776,15 @@
     osd.openPanel();
     
     start_Time = (millis()/1000) - FTime;
-    osd.printf("%c%2i%c%02i", 0x08,((int)start_Time/60)%60,0x3A,(int)start_Time%60);
+//    osd.printf("%c%2i%c%02i", 0x08,((int)start_Time/60)%60,0x3A,(int)start_Time%60);
+//    osd.printf("%2i%c%02i",((int)start_Time/60)%60,0x3A,(int)start_Time%60);
+
+
+//    if((int)start_Time < 36000) oszt=60;
+    osd.printf("%3i%c%02i", ((int)start_Time/60), 0x3A, (int)start_Time%60);
+//    }else{
+//    osd.printf("%2i%c%02i", ((int)start_Time/3600)%60, 0x3A, ((int)start_Time/60)%60);
+//    }
     osd.closePanel();
 }
 
@@ -751,7 +798,15 @@
 void panHomeDis(int first_col, int first_line){
     osd.setPanel(first_col, first_line);
     osd.openPanel();
-    osd.printf("%c%5.0f%c", 0x0b, (double)((osd_home_distance) * converth), high);
+//    osd.printf("%c%5.0f%c", 0x0b, (double)((osd_home_distance) * converth), high);
+
+
+    if ((osd_home_distance * converth) > 9999.0) {
+      osd.printf("%c%5.2f%c", 0x0b, ((osd_home_distance * converth) / distconv), distchar);
+    }else{
+      osd.printf("%c%5.0f%c", 0x0b, (osd_home_distance * converth), high);
+    }
+
     osd.closePanel();
 }
 
@@ -829,19 +884,20 @@
         osd.printf(" %c%5.2f%c", 0xbc, (double)osd_vbat_A, 0x0d);
     else osd.printf("%c%5.2f%c%c", 0xbc, (double)osd_vbat_A, 0x0d, osd_battery_pic_A);
     */
-    osd.printf("%c%5.2f%c", 0xbc, (double)osd_vbat_A, 0x0d);
+//    osd.printf("%c%5.2f%c", 0xbc, (double)osd_vbat_A, 0x0d);
+    osd.printf("%5.2f%c", (double)osd_vbat_A, 0x0d);
     osd.closePanel();
 }
 
 //------------------ Panel: Waiting for MAVLink HeartBeats -------------------------------
 
-//void panWaitMAVBeats(int first_col, int first_line){
-//    panLogo();
-//    osd.setPanel(first_col, first_line);
-//    osd.openPanel();
-//    osd.printf_P(PSTR("Waiting for|MAVLink heartbeats..."));
-//    osd.closePanel();
-//}
+void panWaitMAVBeats(int first_col, int first_line){
+  //panLogo();
+  osd.setPanel(first_col, first_line);
+  osd.openPanel();
+  osd.printf_P(PSTR("No mav data!"));
+  osd.closePanel();
+}
 
 /* **************************************************************** */
 // Panel  : panGPL
@@ -879,9 +935,12 @@
     osd.setPanel(first_col, first_line);
     osd.openPanel();
     
-    byte gps_str = 0x1f;
-    if(osd_fix_type >= 2)
-      gps_str = 0x0f;
+    byte gps_str = 0x2a;
+    if (osd_fix_type == 2) gps_str = 0x1f;
+    else if (osd_fix_type == 3) gps_str = 0x0f;
+    
+    if ((eph >= 200) && blinker)
+       gps_str = 0x20;
     
     osd.printf("%c%2i", gps_str, osd_satellites_visible);
     osd.closePanel();
@@ -897,7 +956,10 @@
 void panGPS(int first_col, int first_line){
     osd.setPanel(first_col, first_line);
     osd.openPanel();
-    osd.printf("%c%10.6f|%c%10.6f", 0x03, (double)osd_lat, 0x04, (double)osd_lon);
+//    osd.printf("%c%10.6f|%c%10.6f", 0x03, (double)osd_lat, 0x04, (double)osd_lon);
+    osd.printf("%11.6f|%11.6f", (double)osd_lat, (double)osd_lon);
+//    if (blinker == 0) osd.printf("%c%10.6f", 0x03, (double)osd_lat);
+//    else osd.printf("%c%10.6f", 0x04, (double)osd_lon);
     osd.closePanel();
 }
 
@@ -999,7 +1061,8 @@
     osd.openPanel();
     
     wp_target_bearing_rotate_int = round(((float)wp_target_bearing - osd_heading)/360.0 * 16.0) + 1; //Convert to int 0-16 
-    if(wp_target_bearing_rotate_int < 0 ) wp_target_bearing_rotate_int += 16; //normalize
+    if(wp_target_bearing_rotate_int < 1 ) wp_target_bearing_rotate_int += 16; //normalize
+//    else if(wp_target_bearing_rotate_int == 0 ) wp_target_bearing_rotate_int = 16; //normalize
     else if(wp_target_bearing_rotate_int > 16 ) wp_target_bearing_rotate_int -= 16; //normalize
       if (xtrack_error > 999) xtrack_error = 999;
       else if (xtrack_error < -999) xtrack_error = -999;
@@ -1007,7 +1070,7 @@
       osd.printf("%c%c%2i%c%4.0f%c|",0x57, 0x70, wp_number,0x0,(double)((float)(wp_dist) * converth),high);
       showArrow((uint8_t)wp_target_bearing_rotate_int,0);
       
-      if (osd_mode == 10 || osd_mode == 15){     
+      if (osd_mode == 10 || osd_mode == 15 || osd_mode == 7){     
         osd.printf("%c%c%c%4.0f%c", 0x20, 0x58, 0x65, (xtrack_error* converth), high);
       }else{
         osd.printf_P(PSTR("\x20\x20\x20\x20\x20\x20\x20\x20"));
@@ -1048,15 +1111,15 @@
     if (osd_mode == 4) mode_str = "acro"; //ACRO
     if (osd_mode == 5) mode_str = "fbwa"; //FLY_BY_WIRE_A
     if (osd_mode == 6) mode_str = "fbwb"; //FLY_BY_WIRE_B
-    if (osd_mode == 7) mode_str = "crui"; //Cruise
+    if (osd_mode == 7) mode_str = "cruz"; //Cruise
     if (osd_mode == 10) mode_str = "auto"; //AUTO
     if (osd_mode == 11) mode_str = "retl"; //Return to Launch 
     if (osd_mode == 12) mode_str = "loit"; //Loiter
     if (osd_mode == 15) mode_str = "guid"; //GUIDED
     
     
-    osd.printf("%c%s", 0x7f, mode_str);
-//      osd.printf("%c%i", 0x7f, osd_mode);
+//    osd.printf("%c%s", 0x7f, mode_str);
+    osd.printf("%s", mode_str);
     osd.closePanel();
 }
 
@@ -1167,6 +1230,7 @@
 }
 
 // Calculate and shows ILS
+/*
 void showILS(int start_col, int start_row) { 
     //Vertical calculation
     int currentAngleDisplacement = atan(osd_alt_to_home / osd_home_distance) * 57.2957795 - 10;
@@ -1195,9 +1259,32 @@
       osd.printf("%c", selectedChar);
     }
 }
-
+*/
 void do_converts()
 {
+//  if(EEPROM.read(SIGN_MSL_ON_ADDR) == 0) EEPROM.write(876, 1);
+//  if(EEPROM.read(SIGNS_ON_ADDR) != 0) iconGS = 1;
+//  if(EEPROM.read(SIGNS_ON_ADDR) != 0) iconHA = 1;
+//  if(EEPROM.read(SIGNS_ON_ADDR) != 0) iconMSL = 1;
+    //      signDist = 0x8f;
+ //     signTemp = 0x0a;
+ //     signEff = 0x16;
+ //     signRssi = 0x09;
+ //     signCurr = 0xbd;
+ //     signAlt = 0x11;
+ //     signClimb = 0x15;
+ //     signHomeAlt = 0x12;
+ //     signVel = 0x14;
+//      signASpeed = 0x13;
+//      signThrot = 0x02;
+//      signBat = 0x17;
+ //     signTime = 0x08;
+//      signHomeDist = 0x0b;
+//      signBatA = 0xbc;
+//      signMode = 0x7f;
+//      signLat = 0x03;
+//      signLon = 0x04;
+
     if (EEPROM.read(measure_ADDR) == 0) {
         converts = 3.6;
         converth = 1.0;
@@ -1216,12 +1303,29 @@
         high = 0x66;
         temps = 0xbb;
         tempconv = 18;
-        tempconvAdd = 3200;
+        tempconvAdd = 32000;
         distchar = 0x1c;
         distconv = 5280;
         climbchar = 0x1e;
     }
 }
 
-

-
+void timers()
+{
+  if (one_sec_timer_switch == 1){ 
+    one_sec_timer = millis() + 1000;
+    one_sec_timer_switch = 0;
+    blinker = !blinker;
+  }
+  if (millis() > one_sec_timer) one_sec_timer_switch = 1;  
+//if (one_sec_timer_switch == 1){ 
+//  one_sec_timer = millis() + 1000;
+//  one_sec_timer_switch = 0;
+//  if (blinker == 0){
+//  blinker = 1;
+//  }else{
+//  blinker = 0;  
+//  }
+//  }
+}

+

file:a/OSD_Vars.h -> file:b/OSD_Vars.h
--- a/OSD_Vars.h
+++ b/OSD_Vars.h
@@ -13,7 +13,8 @@
 static float ddistance = 0;
 static char strclear[]="\x20\x20\x20\x20\x20\x20\x20\x20";
 
-
+//static uint8_t oszt = 1;
+ 
 //static float	      nav_roll = 0; // Current desired roll in degrees
 //static float        nav_pitch = 0; // Current desired pitch in degrees
 //static int16_t      nav_bearing = 0; // Current desired heading in degrees
@@ -21,13 +22,16 @@
 static int8_t       wp_target_bearing_rotate_int = 0;
 static uint16_t     wp_dist = 0; // Distance to active MISSION in meters
 static uint8_t      wp_number = 0; // Current waypoint number
-//static float	      alt_error = 0; // Current altitude error in meters
-//static float        aspd_error = 0; // Current airspeed error in meters/second
+static float	      alt_error = 0; // Current altitude error in meters
+static float        aspd_error = 0; // Current airspeed error in meters/second
 static float	    xtrack_error = 0; // Current crosstrack error on x-y plane in meters
 static float        eff = 0; //Efficiency
-
+static uint16_t     eph = 0;
+
+static uint8_t      currentBasePanel=255; //0 - Normal OSD; 1 - Flight summary; 2 - No mavlink data (pre-set = 255 to force osd.clear() after boot screen
 static uint8_t      base_mode=0;
-static bool         motor_armed = 0;
+static uint8_t      panel_auto_switch=0;
+//static bool         motor_armed = 0;
 static bool         ma = 0;
 static bool         osd_clear = 0;
 static uint16_t     ch_raw = 0;
@@ -43,10 +47,11 @@
 //static uint16_t     chan2_raw_middle = 0;
 
 static uint8_t      ch_toggle = 0;
+static uint8_t      check_warning = 1;
 //static boolean      osd_set = 0;
 static boolean      switch_mode = 0;
 static boolean      takeofftime = 0;
-static boolean      haltset = 0;
+//static boolean      haltset = 0;
 //static boolean      pal_ntsc = 0;
 
 //static int8_t       setup_menu = 0;
@@ -60,11 +65,19 @@
 
 static uint8_t      spe = 0;
 static uint8_t      high = 0;
-static uint8_t      temps = 0;
+static int16_t      temps = 0;
 static float        osd_vbat_A = 0;                 // Battery A voltage in milivolt
+#ifdef FRSKY
+static float        osd_curr_A = 0;                 // Battery A current
+#else
 static int16_t      osd_curr_A = 0;                 // Battery A current
+#endif
 static float        mah_used = 0;
+#ifdef FRSKY
+static float        osd_battery_remaining_A = 0;    // 0 to 100 <=> 0 to 1000
+#else
 static int8_t       osd_battery_remaining_A = 0;    // 0 to 100 <=> 0 to 1000
+#endif
 static uint8_t      batt_warn_level = 0;
 
 //static uint8_t    osd_battery_pic_A = 0xb4;       // picture to show battery remaining
@@ -75,18 +88,22 @@
 static float        start_Time = -1.0;
 static uint8_t      osd_mode = 0;                   // Navigation mode from RC AC2 = CH5, APM = CH8
 static uint8_t      osd_nav_mode = 0;               // Navigation mode from RC AC2 = CH5, APM = CH8
-static unsigned long text_timer = 0;
-static unsigned long warning_timer =0;
+//static unsigned long text_timer = 0;
+static unsigned long one_sec_timer = 0;
+static unsigned long mavLinkTimer = 0;
+//static unsigned long warning_timer =0;
 static unsigned long runt =0;
 static unsigned long FTime = 0;
-static unsigned long CallSignBlink = 0;
-static unsigned long landed = 0;
-
-static uint8_t      warning_type = 0;
-static uint8_t      last_warning = 0;
-static uint8_t      warning = 0;
+//static unsigned long CallSignBlink = 0;
+static unsigned long landed = 4294967295;
+
+//static uint8_t      warning_type = 0;
+static char*        warning_string;
+static boolean      warning_found = 0;
+static boolean      canswitch = 1;
 static uint8_t      osd_off_switch = 0;
 static uint8_t      osd_switch_last = 100;
+static uint8_t      rotation = 0;
 static unsigned long         osd_switch_time = 0;
 //static unsigned long         descendt = 0;
 static float         palt = 0;
@@ -106,7 +123,7 @@
 static float        osd_alt_to_home = 0; 
 static long         osd_home_distance = 0;          // distance from home
 static uint8_t      osd_home_direction;             // Arrow direction pointing to home (1-16 to CW loop)
-static int          takeoff_heading = -400;         // Calculated takeoff heading
+//static int          takeoff_heading = -400;         // Calculated takeoff heading
 
 static int16_t       osd_pitch = 0;                  // pitch from DCM
 static int16_t       osd_roll = 0;                   // roll from DCM
@@ -122,16 +139,34 @@
 static int8_t       osd_wind_arrow_rotate_int;
 static int8_t       osd_COG_arrow_rotate_int;
 
-static uint8_t      osd_alt_cnt = 0;              // counter for stable osd_alt
-static float        osd_alt_prev = 0;             // previous altitude
+//static uint8_t      osd_alt_cnt = 0;              // counter for stable osd_alt
+//static float        osd_alt_prev = 0;             // previous altitude
 
 static float        osd_groundspeed = 0;            // ground speed
 static uint8_t     osd_throttle = 0;               // throtle
 static uint16_t     temperature = 0;
 static uint8_t      tempconv = 1;
 static uint16_t     tempconvAdd = 0;
-static uint16_t     distchar = 0;
-static uint16_t     climbchar = 0;
+static byte     distchar = 0;
+static byte     climbchar = 0;
+//static byte     signDist = 0x20;
+//static byte     signTemp = 0x20;
+//static byte     signEff = 0x20;
+//static byte     signRssi = 0x20;
+//static byte     signCurr = 0x20;
+//static byte     signAlt = 0x20;
+//static byte     signClimb = 0x20;
+//static byte     signHomeAlt = 0x20;
+//static byte     signVel = 0x20;
+//static byte     signASpeed = 0x20;
+//static byte     signThrot = 0x20;
+//static byte     signBat = 0x20;
+//static byte     signTime = 0x20;
+//static byte     signHomeDist = 0x20;
+//static byte     signBatA = 0x20;
+//static byte     signMode = 0x20;
+//static byte     signLat = 0x20;
+//static byte     signLon = 0x20;
 
 
 static float     convertt = 0;
@@ -140,13 +175,19 @@
 
 //MAVLink session control
 static boolean      mavbeat = 0;
-static boolean      landing = 0;
+//static boolean      iconAS = 0;
+//static boolean      iconGS = 0;
+//static boolean      iconHA = 0;
+//static boolean      iconMSL = 0;
+//static boolean      landing = 0;
 static float        lastMAVBeat = 0;
 static boolean      waitingMAVBeats = 1;
 //static uint8_t      apm_mav_type;
 static uint8_t      apm_mav_system; 
 static uint8_t      apm_mav_component;
 static boolean      enable_mav_request = 0;
+static boolean      blinker = 0;
+static boolean      one_sec_timer_switch = 0;
 
 static const uint8_t npanels = 2;
 static uint8_t panel = 0; 
@@ -200,7 +241,7 @@
 byte panWarn_XY[2][npanels];
 byte panWindSpeed_XY[2][npanels];
 byte panClimb_XY[2][npanels];
-//byte panTune_XY[2][npanels];
+byte panTune_XY[2][npanels];
 byte panRSSI_XY[2][npanels];
 byte panEff_XY[2][npanels];
 byte panCALLSIGN_XY[2][npanels];
@@ -214,7 +255,7 @@
 static uint8_t      rssical = 0;
 static uint8_t      osd_rssi = 0; //raw value from mavlink
 static int16_t      rssi = -99; // scaled value 0-100%
-static bool         rssiraw_on = false; // 0- display scale value | 1- display raw value
+static uint8_t      rssiraw_on = 0; 
 static uint8_t      rssi_warn_level = 0;
 

 

comments