From 048e58372235dadc0059a5bce7445ec00e5851a0 Mon Sep 17 00:00:00 2001
From: MikeBland <Mike.P.Blandford@gmail.com>
Date: Wed, 30 Nov 2016 18:24:11 +0000
Subject: [PATCH] Final commit for Release: er9x - 821 ersky9x - 219
 eepe/eepskye - 421

---
 Revisions.txt                         | 10 ++++----
 radio/er9x/src/er9x.cpp               |  4 +++
 radio/ersky9x/src/ersky9x.cpp         | 16 ++++++++++--
 radio/ersky9x/src/mavlink/Mavlink.cpp | 36 ++++++++++++++++++++-------
 radio/ersky9x/src/menus.cpp           |  2 ++
 radio/ersky9x/src/s6rimg.lbm          |  9 ++++++-
 6 files changed, 60 insertions(+), 17 deletions(-)

diff --git a/Revisions.txt b/Revisions.txt
index d6c9c33..1641a01 100644
--- a/Revisions.txt
+++ b/Revisions.txt
@@ -1,6 +1,6 @@
-    218
+    219
 Revisions:
-er9x -r820
-ersky9x -r218
-eepe -r420
-eepskye -r420
+er9x -r821
+ersky9x -r219
+eepe -r421
+eepskye -r421
diff --git a/radio/er9x/src/er9x.cpp b/radio/er9x/src/er9x.cpp
index 2a50dfa..bc1a29b 100644
--- a/radio/er9x/src/er9x.cpp
+++ b/radio/er9x/src/er9x.cpp
@@ -4330,6 +4330,10 @@ extern uint8_t serialDat0 ;
 #if defined(CPUM128) || defined(CPUM2561)
 		lcd_puts_Pleft( FH, PSTR("Hardware Menu Enabled") ) ;
 		refreshDiplay() ;
+  	if ( mcusr & (1<<WDRF) )
+		{
+			break ;
+		}
 #endif
 	}
 	if ( in == 0x82 )
diff --git a/radio/ersky9x/src/ersky9x.cpp b/radio/ersky9x/src/ersky9x.cpp
index 472fcc7..7cff0b5 100644
--- a/radio/ersky9x/src/ersky9x.cpp
+++ b/radio/ersky9x/src/ersky9x.cpp
@@ -1704,9 +1704,21 @@ uint32_t updateSlave() ;
 	
 	}
 
-	if ( ( read_trims() & 0x01 )== 0x01 )
+	while ( ( read_trims() & 0x01 )== 0x01 )
 	{
 		HardwareMenuEnabled = 1 ;
+		lcd_puts_Pleft( FH, XPSTR("Hardware Menu Enabled") ) ;
+		refreshDisplay() ;
+
+#ifdef PCBSKY
+		if ( ( ( ResetReason & RSTC_SR_RSTTYP ) == (2 << 8) ) || unexpectedShutdown )	// Not watchdog
+#endif
+#if defined(PCBX9D) || defined(PCB9XT)
+		if ( ( ( ResetReason & RCC_CSR_WDGRSTF ) == RCC_CSR_WDGRSTF ) || unexpectedShutdown )	// Not watchdog
+#endif
+		{
+			break ;
+		}
 	}
 
   resetTimer();
@@ -6213,7 +6225,7 @@ void doSplash()
 //	SplashDebug[8] = 0 ;
 //#endif
 
