--- a/src/sleepmode.asm	Sun Jun 23 13:29:17 2019 +0200
+++ b/src/sleepmode.asm	Sun Jun 23 15:21:16 2019 +0200
@@ -66,7 +66,8 @@
 	clrf	sm_timer1
 	clrf	sm_timer2
 	clrf	sm_timer3
-	call	speed_normal
+;	call	speed_normal
+	call	speed_eco
 	bsf		no_sensor_int					; inhibit sensor interrupts
 	clrf	ADCON0							; power-down ADC module
 
@@ -127,6 +128,7 @@
 	bra		deepsleep						; YES
 
 no_deepsleep:
+	bcf	deep_sleep					; Clear flag to avoid putting the unit into deep sleep immediately when removed from charger
 	rcall	sleepmode_sleep					; wait at least 35 ms (every 62.5ms timer7 wakeup)
 
 	; Any button pressed in sleep?
@@ -143,6 +145,8 @@
 deepsleep:
 	btfss	analog_switches
 	bra		no_deepsleep					; no analog switches, no deep sleep required
+	btfsc	cc_active			; Charging?
+	bra	no_deepsleep			; Yes, no deep sleep required
 
 	bcf		PIE1,0							; stop timer 1 interrupt
 	bcf		PIE2,1							; stop timer 2 interrupt
@@ -161,6 +165,9 @@
 	btfsc	onesecupdate					; one second in sleep?
 	rcall	onesec_sleep					; YES - check switches, check pressure sensor, etc.
 
+	btfsc	cc_active			; Charging?
+	bcf	deep_sleep			; Yes, no deep sleep required
+	
 	rcall	sleepmode_sleep
 
 	btfss	deep_sleep						; enter normal sleep mode?
@@ -191,6 +198,7 @@
 deepsleep_get_accel:
 	call	I2C_init_compass				; required for compass1
 	call	I2C_init_accelerometer			; required for compass2
+	rcall	sleepmode_sleep
 	call	I2C_RX_accelerometer			; read accelerometer
 	call	I2C_sleep_compass				; required for compass1
 	call	I2C_sleep_accelerometer			; required for compass2