diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index f53b7a9571..38391e7e4c 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -2204,7 +2204,7 @@ static void updateNavigationFlightStatistics(void)
     }
 }
 
-int32_t getTotalTravelDistance(void)
+uint32_t getTotalTravelDistance(void)
 {
     return lrintf(posControl.totalTripDistance);
 }
@@ -3226,7 +3226,9 @@ int32_t navigationGetHomeHeading(void)
 
 // returns m/s
 float calculateAverageSpeed() {
-    return (float)getTotalTravelDistance() / (getFlightTime() * 100);
+    float flightTime = getFlightTime();
+    if (flightTime == 0.0f) return 0;
+    return (float)getTotalTravelDistance() / (flightTime * 100);
 }
 
 const navigationPIDControllers_t* getNavigationPIDControllers(void) {
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index ac0219537c..824faca581 100755
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -404,7 +404,7 @@ typedef struct {
 
 float getEstimatedActualVelocity(int axis);
 float getEstimatedActualPosition(int axis);
-int32_t getTotalTravelDistance(void);
+uint32_t getTotalTravelDistance(void);
 void getEstimatedPositionAndVelocity(navPositionAndVelocity_t * pos);
 
 /* Waypoint list access functions */
diff --git a/src/main/telemetry/sim.c b/src/main/telemetry/sim.c
index c4f9550d57..3d4e107fea 100644
--- a/src/main/telemetry/sim.c
+++ b/src/main/telemetry/sim.c
@@ -1,46 +1,119 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
 #include "platform.h"
 
 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SIM)
 
 #include <string.h>
 
-#include "common/axis.h"
-#include "common/streambuf.h"
-#include "common/utils.h"
 #include "common/printf.h"
 
 #include "drivers/time.h"
 
-#include "fc/config.h"
 #include "fc/fc_core.h"
-#include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
-#include "fc/fc_init.h"
 
 #include "flight/imu.h"
-#include "flight/failsafe.h"
-#include "flight/pid.h"
 
 #include "io/gps.h"
 #include "io/serial.h"
 
 #include "navigation/navigation.h"
-#include "navigation/navigation_private.h"
 
-#include "sensors/acceleration.h"
 #include "sensors/battery.h"
-#include "sensors/boardalignment.h"
-#include "sensors/diagnostics.h"
-#include "sensors/gyro.h"
 #include "sensors/sensors.h"
 
 #include "common/string_light.h"
 #include "common/typeconversion.h"
-#include "build/debug.h"
 
 #include "telemetry/sim.h"
 #include "telemetry/telemetry.h"
 
+
+#define SIM_AT_COMMAND_MAX_SIZE 255
+#define SIM_RESPONSE_BUFFER_SIZE 255
+#define SIM_CYCLE_MS 5000                       // wait between sim command cycles
+#define SIM_AT_COMMAND_DELAY_MS 3000
+#define SIM_AT_COMMAND_DELAY_MIN_MS 500
+#define SIM_STARTUP_DELAY_MS 10000
+#define SIM_SMS_COMMAND_RTH "RTH"
+#define SIM_PIN "0000"
+#define SIM_LOW_ALT_WARNING_MODES (NAV_ALTHOLD_MODE || NAV_RTH_MODE || NAV_WP_MODE || FAILSAFE_MODE)
+
+#define SIM_RESPONSE_CODE_OK    ('O' << 24 | 'K' << 16)
+#define SIM_RESPONSE_CODE_ERROR ('E' << 24 | 'R' << 16 | 'R' << 8 | 'O')
+#define SIM_RESPONSE_CODE_RING  ('R' << 24 | 'I' << 16 | 'N' << 8 | 'G')
+#define SIM_RESPONSE_CODE_CLIP  ('C' << 24 | 'L' << 16 | 'I' << 8 | 'P')
+#define SIM_RESPONSE_CODE_CREG  ('C' << 24 | 'R' << 16 | 'E' << 8 | 'G')
+#define SIM_RESPONSE_CODE_CSQ   ('C' << 24 | 'S' << 16 | 'Q' << 8 | ':')
+#define SIM_RESPONSE_CODE_CMT   ('C' << 24 | 'M' << 16 | 'T' << 8 | ':')
+
+
+typedef enum  {
+    SIM_TX_FLAG                 = (1 << 0),
+    SIM_TX_FLAG_FAILSAFE        = (1 << 1),
+    SIM_TX_FLAG_GPS             = (1 << 2),
+    SIM_TX_FLAG_ACC             = (1 << 3),
+    SIM_TX_FLAG_LOW_ALT         = (1 << 4),
+    SIM_TX_FLAG_RESPONSE        = (1 << 5)
+} simTxFlags_e;
+
+typedef enum  {
+    SIM_MODULE_NOT_DETECTED = 0,
+    SIM_MODULE_NOT_REGISTERED,
+    SIM_MODULE_REGISTERED,
+} simModuleState_e;
+
+typedef enum  {
+    SIM_STATE_INIT = 0,
+    SIM_STATE_INIT2,
+    SIM_STATE_INIT_ENTER_PIN,
+    SIM_STATE_SET_MODES,
+    SIM_STATE_SEND_SMS,
+    SIM_STATE_SEND_SMS_ENTER_MESSAGE
+} simTelemetryState_e;
+
+typedef enum  {
+    SIM_AT_OK = 0,
+    SIM_AT_ERROR,
+    SIM_AT_WAITING_FOR_RESPONSE
+} simATCommandState_e;
+
+typedef enum  {
+    SIM_READSTATE_RESPONSE = 0,
+    SIM_READSTATE_SMS,
+    SIM_READSTATE_SKIP
+} simReadState_e;
+
+typedef enum  {
+    SIM_TX_NO = 0,
+    SIM_TX_FS,
+    SIM_TX
+} simTransmissionState_e;
+
+typedef enum {
+    ACC_EVENT_NONE = 0,
+    ACC_EVENT_HIGH,
+    ACC_EVENT_LOW,
+    ACC_EVENT_NEG_X
+} accEvent_t;
+
+
 static serialPort_t *simPort;
 static serialPortConfig_t *portConfig;
 static bool simEnabled = false;
@@ -55,20 +128,23 @@ static uint8_t readState = SIM_READSTATE_RESPONSE;
 static uint8_t transmitFlags = 0;
 static timeMs_t t_lastMessageSent = 0;
 static uint8_t lastMessageTriggeredBy = 0;
-uint8_t simModuleState = SIM_MODULE_NOT_DETECTED;
+static uint8_t simModuleState = SIM_MODULE_NOT_DETECTED;
 
-int simRssi;
-uint8_t accEvent = ACC_EVENT_NONE;
-char* accEventDescriptions[] = { "", "HIT! ", "DROP ", "HIT " };
-char* modeDescriptions[] = { "MAN", "ACR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "LAU", "FS" };
-const char gpsFixIndicators[] = { '!', '*', ' ' };
+static int simRssi;
+static uint8_t accEvent = ACC_EVENT_NONE;
+static char* accEventDescriptions[] = { "", "HIT! ", "DROP ", "HIT " };
+static char* modeDescriptions[] = { "MAN", "ACR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "LAU", "FS" };
+static const char gpsFixIndicators[] = { '!', '*', ' ' };
 
 
-bool isGroundStationNumberDefined() {
+// XXX UNUSED
+#if 0
+static bool isGroundStationNumberDefined(void) {
     return telemetryConfig()->simGroundStationNumber[0] != '\0';
 }
+#endif
 
-bool checkGroundStationNumber(uint8_t* rv)
+static bool checkGroundStationNumber(uint8_t* rv)
 {
     int i;
     const uint8_t* gsn = telemetryConfig()->simGroundStationNumber;
@@ -99,7 +175,7 @@ bool checkGroundStationNumber(uint8_t* rv)
 }
 
 
-void readOriginatingNumber(uint8_t* rv)
+static void readOriginatingNumber(uint8_t* rv)
 {
     int i;
     uint8_t* gsn = telemetryConfigMutable()->simGroundStationNumber;
@@ -110,7 +186,7 @@ void readOriginatingNumber(uint8_t* rv)
     gsn[i] = '\0';
 }
 
-void readTransmitFlags(const uint8_t* fs)
+static void readTransmitFlags(const uint8_t* fs)
 {
     int i;
 
@@ -136,7 +212,7 @@ void readTransmitFlags(const uint8_t* fs)
     }
 }
 
-void requestSendSMS(uint8_t trigger)
+static void requestSendSMS(uint8_t trigger)
 {
     lastMessageTriggeredBy = trigger;
     if (simTelemetryState == SIM_STATE_SEND_SMS_ENTER_MESSAGE)
@@ -146,10 +222,10 @@ void requestSendSMS(uint8_t trigger)
         sim_t_stateChange = 0; // send immediately
 }
 
-void readSMS()
+static void readSMS(void)
 {
     if (sl_strcasecmp((char*)simResponse, SIM_SMS_COMMAND_RTH) == 0) {
-        if (!posControl.flags.forcedRTHActivated) {
+        if (getStateOfForcedRTH() == RTH_IDLE) {
             activateForcedRTH();
         } else {
             abortForcedRTH();
@@ -160,7 +236,7 @@ void readSMS()
     requestSendSMS(SIM_TX_FLAG_RESPONSE);
 }
 
-void readSimResponse()
+static void readSimResponse(void)
 {
     if (readState == SIM_READSTATE_SKIP) {
         readState = SIM_READSTATE_RESPONSE;
@@ -226,7 +302,7 @@ void readSimResponse()
     }
 }
 
-int getAltMeters()
+static int16_t getAltitudeMeters(void)
 {
 #if defined(USE_NAV)
     return getEstimatedActualPosition(Z) / 100;
@@ -235,7 +311,7 @@ int getAltMeters()
 #endif
 }
 
-void transmit()
+static void transmit(void)
 {
     timeMs_t timeSinceMsg = millis() - t_lastMessageSent;
     uint8_t triggers = SIM_TX_FLAG;
@@ -257,9 +333,9 @@ void transmit()
 
     if (FLIGHT_MODE(FAILSAFE_MODE))
         triggers |= SIM_TX_FLAG_FAILSAFE;
-    if (gpsSol.fixType != GPS_NO_FIX && posControl.flags.estPosStatus < EST_USABLE)
+    if (!navigationPositionEstimateIsHealthy())
         triggers |= SIM_TX_FLAG_GPS;
-    if (gpsSol.fixType != GPS_NO_FIX && FLIGHT_MODE(SIM_LOW_ALT_WARNING_MODES) && getAltMeters() < telemetryConfig()->simLowAltitude)
+    if (gpsSol.fixType != GPS_NO_FIX && FLIGHT_MODE(SIM_LOW_ALT_WARNING_MODES) && getAltitudeMeters() < telemetryConfig()->simLowAltitude)
         triggers |= SIM_TX_FLAG_LOW_ALT;
 
     triggers &= transmitFlags;
@@ -275,50 +351,44 @@ void transmit()
     }
 }
 
-void sendATCommand(const char* command)
+static void sendATCommand(const char* command)
 {
     atCommandStatus = SIM_AT_WAITING_FOR_RESPONSE;
-    int len = strlen((char*)command);
-    if (len > SIM_AT_COMMAND_MAX_SIZE)
-        len = SIM_AT_COMMAND_MAX_SIZE;
+    uint8_t len = MIN((uint8_t)strlen(command), SIM_AT_COMMAND_MAX_SIZE);
     serialWriteBuf(simPort, (const uint8_t*) command, len);
 }
 
-void sendSMS(void)
+static void sendSMS(void)
 {
     int32_t lat = 0, lon = 0;
-    int16_t gs = 0, gc = 0;
-    int vbat = getBatteryVoltage();
+    int16_t groundSpeed = 0;
+    uint16_t vbat = getBatteryVoltage();
     int16_t amps = isAmperageConfigured() ? getAmperage() / 10 : 0; // 1 = 100 milliamps
-    int avgSpeed = (int)round(10 * calculateAverageSpeed());
+    uint16_t avgSpeed = lrintf(10 * calculateAverageSpeed());
     uint32_t now = millis();
 
-    if (getFlightTime() == 0.0F) {
-        avgSpeed = 0;
-    }
-
     if (sensors(SENSOR_GPS)) {
         lat = gpsSol.llh.lat;
         lon = gpsSol.llh.lon;
-        gs = gpsSol.groundSpeed / 100;
-        gc = gpsSol.groundCourse / 10;
+        groundSpeed = gpsSol.groundSpeed / 100;
     }
 
-    int len;
-    int32_t E7 = 10000000;
+    const int32_t E7 = 10000000;
+
     // \x1a sends msg, \x1b cancels
-    len = tfp_sprintf((char*)atCommand, "%s%d.%02dV %d.%dA ALT:%ld SPD:%ld/%d.%d DIS:%d/%d COG:%d SAT:%d%c SIG:%d %s maps.google.com/?q=@%ld.%07ld,%ld.%07ld\x1a",
+    uint8_t len = tfp_sprintf((char*)atCommand, "%s%d.%02dV %d.%dA ALT:%d SPD:%d/%d.%d DIS:%lu/%lu HDG:%d SAT:%d%c SIG:%d %s maps.google.com/?q=@%ld.%07ld,%ld.%07ld\x1a",
         accEventDescriptions[accEvent],
         vbat / 100, vbat % 100,
         amps / 10, amps % 10,
-        getAltMeters(),
-        gs, avgSpeed / 10, avgSpeed % 10,
+        getAltitudeMeters(),
+        groundSpeed, avgSpeed / 10, avgSpeed % 10,
         GPS_distanceToHome, getTotalTravelDistance() / 100,
-        gc,
+        attitude.values.yaw,
         gpsSol.numSat, gpsFixIndicators[gpsSol.fixType],
         simRssi,
-        posControl.flags.forcedRTHActivated ? "RTH" : modeDescriptions[getFlightModeForTelemetry()],
+        getStateOfForcedRTH() == RTH_IDLE ? modeDescriptions[getFlightModeForTelemetry()] : "RTH",
         lat / E7, lat % E7, lon / E7, lon % E7);
+
     serialWriteBuf(simPort, atCommand, len);
     t_lastMessageSent = now;
     accEvent = ACC_EVENT_NONE;
@@ -355,8 +425,8 @@ void handleSimTelemetry()
     if (now < sim_t_stateChange)
         return;
 
-    sim_t_stateChange = now + SIM_AT_COMMAND_DELAY_MS;       // by default, if OK or ERROR not received, wait this long
-    simWaitAfterResponse = false;   // by default, if OK or ERROR received, go to next state immediately.
+    sim_t_stateChange = now + SIM_AT_COMMAND_DELAY_MS;  // by default, if OK or ERROR not received, wait this long
+    simWaitAfterResponse = false;                       // by default, if OK or ERROR received, go to next state immediately.
     switch (simTelemetryState) {
         case SIM_STATE_INIT:
         sendATCommand("AT\r");
@@ -392,27 +462,22 @@ void handleSimTelemetry()
     }
 }
 
-void freeSimTelemetryPort(void)
+// XXX UNUSED
+#if 0
+static void freeSimTelemetryPort(void)
 {
     closeSerialPort(simPort);
     simPort = NULL;
     simEnabled = false;
 }
+#endif
 
 void initSimTelemetry(void)
 {
     portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SIM);
 }
 
-void checkSimTelemetryState(void)
-{
-    if (simEnabled) {
-        return;
-    }
-    configureSimTelemetryPort();
-}
-
-void configureSimTelemetryPort(void)
+static void configureSimTelemetryPort(void)
 {
     if (!portConfig) {
         return;
@@ -432,4 +497,12 @@ void configureSimTelemetryPort(void)
     simEnabled = true;
 }
 
+void checkSimTelemetryState(void)
+{
+    if (simEnabled) {
+        return;
+    }
+    configureSimTelemetryPort();
+}
+
 #endif
diff --git a/src/main/telemetry/sim.h b/src/main/telemetry/sim.h
index 182f496216..2df17f1d3d 100644
--- a/src/main/telemetry/sim.h
+++ b/src/main/telemetry/sim.h
@@ -1,78 +1,28 @@
-#define SIM_AT_COMMAND_MAX_SIZE 256
-#define SIM_RESPONSE_BUFFER_SIZE 256
-#define SIM_CYCLE_MS 5000 								// wait between sim command cycles
-#define SIM_AT_COMMAND_DELAY_MS 3000
-#define SIM_AT_COMMAND_DELAY_MIN_MS 500
-#define SIM_STARTUP_DELAY_MS 10000
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
 #define SIM_MIN_TRANSMIT_INTERVAL 10
 #define SIM_DEFAULT_TRANSMIT_INTERVAL 60
-#define SIM_SMS_COMMAND_RTH             "RTH"
-#define SIM_PIN "0000"
-#define SIM_LOW_ALT_WARNING_MODES (NAV_ALTHOLD_MODE || NAV_RTH_MODE || NAV_WP_MODE || FAILSAFE_MODE)
-
-#define SIM_RESPONSE_CODE_OK    ('O' << 24 | 'K' << 16)
-#define SIM_RESPONSE_CODE_ERROR ('E' << 24 | 'R' << 16 | 'R' << 8 | 'O')
-#define SIM_RESPONSE_CODE_RING  ('R' << 24 | 'I' << 16 | 'N' << 8 | 'G')
-#define SIM_RESPONSE_CODE_CLIP  ('C' << 24 | 'L' << 16 | 'I' << 8 | 'P')
-#define SIM_RESPONSE_CODE_CREG  ('C' << 24 | 'R' << 16 | 'E' << 8 | 'G')
-#define SIM_RESPONSE_CODE_CSQ   ('C' << 24 | 'S' << 16 | 'Q' << 8 | ':')
-#define SIM_RESPONSE_CODE_CMT   ('C' << 24 | 'M' << 16 | 'T' << 8 | ':')
-
-typedef enum  {
-    SIM_TX_FLAG                 = (1 << 0),
-    SIM_TX_FLAG_FAILSAFE        = (1 << 1),
-    SIM_TX_FLAG_GPS             = (1 << 2),
-    SIM_TX_FLAG_ACC             = (1 << 3),
-    SIM_TX_FLAG_LOW_ALT         = (1 << 4),
-    SIM_TX_FLAG_RESPONSE        = (1 << 5)
-} simTxFlags_e;
-
 #define SIM_N_TX_FLAGS 5
 #define SIM_DEFAULT_TX_FLAGS "f"
 
-typedef enum  {
-    SIM_MODULE_NOT_DETECTED = 0,
-    SIM_MODULE_NOT_REGISTERED,
-    SIM_MODULE_REGISTERED,
-} simModuleState_e;
-
-typedef enum  {
-    SIM_STATE_INIT = 0,
-    SIM_STATE_INIT2,
-    SIM_STATE_INIT_ENTER_PIN,
-    SIM_STATE_SET_MODES,
-    SIM_STATE_SEND_SMS,
-    SIM_STATE_SEND_SMS_ENTER_MESSAGE
-} simTelemetryState_e;
-
-typedef enum  {
-    SIM_AT_OK = 0,
-    SIM_AT_ERROR,
-    SIM_AT_WAITING_FOR_RESPONSE
-} simATCommandState_e;
-
-typedef enum  {
-    SIM_READSTATE_RESPONSE = 0,
-    SIM_READSTATE_SMS,
-    SIM_READSTATE_SKIP
-} simReadState_e;
-
-typedef enum  {
-    SIM_TX_NO = 0,
-    SIM_TX_FS,
-    SIM_TX
-} simTransmissionState_e;
-
-typedef enum {
-    ACC_EVENT_NONE = 0,
-    ACC_EVENT_HIGH,
-    ACC_EVENT_LOW,
-    ACC_EVENT_NEG_X
-} accEvent_t;
-
 
 void handleSimTelemetry(void);
-void freeSimTelemetryPort(void);
 void initSimTelemetry(void);
 void checkSimTelemetryState(void);
-void configureSimTelemetryPort(void);