-	if( (!g_eeGeneral.disableSplashScreen) || ( HardwareMenuEnabled ) )
+	if( !g_eeGeneral.disableSplashScreen )
   {
    	check_backlight() ;
     lcd_clear();
diff --git a/radio/ersky9x/src/mavlink/Mavlink.cpp b/radio/ersky9x/src/mavlink/Mavlink.cpp
index 0b1c3e7..124f81c 100644
--- a/radio/ersky9x/src/mavlink/Mavlink.cpp
+++ b/radio/ersky9x/src/mavlink/Mavlink.cpp
@@ -780,7 +780,9 @@ static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* ms
 
 static inline uint32_t mavlink_msg_heartbeat_get_custom_mode(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_uint32_t(msg,  0);
+//	return _MAV_RETURN_uint32_t(msg,  0);
+	uint32_t *p = (uint32_t *)((msg)->payload64) ;
+	return *p ;
 }
 
 static inline uint8_t mavlink_msg_heartbeat_get_base_mode(const mavlink_message_t* msg)
@@ -790,22 +792,30 @@ static inline uint8_t mavlink_msg_heartbeat_get_base_mode(const mavlink_message_
 
 static inline int16_t mavlink_msg_sys_status_get_current_battery(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_int16_t(msg,  16);
+//	return _MAV_RETURN_int16_t(msg,  16);
+	int16_t *p = (int16_t *)((msg)->payload64) ;
+	return p[8] ;
 }
 
 static inline float mavlink_msg_vfr_hud_get_alt(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_float(msg,  8);
+//	return _MAV_RETURN_float(msg,  8);
+	float *p = (float *)((msg)->payload64) ;
+	return p[2] ;
 }
 
 static inline uint16_t mavlink_msg_hwstatus_get_Vcc(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_uint16_t(msg,  0);
+//	return _MAV_RETURN_uint16_t(msg,  0);
+	uint16_t *p = (uint16_t *)((msg)->payload64) ;
+	return p[0] ;
 }
 
 static inline uint16_t mavlink_msg_sys_status_get_load(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_uint16_t(msg,  12);
+//	return _MAV_RETURN_uint16_t(msg,  12);
+	uint16_t *p = (uint16_t *)((msg)->payload64) ;
+	return p[6] ;
 }
 
 static inline uint16_t mavlink_msg_sys_status_get_voltage_battery(const mavlink_message_t* msg)
@@ -815,7 +825,9 @@ static inline uint16_t mavlink_msg_sys_status_get_voltage_battery(const mavlink_
 
 static inline int16_t mavlink_msg_vfr_hud_get_heading(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_int16_t(msg,  16);
+//	return _MAV_RETURN_int16_t(msg,  16);
+	int16_t *p = (int16_t *)((msg)->payload64) ;
+	return p[8] ;
 }
 
 static inline float mavlink_msg_vfr_hud_get_climb(const mavlink_message_t* msg)
@@ -830,17 +842,23 @@ static inline int8_t mavlink_msg_sys_status_get_battery_remaining(const mavlink_
 
 static inline int32_t mavlink_msg_gps_raw_int_get_lat(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_int32_t(msg,  8);
+//	return _MAV_RETURN_int32_t(msg,  8);
+	int32_t *p = (int32_t *)((msg)->payload64) ;
+	return p[2] ;
 }
 
 static inline int32_t mavlink_msg_gps_raw_int_get_lon(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_int32_t(msg,  12);
+//	return _MAV_RETURN_int32_t(msg,  12);
+	int32_t *p = (int32_t *)((msg)->payload64) ;
+	return p[3] ;
 }
 
 static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* msg)
 {
-	return _MAV_RETURN_int32_t(msg,  16);
+//	return _MAV_RETURN_int32_t(msg,  16);
+	int32_t *p = (int32_t *)((msg)->payload64) ;
+	return p[4] ;
 }
 static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg)
 {
diff --git a/radio/ersky9x/src/menus.cpp b/radio/ersky9x/src/menus.cpp
index ef02fbb..b7a1f0e 100644
--- a/radio/ersky9x/src/menus.cpp
+++ b/radio/ersky9x/src/menus.cpp
@@ -11852,6 +11852,8 @@ void menuProcS6R(uint8_t event)
 			{
   			lcd_putsAttIdx( 12*FW, 3*FH, XPSTR("\006Hor   HorRevVer   VerRev"), value, 0 ) ;
 			}
+			lcd_img( 12, 4*FH, value<2 ? S6Rimg1 : S6Rimg3, value<3 ? value & 1 : 2, 0 ) ;
+			lcd_puts_Pleft( 7*FH, "<-Heading" ) ;
 		break ;
 		case 3 :
 		case 4 :
diff --git a/radio/ersky9x/src/s6rimg.lbm b/radio/ersky9x/src/s6rimg.lbm
index 3f7d3c8..9ffba60 100644
--- a/radio/ersky9x/src/s6rimg.lbm
+++ b/radio/ersky9x/src/s6rimg.lbm
@@ -42,5 +42,12 @@ PROGMEM S6Rimg3[] = {
 0xFF,0x00,0x00,0x00,0x00,0x00,0x31,0x4A,0x4C,0x7F,0x00,0x26,0x49,0x49,0x3E,0x00,
 0x26,0x49,0x49,0x49,0x32,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x00,0x80,0x7F,
 0x07,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,
-0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x07,0x02,0x01,0x00,0x00
+0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x07,0x02,0x01,0x00,0x00,
+
+0xF0,0x18,0x14,0x12,0x11,0x11,0x11,0x11,0x11,0x11,0x11,0x11,0x11,0x11,0x11,0x11,
+0x11,0x11,0x11,0x11,0x11,0x11,0x11,0x11,0x11,0x11,0xF1,0x09,0x25,0x03,0xFF,
+0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0x00,0x22,0x80,0x7F,
+0x07,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,
+0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x07,0x02,0x01,0x00,0x00,
 };