From 564e0c94b835cc3d7622673ad521324db3514a5d Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Sun, 25 Jun 2017 06:55:17 +0100
Subject: [PATCH 01/15] Move some GPS variables into struct. Some alignment
 with iNav

---
 src/main/blackbox/blackbox.c             | 23 ++++++------
 src/main/fc/fc_core.c                    |  2 +-
 src/main/fc/fc_msp.c                     | 34 ++++++++---------
 src/main/flight/imu.c                    |  4 +-
 src/main/flight/navigation.c             | 47 +++++++++++++-----------
 src/main/io/beeper.c                     |  4 +-
 src/main/io/dashboard.c                  |  8 ++--
 src/main/io/gps.c                        | 40 ++++++++------------
 src/main/io/gps.h                        | 24 ++++++++----
 src/main/io/ledstrip.c                   |  4 +-
 src/main/io/osd.c                        | 10 ++---
 src/main/target/COLIBRI_RACE/i2c_bst.c   | 14 +++----
 src/main/telemetry/crsf.c                | 16 ++++----
 src/main/telemetry/frsky.c               | 17 +++++----
 src/main/telemetry/hott.c                | 12 +++---
 src/main/telemetry/ltm.c                 | 14 +++----
 src/main/telemetry/mavlink.c             | 30 +++++++--------
 src/main/telemetry/smartport.c           | 14 +++----
 src/test/unit/flight_imu_unittest.cc     | 24 ++++++------
 src/test/unit/ledstrip_unittest.cc       | 19 +++++-----
 src/test/unit/telemetry_crsf_unittest.cc | 24 ++++--------
 src/test/unit/telemetry_hott_unittest.cc |  5 +--
 22 files changed, 192 insertions(+), 197 deletions(-)

diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index acb8b3629..c42eca5a8 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -992,16 +992,16 @@ static void writeGPSFrame(timeUs_t currentTimeUs)
         blackboxWriteUnsignedVB(currentTimeUs - blackboxHistory[1]->time);
     }
 
-    blackboxWriteUnsignedVB(GPS_numSat);
-    blackboxWriteSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]);
-    blackboxWriteSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]);
-    blackboxWriteUnsignedVB(GPS_altitude);
-    blackboxWriteUnsignedVB(GPS_speed);
-    blackboxWriteUnsignedVB(GPS_ground_course);
+    blackboxWriteUnsignedVB(gpsSol.numSat);
+    blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[LAT]);
+    blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[LON]);
+    blackboxWriteUnsignedVB(gpsSol.llh.alt);
+    blackboxWriteUnsignedVB(gpsSol.groundSpeed);
+    blackboxWriteUnsignedVB(gpsSol.groundCourse);
 
-    gpsHistory.GPS_numSat = GPS_numSat;
-    gpsHistory.GPS_coord[0] = GPS_coord[0];
-    gpsHistory.GPS_coord[1] = GPS_coord[1];
+    gpsHistory.GPS_numSat = gpsSol.numSat;
+    gpsHistory.GPS_coord[LAT] = gpsSol.llh.lat;
+    gpsHistory.GPS_coord[LON] = gpsSol.llh.lon;
 }
 #endif
 
@@ -1474,8 +1474,9 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
             if (blackboxShouldLogGpsHomeFrame()) {
                 writeGPSHomeFrame();
                 writeGPSFrame(currentTimeUs);
-            } else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0]
-                    || GPS_coord[1] != gpsHistory.GPS_coord[1]) {
+            } else if (gpsSol.numSat != gpsHistory.GPS_numSat
+                    || gpsSol.llh.lat != gpsHistory.GPS_coord[LAT]
+                    || gpsSol.llh.lon != gpsHistory.GPS_coord[LON]) {
                 //We could check for velocity changes as well but I doubt it changes independent of position
                 writeGPSFrame(currentTimeUs);
             }
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 90a5c22e5..50e126a39 100644
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -231,7 +231,7 @@ void mwArm(void)
 
             //beep to indicate arming
 #ifdef GPS
-            if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5)
+            if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5)
                 beeper(BEEPER_ARMING_GPS_FIX);
             else
                 beeper(BEEPER_ARMING);
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index ce8a74520..53774dc22 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1113,12 +1113,12 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
 
     case MSP_RAW_GPS:
         sbufWriteU8(dst, STATE(GPS_FIX));
-        sbufWriteU8(dst, GPS_numSat);
-        sbufWriteU32(dst, GPS_coord[LAT]);
-        sbufWriteU32(dst, GPS_coord[LON]);
-        sbufWriteU16(dst, GPS_altitude);
-        sbufWriteU16(dst, GPS_speed);
-        sbufWriteU16(dst, GPS_ground_course);
+        sbufWriteU8(dst, gpsSol.numSat);
+        sbufWriteU32(dst, gpsSol.llh.lat);
+        sbufWriteU32(dst, gpsSol.llh.lon);
+        sbufWriteU16(dst, gpsSol.llh.alt);
+        sbufWriteU16(dst, gpsSol.groundSpeed);
+        sbufWriteU16(dst, gpsSol.groundCourse);
         break;
 
     case MSP_COMP_GPS:
@@ -1129,12 +1129,12 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
 
     case MSP_GPSSVINFO:
         sbufWriteU8(dst, GPS_numCh);
-           for (int i = 0; i < GPS_numCh; i++) {
-               sbufWriteU8(dst, GPS_svinfo_chn[i]);
-               sbufWriteU8(dst, GPS_svinfo_svid[i]);
-               sbufWriteU8(dst, GPS_svinfo_quality[i]);
-               sbufWriteU8(dst, GPS_svinfo_cno[i]);
-           }
+       for (int i = 0; i < GPS_numCh; i++) {
+           sbufWriteU8(dst, GPS_svinfo_chn[i]);
+           sbufWriteU8(dst, GPS_svinfo_svid[i]);
+           sbufWriteU8(dst, GPS_svinfo_quality[i]);
+           sbufWriteU8(dst, GPS_svinfo_cno[i]);
+       }
         break;
 #endif
 
@@ -1838,11 +1838,11 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         } else {
             DISABLE_STATE(GPS_FIX);
         }
-        GPS_numSat = sbufReadU8(src);
-        GPS_coord[LAT] = sbufReadU32(src);
-        GPS_coord[LON] = sbufReadU32(src);
-        GPS_altitude = sbufReadU16(src);
-        GPS_speed = sbufReadU16(src);
+        gpsSol.numSat = sbufReadU8(src);
+        gpsSol.llh.lat = sbufReadU32(src);
+        gpsSol.llh.lon = sbufReadU32(src);
+        gpsSol.llh.alt = sbufReadU16(src);
+        gpsSol.groundSpeed = sbufReadU16(src);
         GPS_update |= 2;        // New data signalisation to GPS functions // FIXME Magic Numbers
         break;
 
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 4056e9ce5..efa4b853c 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -419,9 +419,9 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
         useMag = true;
     }
 #if defined(GPS)
-    else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5 && GPS_speed >= 300) {
+    else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= 300) {
         // In case of a fixed-wing aircraft we can use GPS course over ground to correct heading
-        rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - GPS_ground_course);
+        rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - gpsSol.groundCourse);
         useYaw = true;
     }
 #endif
diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c
index 1ee364b87..c34b66819 100644
--- a/src/main/flight/navigation.c
+++ b/src/main/flight/navigation.c
@@ -254,7 +254,7 @@ void GPS_calculateDistanceAndDirectionToHome(void)
     if (STATE(GPS_FIX_HOME)) {      // If we don't have home set, do not display anything
         uint32_t dist;
         int32_t dir;
-        GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
+        GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
         GPS_distanceToHome = dist / 100;
         GPS_directionToHome = dir / 100;
     } else {
@@ -270,7 +270,7 @@ void onGpsNewData(void)
     uint16_t speed;
 
 
-    if (!(STATE(GPS_FIX) && GPS_numSat >= 5)) {
+    if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
         return;
     }
 
@@ -284,7 +284,7 @@ void onGpsNewData(void)
 #if defined(GPS_FILTERING)
     GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
     for (axis = 0; axis < 2; axis++) {
-        GPS_read[axis] = GPS_coord[axis];               // latest unfiltered data is in GPS_latitude and GPS_longitude
+        GPS_read[axis] = axis == LAT ? gpsSol.llh.lat : gpsSol.llh.lon; // latest unfiltered data is in GPS_latitude and GPS_longitude
         GPS_degree[axis] = GPS_read[axis] / 10000000;   // get the degree to assure the sum fits to the int32_t
 
         // How close we are to a degree line ? its the first three digits from the fractions of degree
@@ -296,8 +296,13 @@ void onGpsNewData(void)
         GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
         GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000);
         if (nav_mode == NAV_MODE_POSHOLD) {             // we use gps averaging only in poshold mode...
-            if (fraction3[axis] > 1 && fraction3[axis] < 999)
-                GPS_coord[axis] = GPS_filtered[axis];
+            if (fraction3[axis] > 1 && fraction3[axis] < 999) {
+                if (axis == LAT) {
+                    gpsSol.llh.lat = GPS_filtered[LAT];
+                } else {
+                    gpsSol.llh.lon = GPS_filtered[LON];
+                }
+            }
         }
     }
 #endif
@@ -320,8 +325,8 @@ void onGpsNewData(void)
         // we are navigating
 
         // gps nav calculations, these are common for nav and poshold
-        GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
-        GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);
+        GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
+        GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &gpsSol.llh.lat, &gpsSol.llh.lon);
 
         switch (nav_mode) {
         case NAV_MODE_POSHOLD:
@@ -359,10 +364,10 @@ void onGpsNewData(void)
 
 void GPS_reset_home_position(void)
 {
-    if (STATE(GPS_FIX) && GPS_numSat >= 5) {
-        GPS_home[LAT] = GPS_coord[LAT];
-        GPS_home[LON] = GPS_coord[LON];
-        GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc
+    if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
+        GPS_home[LAT] = gpsSol.llh.lat;
+        GPS_home[LON] = gpsSol.llh.lon;
+        GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc
         nav_takeoff_bearing = DECIDEGREES_TO_DEGREES(attitude.values.yaw);              // save takeoff heading
         // Set ground altitude
         ENABLE_STATE(GPS_FIX_HOME);
@@ -429,10 +434,10 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon)
     GPS_WP[LON] = *lon;
 
     GPS_calc_longitude_scaling(*lat);
-    GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
+    GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
 
     nav_bearing = target_bearing;
-    GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);
+    GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &gpsSol.llh.lat, &gpsSol.llh.lon);
     original_target_bearing = target_bearing;
     waypoint_speed_gov = navigationConfig()->nav_speed_min;
 }
@@ -484,13 +489,11 @@ static void GPS_calc_velocity(void)
     static int16_t speed_old[2] = { 0, 0 };
     static int32_t last_coord[2] = { 0, 0 };
     static uint8_t init = 0;
-    // y_GPS_speed positive = Up
-    // x_GPS_speed positive = Right
 
     if (init) {
         float tmp = 1.0f / dTnav;
-        actual_speed[GPS_X] = (float)(GPS_coord[LON] - last_coord[LON]) * GPS_scaleLonDown * tmp;
-        actual_speed[GPS_Y] = (float)(GPS_coord[LAT] - last_coord[LAT]) * tmp;
+        actual_speed[GPS_X] = (float)(gpsSol.llh.lon - last_coord[LON]) * GPS_scaleLonDown * tmp;
+        actual_speed[GPS_Y] = (float)(gpsSol.llh.lat - last_coord[LAT]) * tmp;
 
         actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
         actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
@@ -500,8 +503,8 @@ static void GPS_calc_velocity(void)
     }
     init = 1;
 
-    last_coord[LON] = GPS_coord[LON];
-    last_coord[LAT] = GPS_coord[LAT];
+    last_coord[LON] = gpsSol.llh.lon;
+    last_coord[LAT] = gpsSol.llh.lat;
 }
 
 ////////////////////////////////////////////////////////////////////////////////////
@@ -666,7 +669,7 @@ void updateGpsWaypointsAndMode(void)
     bool resetNavNow = false;
     static bool gpsReadyBeepDone = false;
 
-    if (STATE(GPS_FIX) && GPS_numSat >= 5) {
+    if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
 
         //
         // process HOME mode
@@ -701,8 +704,8 @@ void updateGpsWaypointsAndMode(void)
 
                     // Transition to HOLD mode
                     ENABLE_FLIGHT_MODE(GPS_HOLD_MODE);
-                    GPS_hold[LAT] = GPS_coord[LAT];
-                    GPS_hold[LON] = GPS_coord[LON];
+                    GPS_hold[LAT] = gpsSol.llh.lat;
+                    GPS_hold[LON] = gpsSol.llh.lon;
                     GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
                     nav_mode = NAV_MODE_POSHOLD;
                     resetNavNow = true;
diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c
index 3b35e4be8..7b3a9d6e7 100644
--- a/src/main/io/beeper.c
+++ b/src/main/io/beeper.c
@@ -295,12 +295,12 @@ void beeperConfirmationBeeps(uint8_t beepCount)
 void beeperGpsStatus(void)
 {
     // if GPS fix then beep out number of satellites
-    if (STATE(GPS_FIX) && GPS_numSat >= 5) {
+    if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
         uint8_t i = 0;
         do {
             beep_multiBeeps[i++] = 5;
             beep_multiBeeps[i++] = 10;
-        } while (i < MAX_MULTI_BEEPS && GPS_numSat > i / 2);
+        } while (i < MAX_MULTI_BEEPS && gpsSol.numSat > i / 2);
 
         beep_multiBeeps[i-1] = 50; // extend last pause
         beep_multiBeeps[i] = BEEPER_COMMAND_STOP;
diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c
index 129b62976..f54511fb5 100644
--- a/src/main/io/dashboard.c
+++ b/src/main/io/dashboard.c
@@ -365,22 +365,22 @@ void showGpsPage() {
 
 
     char fixChar = STATE(GPS_FIX) ? 'Y' : 'N';
-    tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", GPS_numSat, fixChar);
+    tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", gpsSol.numSat, fixChar);
     padLineBuffer();
     i2c_OLED_set_line(rowIndex++);
     i2c_OLED_send_string(lineBuffer);
 
-    tfp_sprintf(lineBuffer, "La/Lo: %d/%d", GPS_coord[LAT] / GPS_DEGREES_DIVIDER, GPS_coord[LON] / GPS_DEGREES_DIVIDER);
+    tfp_sprintf(lineBuffer, "La/Lo: %d/%d", gpsSol.llh.lat / GPS_DEGREES_DIVIDER, gpsSol.llh.lon / GPS_DEGREES_DIVIDER);
     padLineBuffer();
     i2c_OLED_set_line(rowIndex++);
     i2c_OLED_send_string(lineBuffer);
 
-    tfp_sprintf(lineBuffer, "Spd: %d", GPS_speed);
+    tfp_sprintf(lineBuffer, "Spd: %d", gpsSol.groundSpeed);
     padHalfLineBuffer();
     i2c_OLED_set_line(rowIndex);
     i2c_OLED_send_string(lineBuffer);
 
-    tfp_sprintf(lineBuffer, "GC: %d", GPS_ground_course);
+    tfp_sprintf(lineBuffer, "GC: %d", gpsSol.groundCourse);
     padHalfLineBuffer();
     i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
     i2c_OLED_send_string(lineBuffer);
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index 260341c66..46b086be6 100755
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -71,18 +71,11 @@ static char *gpsPacketLogChar = gpsPacketLog;
 // **********************
 // GPS
 // **********************
-int32_t GPS_coord[2];               // LAT/LON
-
-uint8_t GPS_numSat;
-uint16_t GPS_hdop = 9999;           // Compute GPS quality signal
+gpsSolutionData_t gpsSol;
 uint32_t GPS_packetCount = 0;
 uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received.
 uint8_t GPS_update = 0;             // it's a binary toggle to distinct a GPS position update
 
-uint16_t GPS_altitude;              // altitude in 0.1m
-uint16_t GPS_speed;                 // speed in 0.1m/s
-uint16_t GPS_ground_course = 0;     // degrees * 10
-
 uint8_t GPS_numCh;                          // Number of channels
 uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS];     // Channel number
 uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS];    // Satellite ID
@@ -424,7 +417,7 @@ void gpsInitHardware(void)
 static void updateGpsIndicator(timeUs_t currentTimeUs)
 {
     static uint32_t GPSLEDTime;
-    if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) {
+    if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (gpsSol.numSat >= 5)) {
         GPSLEDTime = currentTimeUs + 150000;
         LED1_TOGGLE;
     }
@@ -456,8 +449,7 @@ void gpsUpdate(timeUs_t currentTimeUs)
                 gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
             }
             gpsData.lastMessage = millis();
-            // TODO - move some / all of these into gpsData
-            GPS_numSat = 0;
+            gpsSol.numSat = 0;
             DISABLE_STATE(GPS_FIX);
             gpsSetState(GPS_INITIALIZING);
             break;
@@ -746,16 +738,16 @@ static bool gpsNewFrameNMEA(char c)
                       *gpsPacketLogChar = LOG_NMEA_GGA;
                       frameOK = 1;
                       if (STATE(GPS_FIX)) {
-                            GPS_coord[LAT] = gps_Msg.latitude;
-                            GPS_coord[LON] = gps_Msg.longitude;
-                            GPS_numSat = gps_Msg.numSat;
-                            GPS_altitude = gps_Msg.altitude;
+                            gpsSol.llh.lat = gps_Msg.latitude;
+                            gpsSol.llh.lon = gps_Msg.longitude;
+                            gpsSol.numSat = gps_Msg.numSat;
+                            gpsSol.llh.alt = gps_Msg.altitude;
                         }
                         break;
                     case FRAME_RMC:
                         *gpsPacketLogChar = LOG_NMEA_RMC;
-                        GPS_speed = gps_Msg.speed;
-                        GPS_ground_course = gps_Msg.ground_course;
+                        gpsSol.groundSpeed = gps_Msg.speed;
+                        gpsSol.groundCourse = gps_Msg.ground_course;
                         break;
                     } // end switch
                 } else {
@@ -952,9 +944,9 @@ static bool UBLOX_parse_gps(void)
     case MSG_POSLLH:
         *gpsPacketLogChar = LOG_UBLOX_POSLLH;
         //i2c_dataset.time                = _buffer.posllh.time;
-        GPS_coord[LON] = _buffer.posllh.longitude;
-        GPS_coord[LAT] = _buffer.posllh.latitude;
-        GPS_altitude = _buffer.posllh.altitude_msl / 10 / 100;  //alt in m
+        gpsSol.llh.lon = _buffer.posllh.longitude;
+        gpsSol.llh.lat = _buffer.posllh.latitude;
+        gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10 / 100;  //alt in m
         if (next_fix) {
             ENABLE_STATE(GPS_FIX);
         } else {
@@ -973,14 +965,14 @@ static bool UBLOX_parse_gps(void)
         next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
         if (!next_fix)
             DISABLE_STATE(GPS_FIX);
-        GPS_numSat = _buffer.solution.satellites;
-        GPS_hdop = _buffer.solution.position_DOP;
+        gpsSol.numSat = _buffer.solution.satellites;
+        gpsSol.hdop = _buffer.solution.position_DOP;
         break;
     case MSG_VELNED:
         *gpsPacketLogChar = LOG_UBLOX_VELNED;
         // speed_3d                        = _buffer.velned.speed_3d;  // cm/s
-        GPS_speed = _buffer.velned.speed_2d;    // cm/s
-        GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
+        gpsSol.groundSpeed = _buffer.velned.speed_2d;    // cm/s
+        gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000);     // Heading 2D deg * 100000 rescaled to deg * 10
         _new_speed = true;
         break;
     case MSG_SVINFO:
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index 81ee218e4..d33f04438 100644
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -31,8 +31,6 @@ typedef enum {
     GPS_UBLOX
 } gpsProvider_e;
 
-#define GPS_PROVIDER_MAX GPS_UBLOX
-
 typedef enum {
     SBAS_AUTO = 0,
     SBAS_EGNOS,
@@ -77,6 +75,21 @@ typedef struct gpsCoordinateDDDMMmmmm_s {
     int16_t mmmm;
 } gpsCoordinateDDDMMmmmm_t;
 
+/* LLH Location in NEU axis system */
+typedef struct gpsLocation_s {
+    int32_t lat;                    // latitude * 1e+7
+    int32_t lon;                    // longitude * 1e+7
+    uint16_t alt;                   // altitude in 0.1m
+} gpsLocation_t;
+
+typedef struct gpsSolutionData_s {
+    uint8_t numSat;
+    gpsLocation_t llh;
+    uint16_t GPS_altitude;          // altitude in 0.1m
+    uint16_t groundSpeed;           // speed in 0.1m/s
+    uint16_t groundCourse;          // degrees * 10
+    uint16_t hdop;                  // generic HDOP value (*100)
+} gpsSolutionData_t;
 
 typedef enum {
     GPS_MESSAGE_STATE_IDLE = 0,
@@ -102,16 +115,11 @@ typedef struct gpsData_s {
 extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
 
 extern gpsData_t gpsData;
-extern int32_t GPS_coord[2];               // LAT/LON
+extern gpsSolutionData_t gpsSol;
 
-extern uint8_t GPS_numSat;
-extern uint16_t GPS_hdop;                  // GPS signal quality
 extern uint8_t GPS_update;                 // it's a binary toogle to distinct a GPS position update
 extern uint32_t GPS_packetCount;
 extern uint32_t GPS_svInfoReceivedCount;
-extern uint16_t GPS_altitude;              // altitude in 0.1m
-extern uint16_t GPS_speed;                 // speed in 0.1m/s
-extern uint16_t GPS_ground_course;         // degrees * 10
 extern uint8_t GPS_numCh;                  // Number of channels
 extern uint8_t GPS_svinfo_chn[16];         // Channel number
 extern uint8_t GPS_svinfo_svid[16];        // Satellite ID
diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c
index 0be72d3a9..6baa47362 100644
--- a/src/main/io/ledstrip.c
+++ b/src/main/io/ledstrip.c
@@ -731,7 +731,7 @@ static void applyLedGpsLayer(bool updateNow, timeUs_t *timer)
         static uint8_t gpsFlashCounter = 0;
         if (gpsPauseCounter > 0) {
             gpsPauseCounter--;
-        } else if (gpsFlashCounter >= GPS_numSat) {
+        } else if (gpsFlashCounter >= gpsSol.numSat) {
             gpsFlashCounter = 0;
             gpsPauseCounter = blinkPauseLength;
         } else {
@@ -743,7 +743,7 @@ static void applyLedGpsLayer(bool updateNow, timeUs_t *timer)
 
     const hsvColor_t *gpsColor;
 
-    if (GPS_numSat == 0 || !sensors(SENSOR_GPS)) {
+    if (gpsSol.numSat == 0 || !sensors(SENSOR_GPS)) {
         gpsColor = getSC(LED_SCOLOR_GPSNOSATS);
     } else {
         bool colorOn = gpsPauseCounter == 0;  // each interval starts with pause
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index dcbd7257f..2a9262bad 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -277,12 +277,12 @@ static void osdDrawSingleElement(uint8_t item)
 #ifdef GPS
     case OSD_GPS_SATS:
         buff[0] = 0x1f;
-        tfp_sprintf(buff + 1, "%d", GPS_numSat);
+        tfp_sprintf(buff + 1, "%d", gpsSol.numSat);
         break;
 
     case OSD_GPS_SPEED:
         // FIXME ideally we want to use SYM_KMH symbol but it's not in the font any more, so we use K.
-        tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(GPS_speed));
+        tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(gpsSol.groundSpeed));
         break;
 
     case OSD_GPS_LAT:
@@ -291,10 +291,10 @@ static void osdDrawSingleElement(uint8_t item)
             int32_t val;
             if (item == OSD_GPS_LAT) {
                 buff[0] = SYM_ARROW_EAST;
-                val = GPS_coord[LAT];
+                val = gpsSol.llh.lat;
             } else {
                 buff[0] = SYM_ARROW_SOUTH;
-                val = GPS_coord[LON];
+                val = gpsSol.llh.lon;
             }
 
             char wholeDegreeString[5];
@@ -880,7 +880,7 @@ static void osdUpdateStats(void)
 {
     int16_t value = 0;
 #ifdef GPS
-    value = CM_S_TO_KM_H(GPS_speed);
+    value = CM_S_TO_KM_H(gpsSol.groundSpeed);
 #endif
     if (stats.max_speed < value)
         stats.max_speed = value;
diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c
index 9da520de1..ca870492e 100644
--- a/src/main/target/COLIBRI_RACE/i2c_bst.c
+++ b/src/main/target/COLIBRI_RACE/i2c_bst.c
@@ -778,15 +778,15 @@ static uint8_t numOfSat = 0;
 #ifdef GPS
 bool writeGpsPositionPrameToBST(void)
 {
-    if((lat != GPS_coord[LAT]) || (lon != GPS_coord[LON]) || (alt != GPS_altitude) || (numOfSat != GPS_numSat)) {
-        lat = GPS_coord[LAT];
-        lon = GPS_coord[LON];
-        alt = GPS_altitude;
-        numOfSat = GPS_numSat;
-        uint16_t speed = (GPS_speed * 9 / 25);
+    if((lat != gpsSol.llh.lat) || (lon != gpsSol.llh.lon) || (alt != gpsSol.llh.alt) || (numOfSat != gpsSol.numSat)) {
+        lat = gpsSol.llh.lat;
+        lon = gpsSol.llh.lon;
+        alt = gpsSol.llh.alt;
+        numOfSat = gpsSol.numSat;
+        uint16_t speed = (gpsSol.groundSpeed * 9 / 25);
         uint16_t gpsHeading = 0;
         uint16_t altitude = 0;
-        gpsHeading = GPS_ground_course * 10;
+        gpsHeading = gpsSol.groundCourse * 10;
         altitude = alt * 10 + 1000;
 
         bstMasterStartBuffer(PUBLIC_ADDRESS);
diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index 5f32767a6..1b0a0ab1e 100644
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -111,7 +111,7 @@ int32_t     Latitude ( degree / 10`000`000 )
 int32_t     Longitude (degree / 10`000`000 )
 uint16_t    Groundspeed ( km/h / 10 )
 uint16_t    GPS heading ( degree / 100 )
-uint16      Altitude ( meter ­ 1000m offset )
+uint16      Altitude ( meter ­1000m offset )
 uint8_t     Satellites in use ( counter )
 */
 void crsfFrameGps(sbuf_t *dst)
@@ -119,14 +119,14 @@ void crsfFrameGps(sbuf_t *dst)
     // use sbufWrite since CRC does not include frame length
     sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
     sbufWriteU8(dst, CRSF_FRAMETYPE_GPS);
-    sbufWriteU32BigEndian(dst, GPS_coord[LAT]); // CRSF and betaflight use same units for degrees
-    sbufWriteU32BigEndian(dst, GPS_coord[LON]);
-    sbufWriteU16BigEndian(dst, (GPS_speed * 36 + 5) / 10); // GPS_speed is in 0.1m/s
-    sbufWriteU16BigEndian(dst, GPS_ground_course * 10); // GPS_ground_course is degrees * 10
+    sbufWriteU32BigEndian(dst, gpsSol.llh.lat); // CRSF and betaflight use same units for degrees
+    sbufWriteU32BigEndian(dst, gpsSol.llh.lon);
+    sbufWriteU16BigEndian(dst, (gpsSol.groundSpeed * 36 + 5) / 10); // gpsSol.groundSpeed is in 0.1m/s
+    sbufWriteU16BigEndian(dst, gpsSol.groundCourse * 10); // gpsSol.groundCourse is degrees * 10
     //Send real GPS altitude only if it's reliable (there's a GPS fix)
-    const uint16_t altitude = (STATE(GPS_FIX) ? GPS_altitude : 0) + 1000;
+    const uint16_t altitude = (STATE(GPS_FIX) ? gpsSol.llh.alt : 0) + 1000;
     sbufWriteU16BigEndian(dst, altitude);
-    sbufWriteU8(dst, GPS_numSat);
+    sbufWriteU8(dst, gpsSol.numSat);
 }
 
 /*
@@ -204,7 +204,7 @@ void crsfFrameAttitude(sbuf_t *dst)
 /*
 0x21 Flight mode text based
 Payload:
-char[]      Flight mode ( Null­terminated string )
+char[]      Flight mode ( Null terminated string )
 */
 void crsfFrameFlightMode(sbuf_t *dst)
 {
diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c
index 60a280c43..39451a247 100644
--- a/src/main/telemetry/frsky.c
+++ b/src/main/telemetry/frsky.c
@@ -183,7 +183,7 @@ static void sendBaro(void)
 #ifdef GPS
 static void sendGpsAltitude(void)
 {
-    uint16_t altitude = GPS_altitude;
+    uint16_t altitude = gpsSol.llh.alt;
     //Send real GPS altitude only if it's reliable (there's a GPS fix)
     if (!STATE(GPS_FIX)) {
         altitude = 0;
@@ -230,9 +230,9 @@ static void sendTemperature1(void)
 #ifdef GPS
 static void sendSatalliteSignalQualityAsTemperature2(void)
 {
-    uint16_t satellite = GPS_numSat;
-    if (GPS_hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s
-        satellite = constrain(GPS_hdop, 0, GPS_MAX_HDOP_VAL);
+    uint16_t satellite = gpsSol.numSat;
+    if (gpsSol.hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s
+        satellite = constrain(gpsSol.hdop, 0, GPS_MAX_HDOP_VAL);
     }
     sendDataHead(ID_TEMPRATURE2);
 
@@ -254,9 +254,9 @@ static void sendSpeed(void)
     //Speed should be sent in knots (GPS speed is in cm/s)
     sendDataHead(ID_GPS_SPEED_BP);
     //convert to knots: 1cm/s = 0.0194384449 knots
-    serialize16(GPS_speed * 1944 / 100000);
+    serialize16(gpsSol.groundSpeed * 1944 / 100000);
     sendDataHead(ID_GPS_SPEED_AP);
-    serialize16((GPS_speed * 1944 / 100) % 100);
+    serialize16((gpsSol.groundSpeed * 1944 / 100) % 100);
 }
 #endif
 
@@ -337,11 +337,14 @@ static void sendFakeLatLong(void)
 static void sendGPSLatLong(void)
 {
     static uint8_t gpsFixOccured = 0;
+    int32_t coord[2] = {0,0};
 
     if (STATE(GPS_FIX) || gpsFixOccured == 1) {
         // If we have ever had a fix, send the last known lat/long
         gpsFixOccured = 1;
-        sendLatLong(GPS_coord);
+        coord[LAT] = gpsSol.llh.lat;
+        coord[LON] = gpsSol.llh.lon;
+        sendLatLong(coord);
     } else {
         // otherwise send fake lat/long in order to display compass value
         sendFakeLatLong();
diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c
index c2c3c0907..4f5d893bf 100644
--- a/src/main/telemetry/hott.c
+++ b/src/main/telemetry/hott.c
@@ -185,35 +185,35 @@ void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t
 
 void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
 {
-    hottGPSMessage->gps_satelites = GPS_numSat;
+    hottGPSMessage->gps_satelites = gpsSol.numSat;
 
     if (!STATE(GPS_FIX)) {
         hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE;
         return;
     }
 
-    if (GPS_numSat >= 5) {
+    if (gpsSol.numSat >= 5) {
         hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D;
     } else {
         hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_2D;
     }
 
-    addGPSCoordinates(hottGPSMessage, GPS_coord[LAT], GPS_coord[LON]);
+    addGPSCoordinates(hottGPSMessage, gpsSol.llh.lat, gpsSol.llh.lon);
 
     // GPS Speed is returned in cm/s (from io/gps.c) and must be sent in km/h (Hott requirement)
-    const uint16_t speed = (GPS_speed * 36) / 1000;
+    const uint16_t speed = (gpsSol.groundSpeed * 36) / 1000;
     hottGPSMessage->gps_speed_L = speed & 0x00FF;
     hottGPSMessage->gps_speed_H = speed >> 8;
 
     hottGPSMessage->home_distance_L = GPS_distanceToHome & 0x00FF;
     hottGPSMessage->home_distance_H = GPS_distanceToHome >> 8;
 
-    uint16_t altitude = GPS_altitude;
+    uint16_t altitude = gpsSol.llh.alt;
     if (!STATE(GPS_FIX)) {
         altitude = getEstimatedAltitude() / 100;
     }
 
-    const uint16_t hottGpsAltitude = (altitude) + HOTT_GPS_ALTITUDE_OFFSET; // GPS_altitude in m ; offset = 500 -> O m
+    const uint16_t hottGpsAltitude = (altitude) + HOTT_GPS_ALTITUDE_OFFSET; // gpsSol.llh.alt in m ; offset = 500 -> O m
 
     hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF;
     hottGPSMessage->altitude_H = hottGpsAltitude >> 8;
diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c
index c5997f630..c4cfed818 100644
--- a/src/main/telemetry/ltm.c
+++ b/src/main/telemetry/ltm.c
@@ -133,23 +133,23 @@ static void ltm_gframe(void)
 
     if (!STATE(GPS_FIX))
         gps_fix_type = 1;
-    else if (GPS_numSat < 5)
+    else if (gpsSol.numSat < 5)
         gps_fix_type = 2;
     else
         gps_fix_type = 3;
 
     ltm_initialise_packet('G');
-    ltm_serialise_32(GPS_coord[LAT]);
-    ltm_serialise_32(GPS_coord[LON]);
-    ltm_serialise_8((uint8_t)(GPS_speed / 100));
+    ltm_serialise_32(gpsSol.llh.lat);
+    ltm_serialise_32(gpsSol.llh.lon);
+    ltm_serialise_8((uint8_t)(gpsSol.groundSpeed / 100));
 
 #if defined(BARO) || defined(SONAR)
-    ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() : GPS_altitude * 100;
+    ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() : gpsSol.llh.alt * 100;
 #else
-    ltm_alt = GPS_altitude * 100;
+    ltm_alt = gpsSol.llh.alt * 100;
 #endif
     ltm_serialise_32(ltm_alt);
-    ltm_serialise_8((GPS_numSat << 2) | gps_fix_type);
+    ltm_serialise_8((gpsSol.numSat << 2) | gps_fix_type);
     ltm_finalise();
 #endif
 }
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 1cedab5bb..6b0ae90e5 100755
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -286,7 +286,7 @@ void mavlinkSendPosition(void)
         gpsFixType = 1;
     }
     else {
-        if (GPS_numSat < 5) {
+        if (gpsSol.numSat < 5) {
             gpsFixType = 2;
         }
         else {
@@ -300,21 +300,21 @@ void mavlinkSendPosition(void)
         // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
         gpsFixType,
         // lat Latitude in 1E7 degrees
-        GPS_coord[LAT],
+        gpsSol.llh.lat,
         // lon Longitude in 1E7 degrees
-        GPS_coord[LON],
+        gpsSol.llh.lon,
         // alt Altitude in 1E3 meters (millimeters) above MSL
-        GPS_altitude * 1000,
+        gpsSol.llh.alt * 1000,
         // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
         65535,
         // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
         65535,
         // vel GPS ground speed (m/s * 100). If unknown, set to: 65535
-        GPS_speed,
+        gpsSol.groundSpeed,
         // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
-        GPS_ground_course * 10,
+        gpsSol.groundCourse * 10,
         // satellites_visible Number of satellites visible. If unknown, set to 255
-        GPS_numSat);
+        gpsSol.numSat);
     msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
     mavlinkSerialWrite(mavBuffer, msgLength);
 
@@ -323,16 +323,16 @@ void mavlinkSendPosition(void)
         // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
         micros(),
         // lat Latitude in 1E7 degrees
-        GPS_coord[LAT],
+        gpsSol.llh.lat,
         // lon Longitude in 1E7 degrees
-        GPS_coord[LON],
+        gpsSol.llh.lon,
         // alt Altitude in 1E3 meters (millimeters) above MSL
-        GPS_altitude * 1000,
+        gpsSol.llh.alt * 1000,
         // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
 #if defined(BARO) || defined(SONAR)
-        (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() * 10 : GPS_altitude * 1000,
+        (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() * 10 : gpsSol.llh.alt * 1000,
 #else
-        GPS_altitude * 1000,
+        gpsSol.llh.alt * 1000,
 #endif
         // Ground X Speed (Latitude), expressed as m/s * 100
         0,
@@ -391,7 +391,7 @@ void mavlinkSendHUDAndHeartbeat(void)
 #if defined(GPS)
     // use ground speed if source available
     if (sensors(SENSOR_GPS)) {
-        mavGroundSpeed = GPS_speed / 100.0f;
+        mavGroundSpeed = gpsSol.groundSpeed / 100.0f;
     }
 #endif
 
@@ -404,13 +404,13 @@ void mavlinkSendHUDAndHeartbeat(void)
 #if defined(GPS)
     else if (sensors(SENSOR_GPS)) {
         // No sonar or baro, just display altitude above MLS
-        mavAltitude = GPS_altitude;
+        mavAltitude = gpsSol.llh.alt;
     }
 #endif
 #elif defined(GPS)
     if (sensors(SENSOR_GPS)) {
         // No sonar or baro, just display altitude above MLS
-        mavAltitude = GPS_altitude;
+        mavAltitude = gpsSol.llh.alt;
     }
 #endif
 
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index 8a372c116..08f85752c 100644
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -602,7 +602,7 @@ void handleSmartPortTelemetry(void)
                 if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
                     //convert to knots: 1cm/s = 0.0194384449 knots
                     //Speed should be sent in knots/1000 (GPS speed is in cm/s)
-                    uint32_t tmpui = GPS_speed * 1944 / 100;
+                    uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100;
                     smartPortSendPackage(id, tmpui);
                     smartPortHasRequest = 0;
                 }
@@ -649,14 +649,14 @@ void handleSmartPortTelemetry(void)
                     // the MSB of the sent uint32_t helps FrSky keep track
                     // the even/odd bit of our counter helps us keep track
                     if (smartPortIdCnt & 1) {
-                        tmpui = abs(GPS_coord[LON]);  // now we have unsigned value and one bit to spare
+                        tmpui = abs(gpsSol.llh.lon);  // now we have unsigned value and one bit to spare
                         tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000;  // 6/100 = 1.5/25, division by power of 2 is fast
-                        if (GPS_coord[LON] < 0) tmpui |= 0x40000000;
+                        if (gpsSol.llh.lon < 0) tmpui |= 0x40000000;
                     }
                     else {
-                        tmpui = abs(GPS_coord[LAT]);  // now we have unsigned value and one bit to spare
+                        tmpui = abs(gpsSol.llh.lat);  // now we have unsigned value and one bit to spare
                         tmpui = (tmpui + tmpui / 2) / 25;  // 6/100 = 1.5/25, division by power of 2 is fast
-                        if (GPS_coord[LAT] < 0) tmpui |= 0x40000000;
+                        if (gpsSol.llh.lat < 0) tmpui |= 0x40000000;
                     }
                     smartPortSendPackage(id, tmpui);
                     smartPortHasRequest = 0;
@@ -735,7 +735,7 @@ void handleSmartPortTelemetry(void)
                 if (sensors(SENSOR_GPS)) {
 #ifdef GPS
                     // provide GPS lock status
-                    smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat);
+                    smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + gpsSol.numSat);
                     smartPortHasRequest = 0;
 #endif
                 } else if (feature(FEATURE_GPS)) {
@@ -776,7 +776,7 @@ void handleSmartPortTelemetry(void)
 #ifdef GPS
             case FSSP_DATAID_GPS_ALT    :
                 if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
-                    smartPortSendPackage(id, GPS_altitude * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7)
+                    smartPortSendPackage(id, gpsSol.llh.alt * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7)
                     smartPortHasRequest = 0;
                 }
                 break;
diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc
index b9fc11a32..290fb7265 100644
--- a/src/test/unit/flight_imu_unittest.cc
+++ b/src/test/unit/flight_imu_unittest.cc
@@ -35,22 +35,24 @@ extern "C" {
     #include "drivers/compass/compass.h"
     #include "drivers/sensor.h"
 
-    #include "sensors/sensors.h"
-    #include "sensors/gyro.h"
-    #include "sensors/compass.h"
-    #include "sensors/acceleration.h"
-    #include "sensors/barometer.h"
-
-    #include "fc/runtime_config.h"
     #include "fc/rc_controls.h"
     #include "fc/rc_modes.h"
-
-    #include "rx/rx.h"
+    #include "fc/runtime_config.h"
 
     #include "flight/mixer.h"
     #include "flight/pid.h"
     #include "flight/imu.h"
 
+    #include "io/gps.h"
+
+    #include "rx/rx.h"
+
+    #include "sensors/acceleration.h"
+    #include "sensors/barometer.h"
+    #include "sensors/compass.h"
+    #include "sensors/gyro.h"
+    #include "sensors/sensors.h"
+
     void imuComputeRotationMatrix(void);
     void imuUpdateEulerAngles(void);
 
@@ -206,9 +208,7 @@ gyro_t gyro;
 acc_t acc;
 mag_t mag;
 
-uint8_t GPS_numSat;
-uint16_t GPS_speed;
-uint16_t GPS_ground_course;
+gpsSolutionData_t gpsSol;
 
 uint8_t debugMode;
 int16_t debug[DEBUG16_VALUE_COUNT];
diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc
index b2b712bcb..058cf2131 100644
--- a/src/test/unit/ledstrip_unittest.cc
+++ b/src/test/unit/ledstrip_unittest.cc
@@ -24,27 +24,28 @@
 extern "C" {
     #include "build/build_config.h"
 
-    #include "common/color.h"
     #include "common/axis.h"
+    #include "common/color.h"
 
     #include "config/parameter_group_ids.h"
 
-    #include "sensors/battery.h"
-
     #include "drivers/io.h"
-    #include "rx/rx.h"
+    #include "drivers/light_ws2811strip.h"
+    #include "drivers/timer.h"
+    #include "drivers/timer_def.h"
 
     #include "fc/config.h"
     #include "fc/rc_controls.h"
     #include "fc/rc_modes.h"
     #include "fc/runtime_config.h"
 
-    #include "drivers/light_ws2811strip.h"
-    #include "drivers/timer.h"
-    #include "drivers/timer_def.h"
-
+    #include "io/gps.h"
     #include "io/ledstrip.h"
 
+    #include "rx/rx.h"
+
+    #include "sensors/battery.h"
+
     #include "target.h"
 }
 
@@ -298,7 +299,7 @@ float rcCommand[4];
 int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 uint32_t rcModeActivationMask;
 uint16_t rssi = 0;
-uint8_t GPS_numSat = 0;
+gpsSolutionData_t gpsSol;
 
 batteryState_e getBatteryState(void) {
     return BATTERY_OK;
diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc
index 0f4afaf44..661cb4569 100644
--- a/src/test/unit/telemetry_crsf_unittest.cc
+++ b/src/test/unit/telemetry_crsf_unittest.cc
@@ -82,12 +82,7 @@ uint16_t    Groundspeed ( km/h / 10 )
 uint16_t    GPS heading ( degree / 100 )
 uint16      Altitude ( meter ­ 1000m offset )
 uint8_t     Satellites in use ( counter )
-uint8_t GPS_numSat;
-int32_t GPS_coord[2];
 uint16_t GPS_distanceToHome;        // distance to home point in meters
-uint16_t GPS_altitude;              // altitude in m
-uint16_t GPS_speed;                 // speed in 0.1m/s
-uint16_t GPS_ground_course = 0;     // degrees * 10
 */
 #define FRAME_HEADER_FOOTER_LEN 4
 
@@ -114,13 +109,13 @@ TEST(TelemetryCrsfTest, TestGPS)
     EXPECT_EQ(0, satelliteCount);
     EXPECT_EQ(crfsCrc(frame, frameLen), frame[18]);
 
-    GPS_coord[LAT] = 56 * GPS_DEGREES_DIVIDER;
-    GPS_coord[LON] = 163 * GPS_DEGREES_DIVIDER;
+    gpsSol.llh.lat = 56 * GPS_DEGREES_DIVIDER;
+    gpsSol.llh.lon = 163 * GPS_DEGREES_DIVIDER;
     ENABLE_STATE(GPS_FIX);
-    GPS_altitude = 2345;              // altitude in m
-    GPS_speed = 163;                 // speed in 0.1m/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587
-    GPS_numSat = 9;
-    GPS_ground_course = 1479;     // degrees * 10
+    gpsSol.llh.alt = 2345;              // altitude in m
+    gpsSol.groundSpeed = 163;                 // speed in 0.1m/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587
+    gpsSol.numSat = 9;
+    gpsSol.groundCourse = 1479;     // degrees * 10
     frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS);
     lattitude = frame[3] << 24 | frame[4] << 16 | frame[5] << 8 | frame[6];
     EXPECT_EQ(560000000, lattitude);
@@ -279,12 +274,8 @@ uint8_t useHottAlarmSoundPeriod (void) { return 0; }
 
 attitudeEulerAngles_t attitude = { { 0, 0, 0 } };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
 
-uint8_t GPS_numSat;
-int32_t GPS_coord[2];
 uint16_t GPS_distanceToHome;        // distance to home point in meters
-uint16_t GPS_altitude;              // altitude in m
-uint16_t GPS_speed;                 // speed in 0.1m/s
-uint16_t GPS_ground_course = 0;     // degrees * 10
+gpsSolutionData_t gpsSol;
 
 void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);}
 
@@ -327,4 +318,3 @@ uint8_t calculateBatteryPercentageRemaining(void) {
 }
 
 }
-
diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc
index d4d427969..f8843b2e5 100644
--- a/src/test/unit/telemetry_hott_unittest.cc
+++ b/src/test/unit/telemetry_hott_unittest.cc
@@ -171,11 +171,8 @@ uint16_t batteryWarningVoltage;
 uint8_t useHottAlarmSoundPeriod (void) { return 0; }
 
 
-uint8_t GPS_numSat;
-int32_t GPS_coord[2];
-uint16_t GPS_speed;                 // speed in 0.1m/s
+gpsSolutionData_t gpsSol;
 uint16_t GPS_distanceToHome;        // distance to home point in meters
-uint16_t GPS_altitude;              // altitude in 0.1m
 int16_t GPS_directionToHome;        // direction to home or hol point in degrees
 
 

From 4113f9be011d8a7f5a2803fdae2d53d9b8bb5c08 Mon Sep 17 00:00:00 2001
From: mikeller <github@ike.ch>
Date: Tue, 27 Jun 2017 00:04:19 +1200
Subject: [PATCH 02/15] Fixed Dshot idle value.

---
 src/main/flight/mixer.c | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 4f8d8c2d4..18f4689fc 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -339,8 +339,13 @@ bool mixerIsOutputSaturated(int axis, float errorRate)
 // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator
 // DSHOT scaling is done to the actual dshot range
 void initEscEndpoints(void) {
+    switch(motorConfig()->dev.motorPwmProtocol) {
 #ifdef USE_DSHOT
-    if (isMotorProtocolDshot()) {
+    case PWM_TYPE_PROSHOT1000:
+    case PWM_TYPE_DSHOT1200:
+    case PWM_TYPE_DSHOT600:
+    case PWM_TYPE_DSHOT300:
+    case PWM_TYPE_DSHOT150:
         disarmMotorOutput = DSHOT_DISARM_COMMAND;
         if (feature(FEATURE_3D))
             motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
@@ -349,14 +354,17 @@ void initEscEndpoints(void) {
         motorOutputHigh = DSHOT_MAX_THROTTLE;
         deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + ((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); // TODO - Not working yet !! Mixer requires some throttle rescaling changes
         deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
-    } else
+
+        break;
 #endif
-    {
+    default:
         disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig()->neutral3d : motorConfig()->mincommand;
         motorOutputLow = motorConfig()->minthrottle;
         motorOutputHigh = motorConfig()->maxthrottle;
         deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
         deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
+
+        break;
     }
 
     rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig()->mincheck);

From 427c5fe5244b51d73face2cfd0c4d6ae749f077d Mon Sep 17 00:00:00 2001
From: Dan Nixon <dan@dan-nixon.com>
Date: Thu, 15 Jun 2017 15:49:35 +0100
Subject: [PATCH 03/15] OSD unit test

---
 src/main/cms/cms.c                   |  10 +-
 src/main/io/osd.c                    |  19 +-
 src/main/io/osd.h                    |   7 +
 src/main/target/KROOZX/config.c      |   2 -
 src/test/Makefile                    |  15 +-
 src/test/unit/cms_unittest.cc        |  96 +----
 src/test/unit/osd_unittest.cc        | 568 +++++++++++++++++++++++++++
 src/test/unit/unittest_displayport.h | 163 ++++++++
 8 files changed, 761 insertions(+), 119 deletions(-)
 create mode 100644 src/test/unit/osd_unittest.cc
 create mode 100644 src/test/unit/unittest_displayport.h

diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c
index 5e97c5eeb..0964acca9 100644
--- a/src/main/cms/cms.c
+++ b/src/main/cms/cms.c
@@ -315,10 +315,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
 #ifdef OSD
     case OME_VISIBLE:
         if (IS_PRINTVALUE(p) && p->data) {
-            uint32_t address = (uint32_t)p->data;
-            uint16_t *val;
-
-            val = (uint16_t *)address;
+            uint16_t *val = (uint16_t *)p->data;
 
             if (VISIBLE(*val)) {
                 cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES");
@@ -757,10 +754,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
 #ifdef OSD
         case OME_VISIBLE:
             if (p->data) {
-                uint32_t address = (uint32_t)p->data;
-                uint16_t *val;
-
-                val = (uint16_t *)address;
+                uint16_t *val = (uint16_t *)p->data;
 
                 if (key == KEY_RIGHT)
                     *val |= VISIBLE_FLAG;
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index dcbd7257f..aa2903859 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -36,6 +36,7 @@
 #include "blackbox/blackbox.h"
 #include "blackbox/blackbox_io.h"
 
+#include "build/build_config.h"
 #include "build/debug.h"
 #include "build/version.h"
 
@@ -88,13 +89,6 @@
 
 #define VIDEO_BUFFER_CHARS_PAL    480
 
-// Character coordinate
-
-#define OSD_POSITION_BITS 5 // 5 bits gives a range 0-31
-#define OSD_POS(x,y)  ((x & 0x001F) | ((y & 0x001F) << OSD_POSITION_BITS))
-#define OSD_X(x)      (x & 0x001F)
-#define OSD_Y(x)      ((x >> OSD_POSITION_BITS) & 0x001F)
-
 // Blink control
 
 static bool blinkState = true;
@@ -168,9 +162,9 @@ static char osdGetMetersToSelectedUnitSymbol()
 {
     switch (osdConfig()->units) {
     case OSD_UNIT_IMPERIAL:
-        return 0xF;
+        return SYM_FT;
     default:
-        return 0xC;
+        return SYM_M;
     }
 }
 
@@ -761,7 +755,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
 static void osdDrawLogo(int x, int y)
 {
     // display logo and help
-    char fontOffset = 160;
+    unsigned char fontOffset = 160;
     for (int row = 0; row < 4; row++) {
         for (int column = 0; column < 24; column++) {
             if (fontOffset != 255) // FIXME magic number
@@ -810,7 +804,6 @@ void osdUpdateAlarms(void)
     // uint16_t *itemPos = osdConfig()->item_pos;
 
     int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitude()) / 100;
-    statRssi = rssi * 100 / 1024;
 
     if (statRssi < osdConfig()->rssi_alarm)
         SET_BLINK(OSD_RSSI_VALUE);
@@ -1039,7 +1032,7 @@ static void osdShowArmed(void)
     displayWrite(osdDisplayPort, 12, 7, "ARMED");
 }
 
-static void osdRefresh(timeUs_t currentTimeUs)
+STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
 {
     static uint8_t lastSec = 0;
     uint8_t sec;
@@ -1058,6 +1051,8 @@ static void osdRefresh(timeUs_t currentTimeUs)
         armState = ARMING_FLAG(ARMED);
     }
 
+    statRssi = rssi * 100 / 1024;
+
     osdUpdateStats();
 
     sec = currentTimeUs / 1000000;
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 1b500786e..a5d15e18f 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -26,6 +26,13 @@
 #define OSD_POS_MAX   0x3FF
 #define OSD_POSCFG_MAX   (VISIBLE_FLAG|0x3FF) // For CLI values
 
+// Character coordinate
+
+#define OSD_POSITION_BITS 5 // 5 bits gives a range 0-31
+#define OSD_POS(x,y)  ((x & 0x001F) | ((y & 0x001F) << OSD_POSITION_BITS))
+#define OSD_X(x)      (x & 0x001F)
+#define OSD_Y(x)      ((x >> OSD_POSITION_BITS) & 0x001F)
+
 typedef enum {
     OSD_RSSI_VALUE,
     OSD_MAIN_BATT_VOLTAGE,
diff --git a/src/main/target/KROOZX/config.c b/src/main/target/KROOZX/config.c
index 05a009b06..e18f3672e 100755
--- a/src/main/target/KROOZX/config.c
+++ b/src/main/target/KROOZX/config.c
@@ -30,8 +30,6 @@
 
 #define VBAT_SCALE       113
 
-#define OSD_POS(x,y)  (x | (y << 5))
-
 #ifdef TARGET_CONFIG
 void targetConfiguration(void)
 {
diff --git a/src/test/Makefile b/src/test/Makefile
index 0407c27bd..4776ac9df 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -107,6 +107,17 @@ maths_unittest_SRC := \
 		$(USER_DIR)/common/maths.c
 
 
+osd_unittest_SRC := \
+		$(USER_DIR)/io/osd.c \
+		$(USER_DIR)/common/typeconversion.c \
+		$(USER_DIR)/drivers/display.c \
+		$(USER_DIR)/common/maths.c \
+		$(USER_DIR)/common/printf.c
+
+osd_unittest_DEFINES := \
+		OSD
+
+
 parameter_groups_unittest_SRC := \
 		$(USER_DIR)/config/parameter_group.c
 
@@ -359,7 +370,7 @@ $(OBJECT_DIR)/$1/%.c.o: $(USER_DIR)/%.c
 	@echo "compiling $$<" "$(STDOUT)"
 	$(V1) mkdir -p $$(dir $$@)
 	$(V1) $(CC) $(C_FLAGS) $(TEST_CFLAGS) \
-                $(foreach def,$1_DEFINES,-D $(def)) \
+                $(foreach def,$($1_DEFINES),-D $(def)) \
                 -c $$< -o $$@
 
 
@@ -367,7 +378,7 @@ $(OBJECT_DIR)/$1/$1.o: $(TEST_DIR)/$1.cc
 	@echo "compiling $$<" "$(STDOUT)"
 	$(V1) mkdir -p $$(dir $$@)
 	$(V1) $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS)  \
-                 $(foreach def,$1_DEFINES,-D $(def)) \
+                 $(foreach def,$($1_DEFINES),-D $(def)) \
                  -c $$< -o $$@
 
 
diff --git a/src/test/unit/cms_unittest.cc b/src/test/unit/cms_unittest.cc
index c422e2473..a61f2a4df 100644
--- a/src/test/unit/cms_unittest.cc
+++ b/src/test/unit/cms_unittest.cc
@@ -28,7 +28,6 @@
 extern "C" {
     #include "platform.h"
     #include "target.h"
-    #include "drivers/display.h"
     #include "cms/cms.h"
     #include "cms/cms_types.h"
     void cmsMenuOpen(void);
@@ -38,102 +37,9 @@ extern "C" {
 }
 
 #include "unittest_macros.h"
+#include "unittest_displayport.h"
 #include "gtest/gtest.h"
 
-static displayPort_t testDisplayPort;
-static int displayPortTestGrab(displayPort_t *displayPort)
-{
-    UNUSED(displayPort);
-    return 0;
-}
-
-static int displayPortTestRelease(displayPort_t *displayPort)
-{
-    UNUSED(displayPort);
-    return 0;
-}
-
-static int displayPortTestClearScreen(displayPort_t *displayPort)
-{
-    UNUSED(displayPort);
-    return 0;
-}
-
-static int displayPortTestDrawScreen(displayPort_t *displayPort)
-{
-    UNUSED(displayPort);
-    return 0;
-}
-
-static int displayPortTestScreenSize(const displayPort_t *displayPort)
-{
-    UNUSED(displayPort);
-    return 0;
-}
-
-static int displayPortTestWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
-{
-    UNUSED(displayPort);
-    UNUSED(x);
-    UNUSED(y);
-    UNUSED(s);
-    return 0;
-}
-
-static int displayPortTestWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c)
-{
-    UNUSED(displayPort);
-    UNUSED(x);
-    UNUSED(y);
-    UNUSED(c);
-    return 0;
-}
-
-static bool displayPortTestIsTransferInProgress(const displayPort_t *displayPort)
-{
-    UNUSED(displayPort);
-    return 0;
-}
-
-static int displayPortTestHeartbeat(displayPort_t *displayPort)
-{
-    UNUSED(displayPort);
-    return 0;
-}
-
-static void displayPortTestResync(displayPort_t *displayPort)
-{
-    UNUSED(displayPort);
-}
-
-static uint32_t displayPortTestTxBytesFree(const displayPort_t *displayPort)
-{
-    UNUSED(displayPort);
-    return 0;
-}
-
-static const displayPortVTable_t testDisplayPortVTable = {
-    .grab = displayPortTestGrab,
-    .release = displayPortTestRelease,
-    .clearScreen = displayPortTestClearScreen,
-    .drawScreen = displayPortTestDrawScreen,
-    .screenSize = displayPortTestScreenSize,
-    .write = displayPortTestWrite,
-    .writeChar = displayPortTestWriteChar,
-    .isTransferInProgress = displayPortTestIsTransferInProgress,
-    .heartbeat = displayPortTestHeartbeat,
-    .resync = displayPortTestResync,
-    .txBytesFree = displayPortTestTxBytesFree
-};
-
-displayPort_t *displayPortTestInit(void)
-{
-    displayInit(&testDisplayPort, &testDisplayPortVTable);
-    testDisplayPort.rows = 10;
-    testDisplayPort.cols = 40;
-    return &testDisplayPort;
-}
-
 TEST(CMSUnittest, TestCmsDisplayPortRegister)
 {
     cmsInit();
diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc
new file mode 100644
index 000000000..dcccdfc61
--- /dev/null
+++ b/src/test/unit/osd_unittest.cc
@@ -0,0 +1,568 @@
+/*
+ * 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 <stdint.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <string.h>
+
+extern "C" {
+    #include "platform.h"
+
+    #include "build/debug.h"
+    #include "blackbox/blackbox.h"
+    #include "config/parameter_group_ids.h"
+    #include "drivers/max7456_symbols.h"
+    #include "fc/config.h"
+    #include "fc/rc_controls.h"
+    #include "fc/rc_modes.h"
+    #include "fc/runtime_config.h"
+    #include "flight/pid.h"
+    #include "flight/imu.h"
+    #include "io/osd.h"
+    #include "sensors/battery.h"
+    #include "rx/rx.h"
+
+    void osdRefresh(timeUs_t currentTimeUs);
+
+    uint8_t stateFlags;
+    uint8_t armingFlags;
+    uint16_t flightModeFlags;
+    uint16_t rssi;
+    attitudeEulerAngles_t attitude;
+    pidProfile_t *currentPidProfile;
+    int16_t debug[DEBUG16_VALUE_COUNT];
+    int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
+    uint8_t GPS_numSat;
+    uint16_t GPS_speed;
+    uint16_t GPS_distanceToHome;
+    uint16_t GPS_directionToHome;
+    int32_t GPS_coord[2];
+
+    PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
+    PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
+    PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
+
+    timeUs_t simulationTime = 0;
+    batteryState_e simulationBatteryState;
+    uint8_t simulationBatteryCellCount;
+    uint16_t simulationBatteryVoltage;
+    int32_t simulationAltitude;
+    int32_t simulationVerticalSpeed;
+}
+
+/* #define DEBUG_OSD */
+
+#include "unittest_macros.h"
+#include "unittest_displayport.h"
+#include "gtest/gtest.h"
+
+void setDefualtSimulationState()
+{
+    rssi = 1024;
+
+    simulationBatteryState = BATTERY_OK;
+    simulationBatteryCellCount = 4;
+    simulationBatteryVoltage = 168;
+    simulationAltitude = 0;
+    simulationVerticalSpeed = 0;
+}
+
+/*
+ * Performs a test of the OSD actions on arming.
+ * (reused throughout the test suite)
+ */
+void doTestArm(bool testEmpty = true)
+{
+    // given
+    // craft has been armed
+    ENABLE_ARMING_FLAG(ARMED);
+
+    // when
+    // sufficient OSD updates have been called
+    osdRefresh(simulationTime);
+
+    // then
+    // arming alert displayed
+    displayPortTestBufferSubstring(12, 7, "ARMED");
+
+    // given
+    // armed alert times out (0.5 seconds)
+    simulationTime += 5e5;
+
+    // when
+    // sufficient OSD updates have been called
+    osdRefresh(simulationTime);
+
+    // then
+    // arming alert disappears
+#ifdef DEBUG_OSD
+    displayPortTestPrint();
+#endif
+    if (testEmpty) {
+        for (size_t i = 0; i < UNITTEST_DISPLAYPORT_BUFFER_LEN; i++) {
+            EXPECT_EQ(' ', testDisplayPortBuffer[i]);
+        }
+    }
+}
+
+/*
+ * Performs a test of the OSD actions on disarming.
+ * (reused throughout the test suite)
+ */
+void doTestDisarm()
+{
+    // given
+    // craft is disarmed after having been armed
+    DISABLE_ARMING_FLAG(ARMED);
+
+    // when
+    // sufficient OSD updates have been called
+    osdRefresh(simulationTime);
+
+    // then
+    // post flight statistics displayed
+    displayPortTestBufferSubstring(2, 2, "  --- STATS ---");
+}
+
+
+/*
+ * Tests initialisation of the OSD and the power on splash screen.
+ */
+TEST(OsdTest, TestInit)
+{
+    // given
+    // display port is initialised
+    displayPortTestInit();
+
+    // and
+    // default state values are set
+    setDefualtSimulationState();
+
+    // and
+    // this battery configuration (used for battery voltage elements)
+    batteryConfigMutable()->vbatmincellvoltage = 33;
+    batteryConfigMutable()->vbatmaxcellvoltage = 43;
+
+    // when
+    // OSD is initialised
+    osdInit(&testDisplayPort);
+
+    // then
+    // display buffer should contain splash screen
+    displayPortTestBufferSubstring(7, 8, "MENU: THR MID");
+    displayPortTestBufferSubstring(11, 9, "+ YAW LEFT");
+    displayPortTestBufferSubstring(11, 10, "+ PITCH UP");
+
+    // when
+    // splash screen timeout has elapsed
+    simulationTime += 4e6;
+    osdUpdate(simulationTime);
+
+    // then
+    // display buffer should be empty
+#ifdef DEBUG_OSD
+    displayPortTestPrint();
+#endif
+    displayPortTestBufferIsEmpty();
+}
+
+/*
+ * Tests visibility of the ARMED notification after arming.
+ */
+TEST(OsdTest, TestArm)
+{
+    doTestArm();
+}
+
+/*
+ * Tests display and timeout of the post flight statistics screen after disarming.
+ */
+TEST(OsdTest, TestDisarm)
+{
+    doTestDisarm();
+
+    // given
+    // post flight stats times out (60 seconds)
+    simulationTime += 60e6;
+
+    // when
+    // sufficient OSD updates have been called
+    osdRefresh(simulationTime);
+
+    // then
+    // post flight stats screen disappears
+#ifdef DEBUG_OSD
+    displayPortTestPrint();
+#endif
+    displayPortTestBufferIsEmpty();
+}
+
+/*
+ * Tests disarming and immediately rearming clears post flight stats and shows ARMED notification.
+ */
+TEST(OsdTest, TestDisarmWithImmediateRearm)
+{
+    doTestArm();
+    doTestDisarm();
+    doTestArm();
+}
+
+/*
+ * Tests dismissing the statistics screen with pitch stick after disarming.
+ */
+TEST(OsdTest, TestDisarmWithDismissStats)
+{
+    // Craft is alread armed after previous test
+
+    doTestDisarm();
+
+    // given
+    // sticks have been moved
+    rcData[PITCH] = 1800;
+
+    // when
+    // sufficient OSD updates have been called
+    osdRefresh(simulationTime);
+    osdRefresh(simulationTime);
+
+    // then
+    // post flight stats screen disappears
+#ifdef DEBUG_OSD
+    displayPortTestPrint();
+#endif
+    displayPortTestBufferIsEmpty();
+
+    rcData[PITCH] = 1500;
+}
+
+/*
+ * Tests the calculation of statistics with imperial unit output.
+ */
+TEST(OsdTest, TestStatsImperial)
+{
+    // given
+    // this set of enabled post flight statistics
+    osdConfigMutable()->enabled_stats[OSD_STAT_MAX_SPEED]       = true;
+    osdConfigMutable()->enabled_stats[OSD_STAT_MIN_BATTERY]     = true;
+    osdConfigMutable()->enabled_stats[OSD_STAT_MIN_RSSI]        = true;
+    osdConfigMutable()->enabled_stats[OSD_STAT_MAX_CURRENT]     = false;
+    osdConfigMutable()->enabled_stats[OSD_STAT_USED_MAH]        = false;
+    osdConfigMutable()->enabled_stats[OSD_STAT_MAX_ALTITUDE]    = true;
+    osdConfigMutable()->enabled_stats[OSD_STAT_BLACKBOX]        = false;
+    osdConfigMutable()->enabled_stats[OSD_STAT_END_BATTERY]     = true;
+    osdConfigMutable()->enabled_stats[OSD_STAT_FLYTIME]         = true;
+    osdConfigMutable()->enabled_stats[OSD_STAT_ARMEDTIME]       = true;
+    osdConfigMutable()->enabled_stats[OSD_STAT_MAX_DISTANCE]    = true;
+    osdConfigMutable()->enabled_stats[OSD_STAT_BLACKBOX_NUMBER] = false;
+
+    // and
+    // using imperial unit system
+    osdConfigMutable()->units = OSD_UNIT_IMPERIAL;
+
+    // and
+    // a GPS fix is present
+    stateFlags |= GPS_FIX | GPS_FIX_HOME;
+
+    // when
+    // the craft is armed
+    doTestArm();
+
+    // and
+    // these conditions occur during flight
+    rssi = 1024;
+    GPS_speed = 500;
+    GPS_distanceToHome = 20;
+    simulationBatteryVoltage = 158;
+    simulationAltitude = 100;
+    simulationTime += 1e6;
+    osdRefresh(simulationTime);
+
+    rssi = 512;
+    GPS_speed = 800;
+    GPS_distanceToHome = 50;
+    simulationBatteryVoltage = 147;
+    simulationAltitude = 150;
+    simulationTime += 1e6;
+    osdRefresh(simulationTime);
+
+    rssi = 256;
+    GPS_speed = 200;
+    GPS_distanceToHome = 100;
+    simulationBatteryVoltage = 152;
+    simulationAltitude = 200;
+    simulationTime += 1e6;
+    osdRefresh(simulationTime);
+
+    // and
+    // the craft is disarmed
+    doTestDisarm();
+
+    // then
+    // statistics screen should display the following
+    displayPortTestBufferSubstring(2, 3,  "ARMED TIME        : 00:04");
+    displayPortTestBufferSubstring(2, 4,  "FLY TIME          : 00:07");
+    displayPortTestBufferSubstring(2, 5,  "MAX SPEED         : 28");
+    displayPortTestBufferSubstring(2, 6,  "MAX DISTANCE      : 328%c", SYM_FT);
+    displayPortTestBufferSubstring(2, 7,  "MIN BATTERY       : 14.7%c", SYM_VOLT);
+    displayPortTestBufferSubstring(2, 8,  "END BATTERY       : 15.2%c", SYM_VOLT);
+    displayPortTestBufferSubstring(2, 9,  "MIN RSSI          : 25%");
+    displayPortTestBufferSubstring(2, 10, "MAX ALTITUDE      :  6.5%c", SYM_FT);
+}
+
+/*
+ * Tests the calculation of statistics with metric unit output.
+ * (essentially an abridged version of the previous test
+ */
+TEST(OsdTest, TestStatsMetric)
+{
+    // given
+    // using metric unit system
+    osdConfigMutable()->units = OSD_UNIT_METRIC;
+
+    // and
+    // default state values are set
+    setDefualtSimulationState();
+
+    // when
+    // the craft is armed
+    doTestArm();
+
+    // and
+    // these conditions occur during flight (simplified to less assignments than previous test)
+    rssi = 256;
+    GPS_speed = 800;
+    GPS_distanceToHome = 100;
+    simulationBatteryVoltage = 147;
+    simulationAltitude = 200;
+    simulationTime += 1e6;
+    osdRefresh(simulationTime);
+    osdRefresh(simulationTime);
+
+    simulationBatteryVoltage = 152;
+    simulationTime += 1e6;
+    osdRefresh(simulationTime);
+
+    // and
+    // the craft is disarmed
+    doTestDisarm();
+
+    // then
+    // statistics screen should display the following
+    displayPortTestBufferSubstring(2, 3,  "ARMED TIME        : 00:02");
+    displayPortTestBufferSubstring(2, 4,  "FLY TIME          : 00:09");
+    displayPortTestBufferSubstring(2, 5,  "MAX SPEED         : 28");
+    displayPortTestBufferSubstring(2, 6,  "MAX DISTANCE      : 100%c", SYM_M);
+    displayPortTestBufferSubstring(2, 7,  "MIN BATTERY       : 14.7%c", SYM_VOLT);
+    displayPortTestBufferSubstring(2, 8,  "END BATTERY       : 15.2%c", SYM_VOLT);
+    displayPortTestBufferSubstring(2, 9,  "MIN RSSI          : 25%");
+    displayPortTestBufferSubstring(2, 10, "MAX ALTITUDE      :  2.0%c", SYM_M);
+}
+
+/*
+ * Tests activation of alarms and element flashing.
+ */
+TEST(OsdTest, TestAlarms)
+{
+    // given
+    // default state is set
+    setDefualtSimulationState();
+
+    // and
+    // the following OSD elements are visible
+    osdConfigMutable()->item_pos[OSD_RSSI_VALUE]        = OSD_POS(8, 1)  | VISIBLE_FLAG;
+    osdConfigMutable()->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 1) | VISIBLE_FLAG;
+    osdConfigMutable()->item_pos[OSD_FLYTIME]           = OSD_POS(1, 1)  | VISIBLE_FLAG;
+    osdConfigMutable()->item_pos[OSD_ALTITUDE]          = OSD_POS(23, 7) | VISIBLE_FLAG;
+
+    // and
+    // this set of alarm values
+    osdConfigMutable()->rssi_alarm = 20;
+    osdConfigMutable()->cap_alarm  = 2200;
+    osdConfigMutable()->time_alarm = 1; // in minutes
+    osdConfigMutable()->alt_alarm  = 100; // meters
+
+    // and
+    // using the metric unit system
+    osdConfigMutable()->units = OSD_UNIT_METRIC;
+
+    // when
+    // the craft is armed
+    doTestArm(false);
+
+    // then
+    // no elements should flash as all values are out of alarm range
+    for (int i = 0; i < 6; i++) {
+        simulationTime += 1e6;
+        osdRefresh(simulationTime);
+
+#ifdef DEBUG_OSD
+        printf("%d\n", i);
+#endif
+        displayPortTestBufferSubstring(8,  1, "%c99", SYM_RSSI);
+        displayPortTestBufferSubstring(12, 1, "%c16.8%c", SYM_BATT_FULL, SYM_VOLT);
+        displayPortTestBufferSubstring(1,  1, "%c00:", SYM_FLY_M); // only test the minute part of the timer
+        displayPortTestBufferSubstring(23, 7, " 0.0%c", SYM_M);
+    }
+
+    // when
+    // all values are out of range
+    rssi = 128;
+    simulationBatteryState = BATTERY_CRITICAL;
+    simulationBatteryVoltage = 135;
+    simulationAltitude = 12000;
+    // Fly timer is incremented on periodic calls to osdRefresh, can't simply just increment the simulated system clock
+    for (int i = 0; i < 60; i++) {
+        simulationTime += 1e6;
+        osdRefresh(simulationTime);
+    }
+
+    // then
+    // elements showing values in alarm range should flash
+    for (int i = 0; i < 6; i++) {
+        simulationTime += 1e6;
+        osdRefresh(simulationTime);
+
+#ifdef DEBUG_OSD
+        printf("%d\n", i);
+        displayPortTestPrint();
+#endif
+        if (i % 2 == 1) {
+            displayPortTestBufferSubstring(8,  1, "%c12", SYM_RSSI);
+            displayPortTestBufferSubstring(12, 1, "%c13.5%c", SYM_MAIN_BATT, SYM_VOLT);
+            displayPortTestBufferSubstring(1,  1, "%c01:", SYM_FLY_M); // only test the minute part of the timer
+            displayPortTestBufferSubstring(23, 7, " 120.0%c", SYM_M);
+        } else {
+            displayPortTestBufferIsEmpty();
+        }
+    }
+}
+
+/*
+ * Tests the RSSI OSD element.
+ */
+TEST(OsdTest, TestElementRssi)
+{
+    // given
+    osdConfigMutable()->item_pos[OSD_RSSI_VALUE] = OSD_POS(8, 1) | VISIBLE_FLAG;
+    osdConfigMutable()->rssi_alarm = 0;
+
+    // when
+    rssi = 1024;
+    displayClearScreen(&testDisplayPort);
+    osdRefresh(simulationTime);
+
+    // then
+    displayPortTestBufferSubstring(8, 1, "%c99", SYM_RSSI);
+
+    // when
+    rssi = 0;
+    displayClearScreen(&testDisplayPort);
+    osdRefresh(simulationTime);
+
+    // then
+    displayPortTestBufferSubstring(8, 1, "%c0", SYM_RSSI);
+
+    // when
+    rssi = 512;
+    displayClearScreen(&testDisplayPort);
+    osdRefresh(simulationTime);
+
+    // then
+    displayPortTestBufferSubstring(8, 1, "%c50", SYM_RSSI);
+}
+
+
+// STUBS
+extern "C" {
+    bool sensors(uint32_t mask) {
+        UNUSED(mask);
+        return true;
+    }
+
+    bool IS_RC_MODE_ACTIVE(boxId_e boxId) {
+        UNUSED(boxId);
+        return false;
+    }
+
+    uint32_t micros() {
+        return simulationTime;
+    }
+
+    bool isBeeperOn() {
+        return false;
+    }
+
+    bool isAirmodeActive() {
+        return false;
+    }
+
+    uint8_t getCurrentPidProfileIndex() {
+        return 0;
+    }
+
+    uint8_t getCurrentControlRateProfileIndex() {
+        return 0;
+    }
+
+    batteryState_e getBatteryState() {
+        return simulationBatteryState;
+    }
+
+    uint8_t getBatteryCellCount() {
+        return simulationBatteryCellCount;
+    }
+
+    uint16_t getBatteryVoltage() {
+        return simulationBatteryVoltage;
+    }
+
+    int32_t getAmperage() {
+        return 0;
+    }
+
+    int32_t getMAhDrawn() {
+        return 0;
+    }
+
+    int32_t getEstimatedAltitude() {
+        return simulationAltitude;
+    }
+
+    int32_t getEstimatedVario() {
+        return simulationVerticalSpeed;
+    }
+
+    unsigned int blackboxGetLogNumber() {
+        return 0;
+    }
+
+    bool isSerialTransmitBufferEmpty(const serialPort_t *instance) {
+        UNUSED(instance);
+        return false;
+    }
+
+    void serialWrite(serialPort_t *instance, uint8_t ch) {
+        UNUSED(instance);
+        UNUSED(ch);
+    }
+
+    bool cmsDisplayPortRegister(displayPort_t *pDisplay) {
+        UNUSED(pDisplay);
+        return false;
+    }
+}
diff --git a/src/test/unit/unittest_displayport.h b/src/test/unit/unittest_displayport.h
new file mode 100644
index 000000000..a2cd7d635
--- /dev/null
+++ b/src/test/unit/unittest_displayport.h
@@ -0,0 +1,163 @@
+/*
+ * 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
+
+#include <string.h>
+
+extern "C" {
+    #include "drivers/display.h"
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+#define UNITTEST_DISPLAYPORT_ROWS 16
+#define UNITTEST_DISPLAYPORT_COLS 30
+#define UNITTEST_DISPLAYPORT_BUFFER_LEN (UNITTEST_DISPLAYPORT_ROWS * UNITTEST_DISPLAYPORT_COLS)
+
+char testDisplayPortBuffer[UNITTEST_DISPLAYPORT_BUFFER_LEN];
+
+static displayPort_t testDisplayPort;
+
+static int displayPortTestGrab(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static int displayPortTestRelease(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static int displayPortTestClearScreen(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    memset(testDisplayPortBuffer, ' ', UNITTEST_DISPLAYPORT_BUFFER_LEN);
+    return 0;
+}
+
+static int displayPortTestDrawScreen(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static int displayPortTestScreenSize(const displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static int displayPortTestWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
+{
+    UNUSED(displayPort);
+    for (unsigned int i = 0; i < strlen(s); i++) {
+        testDisplayPortBuffer[(y * UNITTEST_DISPLAYPORT_COLS) + x + i] = s[i];
+    }
+    return 0;
+}
+
+static int displayPortTestWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c)
+{
+    UNUSED(displayPort);
+    testDisplayPortBuffer[(y * UNITTEST_DISPLAYPORT_COLS) + x] = c;
+    return 0;
+}
+
+static bool displayPortTestIsTransferInProgress(const displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static int displayPortTestHeartbeat(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static void displayPortTestResync(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+}
+
+static uint32_t displayPortTestTxBytesFree(const displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static const displayPortVTable_t testDisplayPortVTable = {
+    .grab = displayPortTestGrab,
+    .release = displayPortTestRelease,
+    .clearScreen = displayPortTestClearScreen,
+    .drawScreen = displayPortTestDrawScreen,
+    .screenSize = displayPortTestScreenSize,
+    .write = displayPortTestWrite,
+    .writeChar = displayPortTestWriteChar,
+    .isTransferInProgress = displayPortTestIsTransferInProgress,
+    .heartbeat = displayPortTestHeartbeat,
+    .resync = displayPortTestResync,
+    .txBytesFree = displayPortTestTxBytesFree
+};
+
+displayPort_t *displayPortTestInit(void)
+{
+    displayInit(&testDisplayPort, &testDisplayPortVTable);
+    testDisplayPort.rows = UNITTEST_DISPLAYPORT_ROWS;
+    testDisplayPort.cols = UNITTEST_DISPLAYPORT_COLS;
+    return &testDisplayPort;
+}
+
+void displayPortTestPrint(void)
+{
+    for (int i = 0; i < UNITTEST_DISPLAYPORT_BUFFER_LEN; i++) {
+        if (i > 0 && i % UNITTEST_DISPLAYPORT_COLS == 0) {
+            printf("\n");
+        }
+        printf("%c", testDisplayPortBuffer[i]);
+    }
+    printf("\n\n");
+}
+
+void displayPortTestBufferIsEmpty()
+{
+    for (size_t i = 0; i < UNITTEST_DISPLAYPORT_BUFFER_LEN; i++) {
+        EXPECT_EQ(' ', testDisplayPortBuffer[i]);
+    }
+}
+
+void displayPortTestBufferSubstring(int x, int y, const char * expectedFormat, ...)
+{
+    char expected[32];
+
+    va_list args;
+    va_start(args, expectedFormat);
+    vsprintf(expected, expectedFormat, args);
+    va_end(args);
+
+#ifdef DEBUG_OSD
+    displayPortTestPrint();
+#endif
+
+    for (size_t i = 0; i < strlen(expected); i++) {
+        EXPECT_EQ(expected[i], testDisplayPortBuffer[(y * testDisplayPort.cols) + x + i]);
+    }
+}

From 270c4bd5fe6424905dcd861f246d6b91e1be04a9 Mon Sep 17 00:00:00 2001
From: Dan Nixon <dan@dan-nixon.com>
Date: Tue, 27 Jun 2017 15:56:57 +0100
Subject: [PATCH 04/15] Update OSD unit test as per comments from @ledvinap

---
 src/main/drivers/max7456_symbols.h   |  2 ++
 src/main/io/osd.c                    |  7 +++----
 src/main/io/osd.h                    |  7 ++++---
 src/test/unit/osd_unittest.cc        | 22 +++++++++++-----------
 src/test/unit/unittest_displayport.h |  6 ++++--
 5 files changed, 24 insertions(+), 20 deletions(-)

diff --git a/src/main/drivers/max7456_symbols.h b/src/main/drivers/max7456_symbols.h
index 98eb7dc98..a80bb6b34 100644
--- a/src/main/drivers/max7456_symbols.h
+++ b/src/main/drivers/max7456_symbols.h
@@ -21,6 +21,8 @@
 
 #pragma once
 
+#define SYM_END_OF_FONT 0xFF
+
 // Character Symbols
 #define SYM_BLANK 0x20
 
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index aa2903859..6e4c2adcb 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -755,10 +755,10 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
 static void osdDrawLogo(int x, int y)
 {
     // display logo and help
-    unsigned char fontOffset = 160;
+    int fontOffset = 160;
     for (int row = 0; row < 4; row++) {
         for (int column = 0; column < 24; column++) {
-            if (fontOffset != 255) // FIXME magic number
+            if (fontOffset <= SYM_END_OF_FONT)
                 displayWriteChar(osdDisplayPort, x + column, y + row, fontOffset++);
         }
     }
@@ -801,7 +801,6 @@ void osdInit(displayPort_t *osdDisplayPortToUse)
 void osdUpdateAlarms(void)
 {
     // This is overdone?
-    // uint16_t *itemPos = osdConfig()->item_pos;
 
     int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitude()) / 100;
 
@@ -1051,7 +1050,7 @@ STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
         armState = ARMING_FLAG(ARMED);
     }
 
-    statRssi = rssi * 100 / 1024;
+    statRssi = scaleRange(rssi, 0, 1024, 0, 100);
 
     osdUpdateStats();
 
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index a5d15e18f..b6cb82dc8 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -29,9 +29,10 @@
 // Character coordinate
 
 #define OSD_POSITION_BITS 5 // 5 bits gives a range 0-31
-#define OSD_POS(x,y)  ((x & 0x001F) | ((y & 0x001F) << OSD_POSITION_BITS))
-#define OSD_X(x)      (x & 0x001F)
-#define OSD_Y(x)      ((x >> OSD_POSITION_BITS) & 0x001F)
+#define OSD_POSITION_XY_MASK ((1 << OSD_POSITION_BITS) - 1)
+#define OSD_POS(x,y)  ((x & OSD_POSITION_XY_MASK) | ((y & OSD_POSITION_XY_MASK) << OSD_POSITION_BITS))
+#define OSD_X(x)      (x & OSD_POSITION_XY_MASK)
+#define OSD_Y(x)      ((x >> OSD_POSITION_BITS) & OSD_POSITION_XY_MASK)
 
 typedef enum {
     OSD_RSSI_VALUE,
diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc
index dcccdfc61..dd3b4ea10 100644
--- a/src/test/unit/osd_unittest.cc
+++ b/src/test/unit/osd_unittest.cc
@@ -102,7 +102,7 @@ void doTestArm(bool testEmpty = true)
 
     // given
     // armed alert times out (0.5 seconds)
-    simulationTime += 5e5;
+    simulationTime += 0.5e6;
 
     // when
     // sufficient OSD updates have been called
@@ -114,9 +114,7 @@ void doTestArm(bool testEmpty = true)
     displayPortTestPrint();
 #endif
     if (testEmpty) {
-        for (size_t i = 0; i < UNITTEST_DISPLAYPORT_BUFFER_LEN; i++) {
-            EXPECT_EQ(' ', testDisplayPortBuffer[i]);
-        }
+        displayPortTestBufferIsEmpty();
     }
 }
 
@@ -320,7 +318,7 @@ TEST(OsdTest, TestStatsImperial)
     displayPortTestBufferSubstring(2, 6,  "MAX DISTANCE      : 328%c", SYM_FT);
     displayPortTestBufferSubstring(2, 7,  "MIN BATTERY       : 14.7%c", SYM_VOLT);
     displayPortTestBufferSubstring(2, 8,  "END BATTERY       : 15.2%c", SYM_VOLT);
-    displayPortTestBufferSubstring(2, 9,  "MIN RSSI          : 25%");
+    displayPortTestBufferSubstring(2, 9,  "MIN RSSI          : 25%%");
     displayPortTestBufferSubstring(2, 10, "MAX ALTITUDE      :  6.5%c", SYM_FT);
 }
 
@@ -369,7 +367,7 @@ TEST(OsdTest, TestStatsMetric)
     displayPortTestBufferSubstring(2, 6,  "MAX DISTANCE      : 100%c", SYM_M);
     displayPortTestBufferSubstring(2, 7,  "MIN BATTERY       : 14.7%c", SYM_VOLT);
     displayPortTestBufferSubstring(2, 8,  "END BATTERY       : 15.2%c", SYM_VOLT);
-    displayPortTestBufferSubstring(2, 9,  "MIN RSSI          : 25%");
+    displayPortTestBufferSubstring(2, 9,  "MIN RSSI          : 25%%");
     displayPortTestBufferSubstring(2, 10, "MAX ALTITUDE      :  2.0%c", SYM_M);
 }
 
@@ -406,8 +404,9 @@ TEST(OsdTest, TestAlarms)
 
     // then
     // no elements should flash as all values are out of alarm range
-    for (int i = 0; i < 6; i++) {
-        simulationTime += 1e6;
+    for (int i = 0; i < 30; i++) {
+        // Check for visibility every 100ms, elements should always be visible
+        simulationTime += 0.1e6;
         osdRefresh(simulationTime);
 
 #ifdef DEBUG_OSD
@@ -433,15 +432,16 @@ TEST(OsdTest, TestAlarms)
 
     // then
     // elements showing values in alarm range should flash
-    for (int i = 0; i < 6; i++) {
-        simulationTime += 1e6;
+    for (int i = 0; i < 15; i++) {
+        // Blinking should happen at 5Hz
+        simulationTime += 0.2e6;
         osdRefresh(simulationTime);
 
 #ifdef DEBUG_OSD
         printf("%d\n", i);
         displayPortTestPrint();
 #endif
-        if (i % 2 == 1) {
+        if (i % 2 == 0) {
             displayPortTestBufferSubstring(8,  1, "%c12", SYM_RSSI);
             displayPortTestBufferSubstring(12, 1, "%c13.5%c", SYM_MAIN_BATT, SYM_VOLT);
             displayPortTestBufferSubstring(1,  1, "%c01:", SYM_FLY_M); // only test the minute part of the timer
diff --git a/src/test/unit/unittest_displayport.h b/src/test/unit/unittest_displayport.h
index a2cd7d635..ded02afe9 100644
--- a/src/test/unit/unittest_displayport.h
+++ b/src/test/unit/unittest_displayport.h
@@ -26,6 +26,8 @@ extern "C" {
 #include "unittest_macros.h"
 #include "gtest/gtest.h"
 
+void displayPortTestBufferSubstring(int x, int y, const char * expectedFormat, ...) __attribute__ ((format (printf, 3, 4)));
+
 #define UNITTEST_DISPLAYPORT_ROWS 16
 #define UNITTEST_DISPLAYPORT_COLS 30
 #define UNITTEST_DISPLAYPORT_BUFFER_LEN (UNITTEST_DISPLAYPORT_ROWS * UNITTEST_DISPLAYPORT_COLS)
@@ -146,11 +148,11 @@ void displayPortTestBufferIsEmpty()
 
 void displayPortTestBufferSubstring(int x, int y, const char * expectedFormat, ...)
 {
-    char expected[32];
+    char expected[UNITTEST_DISPLAYPORT_BUFFER_LEN];
 
     va_list args;
     va_start(args, expectedFormat);
-    vsprintf(expected, expectedFormat, args);
+    vsnprintf(expected, UNITTEST_DISPLAYPORT_BUFFER_LEN, expectedFormat, args);
     va_end(args);
 
 #ifdef DEBUG_OSD

From 545e30e3c43c8662b6305094785d54dc871a8e3c Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Tue, 27 Jun 2017 23:12:38 +0100
Subject: [PATCH 05/15] Merge pull request #2872 from
 cleanflight/fix-servo-filtering

CF/BF - Fix servo filter initialisation.
---
 src/main/fc/fc_init.c    |  4 ++++
 src/main/flight/servos.c | 22 ++++++++++++----------
 src/main/flight/servos.h |  1 +
 3 files changed, 17 insertions(+), 10 deletions(-)

diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 546322f28..0e4d2f30a 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -502,6 +502,10 @@ void init(void)
     // gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidInit()
     pidInit(currentPidProfile);
 
+#ifdef USE_SERVOS
+    servosFilterInit();
+#endif
+
     imuInit();
 
     mspFcInit();
diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index 72b08821e..907a722e1 100755
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -499,23 +499,25 @@ bool isMixerUsingServos(void)
     return useServo;
 }
 
+static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS];
+
+void servosFilterInit(void)
+{
+    if (servoConfig()->servo_lowpass_freq) {
+        for (int servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
+            biquadFilterInitLPF(&servoFilter[servoIdx], servoConfig()->servo_lowpass_freq, targetPidLooptime);
+        }
+    }
+
+}
 static void filterServos(void)
 {
-    static int16_t servoIdx;
-    static bool servoFilterIsSet;
-    static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS];
-
 #if defined(MIXER_DEBUG)
     uint32_t startTime = micros();
 #endif
 
     if (servoConfig()->servo_lowpass_freq) {
-        for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
-            if (!servoFilterIsSet) {
-                biquadFilterInitLPF(&servoFilter[servoIdx], servoConfig()->servo_lowpass_freq, targetPidLooptime);
-                servoFilterIsSet = true;
-            }
-
+        for (int servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
             servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx]));
             // Sanity check
             servo[servoIdx] = constrain(servo[servoIdx], servoParams(servoIdx)->min, servoParams(servoIdx)->max);
diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h
index fe7a4db7d..c2cc8ed37 100644
--- a/src/main/flight/servos.h
+++ b/src/main/flight/servos.h
@@ -132,3 +132,4 @@ void loadCustomServoMixer(void);
 int servoDirection(int servoIndex, int fromChannel);
 void servoConfigureOutput(void);
 void servosInit(void);
+void servosFilterInit(void);

From 6dfff4ac4d98c70c45c655cb4b1f3dfbebe22f01 Mon Sep 17 00:00:00 2001
From: Michael Keller <github@ike.ch>
Date: Wed, 28 Jun 2017 12:45:54 +1200
Subject: [PATCH 06/15] Fixed OSD tests after GPS changes.

---
 src/main/io/serial.c          |  4 +---
 src/test/unit/osd_unittest.cc | 19 ++++++++++++++-----
 2 files changed, 15 insertions(+), 8 deletions(-)

diff --git a/src/main/io/serial.c b/src/main/io/serial.c
index 631ec0093..ec303ca91 100644
--- a/src/main/io/serial.c
+++ b/src/main/io/serial.c
@@ -38,8 +38,6 @@
 #include "drivers/serial_softserial.h"
 #endif
 
-#define USE_SERIAL (defined(USE_UART) || defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
-
 #ifdef SITL
 #include "drivers/serial_tcp.h"
 #endif
@@ -332,7 +330,7 @@ serialPort_t *openSerialPort(
     portMode_t mode,
     portOptions_t options)
 {
-#if !(USE_SERIAL)
+#if !(defined(USE_UART) || defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
     UNUSED(rxCallback);
     UNUSED(baudRate);
     UNUSED(mode);
diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc
index dd3b4ea10..57c94cab6 100644
--- a/src/test/unit/osd_unittest.cc
+++ b/src/test/unit/osd_unittest.cc
@@ -24,17 +24,26 @@ extern "C" {
     #include "platform.h"
 
     #include "build/debug.h"
+
     #include "blackbox/blackbox.h"
+
     #include "config/parameter_group_ids.h"
+
     #include "drivers/max7456_symbols.h"
+
     #include "fc/config.h"
     #include "fc/rc_controls.h"
     #include "fc/rc_modes.h"
     #include "fc/runtime_config.h"
+
     #include "flight/pid.h"
     #include "flight/imu.h"
+
+    #include "io/gps.h"
     #include "io/osd.h"
+
     #include "sensors/battery.h"
+
     #include "rx/rx.h"
 
     void osdRefresh(timeUs_t currentTimeUs);
@@ -48,10 +57,10 @@ extern "C" {
     int16_t debug[DEBUG16_VALUE_COUNT];
     int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
     uint8_t GPS_numSat;
-    uint16_t GPS_speed;
     uint16_t GPS_distanceToHome;
     uint16_t GPS_directionToHome;
     int32_t GPS_coord[2];
+    gpsSolutionData_t gpsSol;
 
     PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
     PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
@@ -283,7 +292,7 @@ TEST(OsdTest, TestStatsImperial)
     // and
     // these conditions occur during flight
     rssi = 1024;
-    GPS_speed = 500;
+    gpsSol.groundSpeed = 500;
     GPS_distanceToHome = 20;
     simulationBatteryVoltage = 158;
     simulationAltitude = 100;
@@ -291,7 +300,7 @@ TEST(OsdTest, TestStatsImperial)
     osdRefresh(simulationTime);
 
     rssi = 512;
-    GPS_speed = 800;
+    gpsSol.groundSpeed = 800;
     GPS_distanceToHome = 50;
     simulationBatteryVoltage = 147;
     simulationAltitude = 150;
@@ -299,7 +308,7 @@ TEST(OsdTest, TestStatsImperial)
     osdRefresh(simulationTime);
 
     rssi = 256;
-    GPS_speed = 200;
+    gpsSol.groundSpeed = 200;
     GPS_distanceToHome = 100;
     simulationBatteryVoltage = 152;
     simulationAltitude = 200;
@@ -343,7 +352,7 @@ TEST(OsdTest, TestStatsMetric)
     // and
     // these conditions occur during flight (simplified to less assignments than previous test)
     rssi = 256;
-    GPS_speed = 800;
+    gpsSol.groundSpeed = 800;
     GPS_distanceToHome = 100;
     simulationBatteryVoltage = 147;
     simulationAltitude = 200;

From b9ebf8f4fd2d628efb7e8a9fd0b39a16a681378f Mon Sep 17 00:00:00 2001
From: blckmn <blackman@xtra.com.au>
Date: Wed, 28 Jun 2017 21:59:42 +1000
Subject: [PATCH 07/15] PWM output calculations dynamic based on clock speed

---
 src/main/drivers/pwm_output.c        | 107 +++++++++++++--------------
 src/main/drivers/pwm_output.h        |  35 +++------
 src/main/drivers/rx_pwm.c            |  26 ++-----
 src/main/drivers/rx_pwm.h            |   2 +-
 src/main/drivers/serial_escserial.c  |  12 ++-
 src/main/drivers/serial_softserial.c |   6 +-
 src/main/drivers/timer.c             |  29 +++++---
 src/main/drivers/timer.h             |   9 ++-
 src/main/drivers/timer_hal.c         |  27 ++++---
 src/main/fc/fc_init.c                |   2 +-
 src/main/target/common_fc_pre.h      |   1 +
 11 files changed, 117 insertions(+), 139 deletions(-)

diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 1f725554e..7d7ed4fc0 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -28,9 +28,6 @@
 #include "timer.h"
 #include "drivers/pwm_output.h"
 
-#define MULTISHOT_5US_PW    (MULTISHOT_TIMER_MHZ * 5)
-#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
-
 static pwmWriteFunc *pwmWrite;
 static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
 static pwmCompleteWriteFunc *pwmCompleteWrite = NULL;
@@ -99,16 +96,19 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8
 #endif
 }
 
-static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value, uint8_t inversion)
+static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion)
 {
 #if defined(USE_HAL_DRIVER)
     TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
     if(Handle == NULL) return;
 #endif
 
-    configTimeBase(timerHardware->tim, period, mhz);
-    pwmOCConfig(timerHardware->tim, timerHardware->channel, value,
-        inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
+    configTimeBase(timerHardware->tim, period, hz);
+    pwmOCConfig(timerHardware->tim, 
+        timerHardware->channel, 
+        value,
+        inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output
+        );
 
 #if defined(USE_HAL_DRIVER)
     if(timerHardware->output & TIMER_OUTPUT_N_CHANNEL)
@@ -122,7 +122,7 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
 #endif
 
     port->ccr = timerChCCR(timerHardware);
-    port->period = period;
+
     port->tim = timerHardware->tim;
 
     *port->ccr = 0;
@@ -134,29 +134,10 @@ static void pwmWriteUnused(uint8_t index, float value)
     UNUSED(value);
 }
 
-static void pwmWriteBrushed(uint8_t index, float value)
-{
-    *motors[index].ccr = lrintf((value - 1000) * motors[index].period / 1000);
-}
-
 static void pwmWriteStandard(uint8_t index, float value)
 {
-    *motors[index].ccr = lrintf(value);
-}
-
-static void pwmWriteOneShot125(uint8_t index, float value)
-{
-    *motors[index].ccr = lrintf(value * ONESHOT125_TIMER_MHZ/8.0f);
-}
-
-static void pwmWriteOneShot42(uint8_t index, float value)
-{
-    *motors[index].ccr = lrintf(value * ONESHOT42_TIMER_MHZ/24.0f);
-}
-
-static void pwmWriteMultiShot(uint8_t index, float value)
-{
-    *motors[index].ccr = lrintf(((value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW);
+    /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */
+    *motors[index].ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset);
 }
 
 #ifdef USE_DSHOT
@@ -246,32 +227,32 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
 {
     memset(motors, 0, sizeof(motors));
     
-    uint32_t timerMhzCounter = 0;
     bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
 
+    float sMin = 0;
+    float sLen = 0;
     switch (motorConfig->motorPwmProtocol) {
     default:
     case PWM_TYPE_ONESHOT125:
-        timerMhzCounter = ONESHOT125_TIMER_MHZ;
-        pwmWrite = &pwmWriteOneShot125;
+        sMin = 125e-6f;
+        sLen = 125e-6f; 
         break;
     case PWM_TYPE_ONESHOT42:
-        timerMhzCounter = ONESHOT42_TIMER_MHZ;
-        pwmWrite = &pwmWriteOneShot42;
+        sMin = 42e-6f;
+        sLen = 42e-6f;
         break;
     case PWM_TYPE_MULTISHOT:
-        timerMhzCounter = MULTISHOT_TIMER_MHZ;
-        pwmWrite = &pwmWriteMultiShot;
+        sMin = 5e-6f;
+        sLen = 20e-6f; 
         break;
     case PWM_TYPE_BRUSHED:
-        timerMhzCounter = PWM_BRUSHED_TIMER_MHZ;
-        pwmWrite = &pwmWriteBrushed;
+        sMin = 0;
         useUnsyncedPwm = true;
         idlePulse = 0;
         break;
     case PWM_TYPE_STANDARD:
-        timerMhzCounter = PWM_TIMER_MHZ;
-        pwmWrite = &pwmWriteStandard;
+        sMin = 1e-3f;
+        sLen = 1e-3f; 
         useUnsyncedPwm = true;
         idlePulse = 0;
         break;
@@ -295,6 +276,7 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
     }
 
     if (!isDshot) {
+        pwmWrite = &pwmWriteStandard;
         pwmCompleteWrite = useUnsyncedPwm ? &pwmCompleteWriteUnused : &pwmCompleteOneshotMotorUpdate;
     }
 
@@ -314,7 +296,9 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
 
 #ifdef USE_DSHOT
         if (isDshot) {
-            pwmDshotMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol,
+            pwmDshotMotorHardwareConfig(timerHardware, 
+                motorIndex, 
+                motorConfig->motorPwmProtocol,
                 motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
             motors[motorIndex].enabled = true;
             continue;
@@ -328,12 +312,24 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
         IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
 #endif
 
-        if (useUnsyncedPwm) {
-            const uint32_t hz = timerMhzCounter * 1000000;
-            pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse, motorConfig->motorPwmInversion);
-        } else {
-            pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0, motorConfig->motorPwmInversion);
-        }
+        /* standard PWM outputs */
+        const unsigned pwmRateHz = useUnsyncedPwm ? motorConfig->motorPwmRate : ceilf(1 / (sMin + sLen));
+
+        const uint32_t clock = timerClock(timerHardware->tim);
+        /* used to find the desired timer frequency for max resolution */
+        const unsigned prescaler = ((clock / pwmRateHz) + 0xffff) / 0x10000; /* rounding up */
+        const uint32_t hz = clock / prescaler;
+        const unsigned period = hz / pwmRateHz;
+
+        /* 
+            if brushed then it is the entire length of the period. 
+            TODO: this can be moved back to periodMin and periodLen 
+            once mixer outputs a 0..1 float value.
+        */
+        motors[motorIndex].pulseScale = ((motorConfig->motorPwmProtocol == PWM_TYPE_BRUSHED) ? period : (sLen * hz)) / 1000.0f;
+        motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000);
+        
+        pwmOutConfig(&motors[motorIndex], timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion);
 
         bool timerAlreadyUsed = false;
         for (int i = 0; i < motorIndex; i++) {
@@ -364,16 +360,16 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
 {
     switch (pwmProtocolType) {
     case(PWM_TYPE_PROSHOT1000):
-        return MOTOR_PROSHOT1000_MHZ * 1000000;
+        return MOTOR_PROSHOT1000_HZ;
     case(PWM_TYPE_DSHOT1200):
-        return MOTOR_DSHOT1200_MHZ * 1000000;
+        return MOTOR_DSHOT1200_HZ;
     case(PWM_TYPE_DSHOT600):
-        return MOTOR_DSHOT600_MHZ * 1000000;
+        return MOTOR_DSHOT600_HZ;
     case(PWM_TYPE_DSHOT300):
-        return MOTOR_DSHOT300_MHZ * 1000000;
+        return MOTOR_DSHOT300_HZ;
     default:
     case(PWM_TYPE_DSHOT150):
-        return MOTOR_DSHOT150_MHZ * 1000000;
+        return MOTOR_DSHOT150_HZ;
     }
 }
 
@@ -460,8 +456,7 @@ void servoDevInit(const servoDevConfig_t *servoConfig)
             /* flag failure and disable ability to arm */
             break;
         }
-
-        pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
+        pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_HZ, PWM_TIMER_HZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
         servos[servoIndex].enabled = true;
     }
 }
@@ -474,7 +469,7 @@ void pwmWriteBeeper(bool onoffBeep)
         if(!beeperPwm.io)
             return;
         if(onoffBeep == true) {
-            *beeperPwm.ccr = (1000000/freqBeep)/2;
+            *beeperPwm.ccr = (PWM_TIMER_HZ / freqBeep) / 2;
             beeperPwm.enabled = true;
         } else {
             *beeperPwm.ccr = 0;
@@ -500,7 +495,7 @@ void beeperPwmInit(IO_t io, uint16_t frequency)
             IOConfigGPIO(beeperPwm.io, IOCFG_AF_PP);
 #endif
             freqBeep = frequency;
-            pwmOutConfig(&beeperPwm, timer, PWM_TIMER_MHZ, 1000000/freqBeep, (1000000/freqBeep)/2,0);
+            pwmOutConfig(&beeperPwm, timer, PWM_TIMER_HZ, PWM_TIMER_HZ / freqBeep, (PWM_TIMER_HZ / freqBeep) / 2, 0);
         }
         *beeperPwm.ccr = 0;
         beeperPwm.enabled = false;
diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h
index c37fa960f..34c56c69d 100644
--- a/src/main/drivers/pwm_output.h
+++ b/src/main/drivers/pwm_output.h
@@ -20,7 +20,9 @@
 #include "drivers/io_types.h"
 #include "timer.h"
 
+#ifndef MAX_SUPPORTED_MOTORS 
 #define MAX_SUPPORTED_MOTORS 12
+#endif
 
 #if defined(USE_QUAD_MIXER_ONLY)
 #define MAX_SUPPORTED_SERVOS 1
@@ -75,45 +77,29 @@ typedef enum {
     PWM_TYPE_MAX
 } motorPwmProtocolTypes_e;
 
-#define PWM_TIMER_MHZ         1
+#define PWM_TIMER_HZ          MHZ_TO_HZ(1)
 
 #ifdef USE_DSHOT
 #define MAX_DMA_TIMERS        8
 
-#define MOTOR_DSHOT1200_MHZ   24
-#define MOTOR_DSHOT600_MHZ    12
-#define MOTOR_DSHOT300_MHZ    6
-#define MOTOR_DSHOT150_MHZ    3
+#define MOTOR_DSHOT1200_HZ    MHZ_TO_HZ(24)
+#define MOTOR_DSHOT600_HZ     MHZ_TO_HZ(12)
+#define MOTOR_DSHOT300_HZ     MHZ_TO_HZ(6)
+#define MOTOR_DSHOT150_HZ     MHZ_TO_HZ(3)
 
 #define MOTOR_BIT_0           7
 #define MOTOR_BIT_1           14
 #define MOTOR_BITLENGTH       19
 
-#define MOTOR_PROSHOT1000_MHZ        24 
+#define MOTOR_PROSHOT1000_HZ         MHZ_TO_HZ(24) 
 #define PROSHOT_BASE_SYMBOL          24 // 1uS
 #define PROSHOT_BIT_WIDTH            3
 #define MOTOR_NIBBLE_LENGTH_PROSHOT  96 // 4uS
 #endif
 
-#if defined(STM32F40_41xxx) // must be multiples of timer clock
-#define ONESHOT125_TIMER_MHZ  12
-#define ONESHOT42_TIMER_MHZ   21
-#define MULTISHOT_TIMER_MHZ   84
-#define PWM_BRUSHED_TIMER_MHZ 21
-#elif defined(STM32F7) // must be multiples of timer clock
-#define ONESHOT125_TIMER_MHZ  9
-#define ONESHOT42_TIMER_MHZ   27
-#define MULTISHOT_TIMER_MHZ   54
-#define PWM_BRUSHED_TIMER_MHZ 27
-#else
-#define ONESHOT125_TIMER_MHZ  8
-#define ONESHOT42_TIMER_MHZ   24
-#define MULTISHOT_TIMER_MHZ   72
-#define PWM_BRUSHED_TIMER_MHZ 24
-#endif
 
 #define DSHOT_DMA_BUFFER_SIZE   18 /* resolution + frame reset (2us) */
-#define PROSHOT_DMA_BUFFER_SIZE 6/* resolution + frame reset (2us) */
+#define PROSHOT_DMA_BUFFER_SIZE 6  /* resolution + frame reset (2us) */
 
 typedef struct {
     TIM_TypeDef *timer;
@@ -149,9 +135,10 @@ typedef struct {
     volatile timCCR_t *ccr;
     TIM_TypeDef *tim;
     bool forceOverflow;
-    uint16_t period;
     bool enabled;
     IO_t io;
+    float pulseScale;
+    float pulseOffset;
 } pwmOutputPort_t;
 
 typedef struct motorDevConfig_s {
diff --git a/src/main/drivers/rx_pwm.c b/src/main/drivers/rx_pwm.c
index 97c5822ea..863e21e8e 100644
--- a/src/main/drivers/rx_pwm.c
+++ b/src/main/drivers/rx_pwm.c
@@ -391,7 +391,7 @@ void pwmRxInit(const pwmConfig_t *pwmConfig)
         IOConfigGPIO(io, IOCFG_AF_PP);
 #endif
 
-        timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_MHZ);
+        timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_HZ);
         timerChCCHandlerInit(&port->edgeCb, pwmEdgeCallback);
         timerChOvrHandlerInit(&port->overflowCb, pwmOverflowCallback);
         timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb);
@@ -408,7 +408,7 @@ void pwmRxInit(const pwmConfig_t *pwmConfig)
 #define UNUSED_PPM_TIMER_REFERENCE 0
 #define FIRST_PWM_PORT 0
 
-void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer, uint8_t pwmProtocol)
+void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer)
 {
     pwmOutputPort_t *motors = pwmGetMotors();
     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS; motorIndex++) {
@@ -416,26 +416,12 @@ void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer, uint8_t pwmProtocol)
             continue;
         }
 
-        switch (pwmProtocol)
-        {
-        case PWM_TYPE_ONESHOT125:
-            ppmCountDivisor = ONESHOT125_TIMER_MHZ;
-            break;
-        case PWM_TYPE_ONESHOT42:
-            ppmCountDivisor = ONESHOT42_TIMER_MHZ;
-            break;
-        case PWM_TYPE_MULTISHOT:
-            ppmCountDivisor = MULTISHOT_TIMER_MHZ;
-            break;
-        case PWM_TYPE_BRUSHED:
-            ppmCountDivisor = PWM_BRUSHED_TIMER_MHZ;
-            break;
-        }
+        ppmCountDivisor = timerClock(pwmTimer) / (pwmTimer->PSC + 1);
         return;
     }
 }
 
-void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol)
+void ppmRxInit(const ppmConfig_t *ppmConfig)
 {
     ppmResetDevice();
 
@@ -447,7 +433,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol)
         return;
     }
 
-    ppmAvoidPWMTimerClash(timer->tim, pwmProtocol);
+    ppmAvoidPWMTimerClash(timer->tim);
 
     port->mode = INPUT_MODE_PPM;
     port->timerHardware = timer;
@@ -462,7 +448,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol)
     IOConfigGPIO(io, IOCFG_AF_PP);
 #endif
 
-    timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_MHZ);
+    timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_HZ);
     timerChCCHandlerInit(&port->edgeCb, ppmEdgeCallback);
     timerChOvrHandlerInit(&port->overflowCb, ppmOverflowCallback);
     timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb);
diff --git a/src/main/drivers/rx_pwm.h b/src/main/drivers/rx_pwm.h
index a5e462918..0943af646 100644
--- a/src/main/drivers/rx_pwm.h
+++ b/src/main/drivers/rx_pwm.h
@@ -36,7 +36,7 @@ typedef struct pwmConfig_s {
     inputFilteringMode_e inputFilteringMode;
 } pwmConfig_t;
 
-void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol);
+void ppmRxInit(const ppmConfig_t *ppmConfig);
 void pwmRxInit(const pwmConfig_t *pwmConfig);
 
 uint16_t pwmRead(uint8_t channel);
diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c
index 5595e5c50..5fbfe9778 100644
--- a/src/main/drivers/serial_escserial.c
+++ b/src/main/drivers/serial_escserial.c
@@ -184,8 +184,7 @@ static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8
         }
     } while (isTimerPeriodTooLarge(timerPeriod));
 
-    uint8_t mhz = clock / 1000000;
-    timerConfigure(timerHardwarePtr, timerPeriod, mhz);
+    timerConfigure(timerHardwarePtr, timerPeriod, clock);
     timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerBL);
     timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
 }
@@ -193,9 +192,8 @@ static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8
 static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, portOptions_t options)
 {
     // start bit is usually a FALLING signal
-    uint8_t mhz = SystemCoreClock / 2000000;
     TIM_DeInit(timerHardwarePtr->tim);
-    timerConfigure(timerHardwarePtr, 0xFFFF, mhz);
+    timerConfigure(timerHardwarePtr, 0xFFFF, SystemCoreClock / 2);
     escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, (options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling);
     timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeBL);
     timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
@@ -203,9 +201,9 @@ static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8
 
 static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
 {
-    uint32_t timerPeriod=34;
+    uint32_t timerPeriod = 34;
     TIM_DeInit(timerHardwarePtr->tim);
-    timerConfigure(timerHardwarePtr, timerPeriod, 1);
+    timerConfigure(timerHardwarePtr, timerPeriod, 1e6);
     timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerEsc);
     timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
 }
@@ -228,7 +226,7 @@ static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint
 {
     // start bit is usually a FALLING signal
     TIM_DeInit(timerHardwarePtr->tim);
-    timerConfigure(timerHardwarePtr, 0xFFFF, 1);
+    timerConfigure(timerHardwarePtr, 0xFFFF, 1e6);
     escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
     timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc);
     timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c
index 7540a521b..1d552bed8 100644
--- a/src/main/drivers/serial_softserial.c
+++ b/src/main/drivers/serial_softserial.c
@@ -204,7 +204,7 @@ static bool isTimerPeriodTooLarge(uint32_t timerPeriod)
 
 static void serialTimerConfigureTimebase(const timerHardware_t *timerHardwarePtr, uint32_t baud)
 {
-    uint32_t baseClock = SystemCoreClock / timerClockDivisor(timerHardwarePtr->tim);
+    uint32_t baseClock = timerClock(timerHardwarePtr->tim);
     uint32_t clock = baseClock;
     uint32_t timerPeriod;
 
@@ -220,9 +220,7 @@ static void serialTimerConfigureTimebase(const timerHardware_t *timerHardwarePtr
         }
     } while (isTimerPeriodTooLarge(timerPeriod));
 
-    uint8_t mhz = baseClock / 1000000;
-
-    timerConfigure(timerHardwarePtr, timerPeriod, mhz);
+    timerConfigure(timerHardwarePtr, timerPeriod, baseClock);
 }
 
 static void resetBuffers(softSerial_t *softSerial)
diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c
index 050600cb0..57a2b69f4 100755
--- a/src/main/drivers/timer.c
+++ b/src/main/drivers/timer.c
@@ -229,16 +229,16 @@ void timerNVICConfigure(uint8_t irq)
     NVIC_Init(&NVIC_InitStructure);
 }
 
-void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
+void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz)
 {
     TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
 
     TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
-    TIM_TimeBaseStructure.TIM_Period = (period - 1) & 0xffff; // AKA TIMx_ARR
+    TIM_TimeBaseStructure.TIM_Period = (period - 1) & 0xFFFF; // AKA TIMx_ARR
 
     // "The counter clock frequency (CK_CNT) is equal to f CK_PSC / (PSC[15:0] + 1)." - STM32F10x Reference Manual 14.4.11
     // Thus for 1Mhz: 72000000 / 1000000 = 72, 72 - 1 = 71 = TIM_Prescaler
-    TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / timerClockDivisor(tim) / ((uint32_t)mhz * 1000000)) - 1;
+    TIM_TimeBaseStructure.TIM_Prescaler = (timerClock(tim) / hz) - 1;
 
     TIM_TimeBaseStructure.TIM_ClockDivision = 0;
     TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
@@ -246,9 +246,9 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
 }
 
 // old interface for PWM inputs. It should be replaced
-void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint8_t mhz)
+void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint32_t hz)
 {
-    configTimeBase(timerHardwarePtr->tim, period, mhz);
+    configTimeBase(timerHardwarePtr->tim, period, hz);
     TIM_Cmd(timerHardwarePtr->tim, ENABLE);
 
     uint8_t irq = timerInputIrq(timerHardwarePtr->tim);
@@ -849,14 +849,19 @@ uint16_t timerDmaSource(uint8_t channel)
 
 uint16_t timerGetPrescalerByDesiredMhz(TIM_TypeDef *tim, uint16_t mhz)
 {
-    // protection here for desired MHZ > timerClock???
-    if ((uint32_t)(mhz * 1000000) > timerClock(tim)) {
-        return 0;
-    }
-    return (uint16_t)(round((timerClock(tim) / (mhz * 1000000)) - 1));
+    return timerGetPrescalerByDesiredHertz(tim, MHZ_TO_HZ(mhz));
 }
 
-uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hertz)
+uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hz)
 {
-    return ((uint16_t)((timerClock(tim) / (prescaler + 1)) / hertz));
+    return ((uint16_t)((timerClock(tim) / (prescaler + 1)) / hz));
+}
+
+uint16_t timerGetPrescalerByDesiredHertz(TIM_TypeDef *tim, uint32_t hz)
+{
+    // protection here for desired hertz > SystemCoreClock???
+    if (hz > timerClock(tim)) {
+        return 0;
+    }
+    return (uint16_t)((timerClock(tim) + hz / 2 ) / hz) - 1;
 }
diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h
index 2292ff6a7..00cf4d8bc 100644
--- a/src/main/drivers/timer.h
+++ b/src/main/drivers/timer.h
@@ -134,6 +134,8 @@ typedef enum {
 #define HARDWARE_TIMER_DEFINITION_COUNT 14
 #endif
 
+#define MHZ_TO_HZ(x) (x * 1000000)
+
 extern const timerHardware_t timerHardware[];
 extern const timerDef_t timerDefinitions[];
 
@@ -155,7 +157,7 @@ typedef enum {
     TYPE_TIMER
 } channelType_t;
 
-void timerConfigure(const timerHardware_t *timHw, uint16_t period, uint8_t mhz);  // This interface should be replaced.
+void timerConfigure(const timerHardware_t *timHw, uint16_t period, uint32_t hz);  // This interface should be replaced.
 
 void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterSamples);
 void timerChConfigICDual(const timerHardware_t* timHw, bool polarityRising, unsigned inputFilterSamples);
@@ -183,7 +185,7 @@ void timerForceOverflow(TIM_TypeDef *tim);
 uint8_t timerClockDivisor(TIM_TypeDef *tim);
 uint32_t timerClock(TIM_TypeDef *tim);
 
-void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz);  // TODO - just for migration
+void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz);  // TODO - just for migration
 
 rccPeriphTag_t timerRCC(TIM_TypeDef *tim);
 uint8_t timerInputIrq(TIM_TypeDef *tim);
@@ -200,5 +202,6 @@ void timerOCPreloadConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t preload);
 volatile timCCR_t *timerCCR(TIM_TypeDef *tim, uint8_t channel);
 uint16_t timerDmaSource(uint8_t channel);
 
+uint16_t timerGetPrescalerByDesiredHertz(TIM_TypeDef *tim, uint32_t hz);
 uint16_t timerGetPrescalerByDesiredMhz(TIM_TypeDef *tim, uint16_t mhz);
-uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hertz);
+uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hz);
diff --git a/src/main/drivers/timer_hal.c b/src/main/drivers/timer_hal.c
index 350c1b432..30eb04e83 100644
--- a/src/main/drivers/timer_hal.c
+++ b/src/main/drivers/timer_hal.c
@@ -241,7 +241,7 @@ TIM_HandleTypeDef* timerFindTimerHandle(TIM_TypeDef *tim)
     return &timerHandle[timerIndex].Handle;
 }
 
-void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
+void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz)
 {
     uint8_t timerIndex = lookupTimerIndex(tim);
     if (timerIndex >= USED_TIMER_COUNT) {
@@ -256,7 +256,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
     timerHandle[timerIndex].Handle.Instance = tim;
 
     timerHandle[timerIndex].Handle.Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR
-    timerHandle[timerIndex].Handle.Init.Prescaler = (SystemCoreClock / timerClockDivisor(tim) / ((uint32_t)mhz * 1000000)) - 1;
+    timerHandle[timerIndex].Handle.Init.Prescaler = (timerClock(tim) / hz) - 1;
 
     timerHandle[timerIndex].Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
     timerHandle[timerIndex].Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
@@ -287,14 +287,14 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
 }
 
 // old interface for PWM inputs. It should be replaced
-void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint8_t mhz)
+void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint32_t hz)
 {
     uint8_t timerIndex = lookupTimerIndex(timerHardwarePtr->tim);
     if (timerIndex >= USED_TIMER_COUNT) {
         return;
     }
 
-    configTimeBase(timerHardwarePtr->tim, period, mhz);
+    configTimeBase(timerHardwarePtr->tim, period, hz);
     HAL_TIM_Base_Start(&timerHandle[timerIndex].Handle);
 
     uint8_t irq = timerInputIrq(timerHardwarePtr->tim);
@@ -898,14 +898,19 @@ uint16_t timerDmaSource(uint8_t channel)
 
 uint16_t timerGetPrescalerByDesiredMhz(TIM_TypeDef *tim, uint16_t mhz)
 {
-    // protection here for desired MHZ > SystemCoreClock???
-    if (mhz * 1000000 > (SystemCoreClock / timerClockDivisor(tim))) {
-        return 0;
-    }
-    return (uint16_t)(round((SystemCoreClock / timerClockDivisor(tim) / (mhz * 1000000)) - 1));
+    return timerGetPrescalerByDesiredHertz(tim, MHZ_TO_HZ(mhz));
 }
 
-uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hertz)
+uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hz)
 {
-    return ((uint16_t)((SystemCoreClock / timerClockDivisor(tim) / (prescaler + 1)) / hertz));
+    return ((uint16_t)((timerClock(tim) / (prescaler + 1)) / hz));
 }
+
+uint16_t timerGetPrescalerByDesiredHertz(TIM_TypeDef *tim, uint32_t hz)
+{
+    // protection here for desired hertz > SystemCoreClock???
+    if (hz > timerClock(tim)) {
+        return 0;
+    }
+    return (uint16_t)((timerClock(tim) + hz / 2 ) / hz) - 1;
+}
\ No newline at end of file
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 546322f28..a4fe04c35 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -384,7 +384,7 @@ void init(void)
     if (0) {}
 #if defined(USE_PPM)
     else if (feature(FEATURE_RX_PPM)) {
-          ppmRxInit(ppmConfig(), motorConfig()->dev.motorPwmProtocol);
+          ppmRxInit(ppmConfig());
     }
 #endif
 #if defined(USE_PWM)
diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h
index b53a0ddb7..a99bbb0e3 100644
--- a/src/main/target/common_fc_pre.h
+++ b/src/main/target/common_fc_pre.h
@@ -36,6 +36,7 @@
 // Using RX DMA disables the use of receive callbacks
 #define USE_UART1_RX_DMA
 #define USE_UART1_TX_DMA
+#define MAX_SUPPORTED_MOTORS 8
 #endif
 
 #ifdef STM32F3

From 4484ee061621e96066b2d05141efb7e7a474c909 Mon Sep 17 00:00:00 2001
From: blckmn <blackman@xtra.com.au>
Date: Wed, 28 Jun 2017 22:04:46 +1000
Subject: [PATCH 08/15] Removed incorrect floats

---
 src/main/drivers/serial_escserial.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c
index 5fbfe9778..f55f70d54 100644
--- a/src/main/drivers/serial_escserial.c
+++ b/src/main/drivers/serial_escserial.c
@@ -203,7 +203,7 @@ static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint
 {
     uint32_t timerPeriod = 34;
     TIM_DeInit(timerHardwarePtr->tim);
-    timerConfigure(timerHardwarePtr, timerPeriod, 1e6);
+    timerConfigure(timerHardwarePtr, timerPeriod, MHZ_TO_HZ(1));
     timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerEsc);
     timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
 }
@@ -226,7 +226,7 @@ static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint
 {
     // start bit is usually a FALLING signal
     TIM_DeInit(timerHardwarePtr->tim);
-    timerConfigure(timerHardwarePtr, 0xFFFF, 1e6);
+    timerConfigure(timerHardwarePtr, 0xFFFF, MHZ_TO_HZ(1));
     escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
     timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc);
     timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);

From 82be9ecd78d7b05f0ffe056c3ee3cbc50f138641 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Sun, 25 Jun 2017 13:30:17 +0100
Subject: [PATCH 09/15] For discussion - Reconfigurable OLED display
 alternative

---
 src/main/config/parameter_group_ids.h    |   3 +-
 src/main/drivers/bus.h                   |   7 +
 src/main/drivers/bus_i2c.h               |   7 +
 src/main/drivers/display.h               |   1 +
 src/main/drivers/display_ug2864hsweg01.c | 124 ++++++++--------
 src/main/drivers/display_ug2864hsweg01.h |  17 ++-
 src/main/fc/settings.c                   |   5 +
 src/main/io/dashboard.c                  | 181 ++++++++++++-----------
 src/main/io/dashboard.h                  |  17 +++
 src/main/io/displayport_oled.c           |  16 +-
 src/main/io/displayport_oled.h           |   2 +-
 11 files changed, 215 insertions(+), 165 deletions(-)

diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h
index e4b0bb3f5..889c7061d 100644
--- a/src/main/config/parameter_group_ids.h
+++ b/src/main/config/parameter_group_ids.h
@@ -107,7 +107,8 @@
 #define PG_SONAR_CONFIG 516
 #define PG_ESC_SENSOR_CONFIG 517
 #define PG_I2C_CONFIG 518
-#define PG_BETAFLIGHT_END 518
+#define PG_DASHBOARD_CONFIG 519
+#define PG_BETAFLIGHT_END 519
 
 
 // OSD configuration (subject to change)
diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h
index 08de5e1a5..e8c143601 100644
--- a/src/main/drivers/bus.h
+++ b/src/main/drivers/bus.h
@@ -19,14 +19,21 @@
 
 #include "platform.h"
 
+#include "drivers/bus_i2c.h"
 #include "drivers/io_types.h"
 
 typedef union busDevice_u {
     struct deviceSpi_s {
+        SPI_TypeDef *instance;
         IO_t csnPin;
     } spi;
+    struct deviceI2C_s {
+       I2CDevice device;
+       uint8_t address;
+    } i2c;
 } busDevice_t;
 
+
 #ifdef TARGET_BUS_INIT
 void targetBusInit(void);
 #endif
diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h
index bd2712599..0f9f0c538 100644
--- a/src/main/drivers/bus_i2c.h
+++ b/src/main/drivers/bus_i2c.h
@@ -45,6 +45,13 @@ typedef enum I2CDevice {
 #define I2CDEV_COUNT 4
 #endif
 
+// Macro to convert CLI bus number to I2CDevice.
+#define I2C_CFG_TO_DEV(x)   ((x) - 1)
+
+// I2C device address range in 8-bit address mode
+#define I2C_ADDR8_MIN       8
+#define I2C_ADDR8_MAX       119
+
 typedef struct i2cConfig_s {
     ioTag_t ioTagScl[I2CDEV_COUNT];
     ioTag_t ioTagSda[I2CDEV_COUNT];
diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h
index 1cecb77ca..6caf1767d 100644
--- a/src/main/drivers/display.h
+++ b/src/main/drivers/display.h
@@ -20,6 +20,7 @@
 struct displayPortVTable_s;
 typedef struct displayPort_s {
     const struct displayPortVTable_s *vTable;
+    void *device;
     uint8_t rows;
     uint8_t cols;
 
diff --git a/src/main/drivers/display_ug2864hsweg01.c b/src/main/drivers/display_ug2864hsweg01.c
index b9ac8528e..abcc402cf 100644
--- a/src/main/drivers/display_ug2864hsweg01.c
+++ b/src/main/drivers/display_ug2864hsweg01.c
@@ -176,78 +176,76 @@ static const uint8_t multiWiiFont[][5] = { // Refer to "Times New Roman" Font Da
                 { 0x7A, 0x7E, 0x7E, 0x7E, 0x7A }, //   (131)    - 0x00C8 Vertical Bargraph - 6 (full)
         };
 
-#define OLED_address   0x3C     // OLED at address 0x3C in 7bit
-
-static bool i2c_OLED_send_cmd(uint8_t command)
+static bool i2c_OLED_send_cmd(busDevice_t *bus, uint8_t command)
 {
-    return i2cWrite(OLED_I2C_INSTANCE, OLED_address, 0x80, command);
+    return i2cWrite(bus->i2c.device, bus->i2c.address, 0x80, command);
 }
 
-static bool i2c_OLED_send_byte(uint8_t val)
+static bool i2c_OLED_send_byte(busDevice_t *bus, uint8_t val)
 {
-    return i2cWrite(OLED_I2C_INSTANCE, OLED_address, 0x40, val);
+    return i2cWrite(bus->i2c.device, bus->i2c.address, 0x40, val);
 }
 
-void i2c_OLED_clear_display(void)
+void i2c_OLED_clear_display(busDevice_t *bus)
 {
-    i2c_OLED_send_cmd(0xa6);              // Set Normal Display
-    i2c_OLED_send_cmd(0xae);              // Display OFF
-    i2c_OLED_send_cmd(0x20);              // Set Memory Addressing Mode
-    i2c_OLED_send_cmd(0x00);              // Set Memory Addressing Mode to Horizontal addressing mode
-    i2c_OLED_send_cmd(0xb0);              // set page address to 0
-    i2c_OLED_send_cmd(0x40);              // Display start line register to 0
-    i2c_OLED_send_cmd(0);                 // Set low col address to 0
-    i2c_OLED_send_cmd(0x10);              // Set high col address to 0
+    i2c_OLED_send_cmd(bus, 0xa6);              // Set Normal Display
+    i2c_OLED_send_cmd(bus, 0xae);              // Display OFF
+    i2c_OLED_send_cmd(bus, 0x20);              // Set Memory Addressing Mode
+    i2c_OLED_send_cmd(bus, 0x00);              // Set Memory Addressing Mode to Horizontal addressing mode
+    i2c_OLED_send_cmd(bus, 0xb0);              // set page address to 0
+    i2c_OLED_send_cmd(bus, 0x40);              // Display start line register to 0
+    i2c_OLED_send_cmd(bus, 0);                 // Set low col address to 0
+    i2c_OLED_send_cmd(bus, 0x10);              // Set high col address to 0
     for(uint16_t i = 0; i < 1024; i++) {  // fill the display's RAM with graphic... 128*64 pixel picture
-        i2c_OLED_send_byte(0x00);  // clear
+        i2c_OLED_send_byte(bus, 0x00);  // clear
     }
-    i2c_OLED_send_cmd(0x81);              // Setup CONTRAST CONTROL, following byte is the contrast Value... always a 2 byte instruction
-    i2c_OLED_send_cmd(200);               // Here you can set the brightness 1 = dull, 255 is very bright
-    i2c_OLED_send_cmd(0xaf);              // display on
+    i2c_OLED_send_cmd(bus, 0x81);              // Setup CONTRAST CONTROL, following byte is the contrast Value... always a 2 byte instruction
+    i2c_OLED_send_cmd(bus, 200);               // Here you can set the brightness 1 = dull, 255 is very bright
+    i2c_OLED_send_cmd(bus, 0xaf);              // display on
 }
 
-void i2c_OLED_clear_display_quick(void)
+void i2c_OLED_clear_display_quick(busDevice_t *bus)
 {
-    i2c_OLED_send_cmd(0xb0);              // set page address to 0
-    i2c_OLED_send_cmd(0x40);              // Display start line register to 0
-    i2c_OLED_send_cmd(0);                 // Set low col address to 0
-    i2c_OLED_send_cmd(0x10);              // Set high col address to 0
+    i2c_OLED_send_cmd(bus, 0xb0);              // set page address to 0
+    i2c_OLED_send_cmd(bus, 0x40);              // Display start line register to 0
+    i2c_OLED_send_cmd(bus, 0);                 // Set low col address to 0
+    i2c_OLED_send_cmd(bus, 0x10);              // Set high col address to 0
     for(uint16_t i = 0; i < 1024; i++) {      // fill the display's RAM with graphic... 128*64 pixel picture
-        i2c_OLED_send_byte(0x00);  // clear
+        i2c_OLED_send_byte(bus, 0x00);  // clear
     }
 }
 
-void i2c_OLED_set_xy(uint8_t col, uint8_t row)
+void i2c_OLED_set_xy(busDevice_t *bus, uint8_t col, uint8_t row)
 {
-    i2c_OLED_send_cmd(0xb0 + row);                      //set page address
-    i2c_OLED_send_cmd(0x00 + ((CHARACTER_WIDTH_TOTAL * col) & 0x0f));         //set low col address
-    i2c_OLED_send_cmd(0x10 + (((CHARACTER_WIDTH_TOTAL * col) >> 4) & 0x0f));  //set high col address
+    i2c_OLED_send_cmd(bus, 0xb0 + row);                      //set page address
+    i2c_OLED_send_cmd(bus, 0x00 + ((CHARACTER_WIDTH_TOTAL * col) & 0x0f));         //set low col address
+    i2c_OLED_send_cmd(bus, 0x10 + (((CHARACTER_WIDTH_TOTAL * col) >> 4) & 0x0f));  //set high col address
 }
 
-void i2c_OLED_set_line(uint8_t row)
+void i2c_OLED_set_line(busDevice_t *bus, uint8_t row)
 {
-    i2c_OLED_send_cmd(0xb0 + row); //set page address
-    i2c_OLED_send_cmd(0);          //set low col address
-    i2c_OLED_send_cmd(0x10);       //set high col address
+    i2c_OLED_send_cmd(bus, 0xb0 + row); //set page address
+    i2c_OLED_send_cmd(bus, 0);          //set low col address
+    i2c_OLED_send_cmd(bus, 0x10);       //set high col address
 }
 
-void i2c_OLED_send_char(unsigned char ascii)
+void i2c_OLED_send_char(busDevice_t *bus, unsigned char ascii)
 {
     unsigned char i;
     uint8_t buffer;
     for (i = 0; i < 5; i++) {
         buffer = multiWiiFont[ascii - 32][i];
         buffer ^= CHAR_FORMAT;  // apply
-        i2c_OLED_send_byte(buffer);
+        i2c_OLED_send_byte(bus, buffer);
     }
-    i2c_OLED_send_byte(CHAR_FORMAT);    // the gap
+    i2c_OLED_send_byte(bus, CHAR_FORMAT);    // the gap
 }
 
-void i2c_OLED_send_string(const char *string)
+void i2c_OLED_send_string(busDevice_t *bus, const char *string)
 {
     // Sends a string of chars until null terminator
     while (*string) {
-        i2c_OLED_send_char(*string);
+        i2c_OLED_send_char(bus, *string);
         string++;
     }
 }
@@ -255,38 +253,38 @@ void i2c_OLED_send_string(const char *string)
 /**
 * according to http://www.adafruit.com/datasheets/UG-2864HSWEG01.pdf Chapter 4.4 Page 15
 */
-bool ug2864hsweg01InitI2C(void)
+bool ug2864hsweg01InitI2C(busDevice_t *bus)
 {
 
     // Set display OFF
-    if (!i2c_OLED_send_cmd(0xAE)) {
+    if (!i2c_OLED_send_cmd(bus, 0xAE)) {
         return false;
     }
 
-    i2c_OLED_send_cmd(0xD4); // Set Display Clock Divide Ratio / OSC Frequency
-    i2c_OLED_send_cmd(0x80); // Display Clock Divide Ratio / OSC Frequency
-    i2c_OLED_send_cmd(0xA8); // Set Multiplex Ratio
-    i2c_OLED_send_cmd(0x3F); // Multiplex Ratio for 128x64 (64-1)
-    i2c_OLED_send_cmd(0xD3); // Set Display Offset
-    i2c_OLED_send_cmd(0x00); // Display Offset
-    i2c_OLED_send_cmd(0x40); // Set Display Start Line
-    i2c_OLED_send_cmd(0x8D); // Set Charge Pump
-    i2c_OLED_send_cmd(0x14); // Charge Pump (0x10 External, 0x14 Internal DC/DC)
-    i2c_OLED_send_cmd(0xA1); // Set Segment Re-Map
-    i2c_OLED_send_cmd(0xC8); // Set Com Output Scan Direction
-    i2c_OLED_send_cmd(0xDA); // Set COM Hardware Configuration
-    i2c_OLED_send_cmd(0x12); // COM Hardware Configuration
-    i2c_OLED_send_cmd(0x81); // Set Contrast
-    i2c_OLED_send_cmd(0xCF); // Contrast
-    i2c_OLED_send_cmd(0xD9); // Set Pre-Charge Period
-    i2c_OLED_send_cmd(0xF1); // Set Pre-Charge Period (0x22 External, 0xF1 Internal)
-    i2c_OLED_send_cmd(0xDB); // Set VCOMH Deselect Level
-    i2c_OLED_send_cmd(0x40); // VCOMH Deselect Level
-    i2c_OLED_send_cmd(0xA4); // Set all pixels OFF
-    i2c_OLED_send_cmd(0xA6); // Set display not inverted
-    i2c_OLED_send_cmd(0xAF); // Set display On
+    i2c_OLED_send_cmd(bus, 0xD4); // Set Display Clock Divide Ratio / OSC Frequency
+    i2c_OLED_send_cmd(bus, 0x80); // Display Clock Divide Ratio / OSC Frequency
+    i2c_OLED_send_cmd(bus, 0xA8); // Set Multiplex Ratio
+    i2c_OLED_send_cmd(bus, 0x3F); // Multiplex Ratio for 128x64 (64-1)
+    i2c_OLED_send_cmd(bus, 0xD3); // Set Display Offset
+    i2c_OLED_send_cmd(bus, 0x00); // Display Offset
+    i2c_OLED_send_cmd(bus, 0x40); // Set Display Start Line
+    i2c_OLED_send_cmd(bus, 0x8D); // Set Charge Pump
+    i2c_OLED_send_cmd(bus, 0x14); // Charge Pump (0x10 External, 0x14 Internal DC/DC)
+    i2c_OLED_send_cmd(bus, 0xA1); // Set Segment Re-Map
+    i2c_OLED_send_cmd(bus, 0xC8); // Set Com Output Scan Direction
+    i2c_OLED_send_cmd(bus, 0xDA); // Set COM Hardware Configuration
+    i2c_OLED_send_cmd(bus, 0x12); // COM Hardware Configuration
+    i2c_OLED_send_cmd(bus, 0x81); // Set Contrast
+    i2c_OLED_send_cmd(bus, 0xCF); // Contrast
+    i2c_OLED_send_cmd(bus, 0xD9); // Set Pre-Charge Period
+    i2c_OLED_send_cmd(bus, 0xF1); // Set Pre-Charge Period (0x22 External, 0xF1 Internal)
+    i2c_OLED_send_cmd(bus, 0xDB); // Set VCOMH Deselect Level
+    i2c_OLED_send_cmd(bus, 0x40); // VCOMH Deselect Level
+    i2c_OLED_send_cmd(bus, 0xA4); // Set all pixels OFF
+    i2c_OLED_send_cmd(bus, 0xA6); // Set display not inverted
+    i2c_OLED_send_cmd(bus, 0xAF); // Set display On
 
-    i2c_OLED_clear_display();
+    i2c_OLED_clear_display(bus);
 
     return true;
 }
diff --git a/src/main/drivers/display_ug2864hsweg01.h b/src/main/drivers/display_ug2864hsweg01.h
index 3a51eab7a..6d31ebf7a 100644
--- a/src/main/drivers/display_ug2864hsweg01.h
+++ b/src/main/drivers/display_ug2864hsweg01.h
@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include "drivers/bus.h"
+
 #define SCREEN_WIDTH 128
 #define SCREEN_HEIGHT 64
 
@@ -34,12 +36,11 @@
 #define VERTICAL_BARGRAPH_ZERO_CHARACTER (128 + 32)
 #define VERTICAL_BARGRAPH_CHARACTER_COUNT 7
 
-bool ug2864hsweg01InitI2C(void);
-
-void i2c_OLED_set_xy(uint8_t col, uint8_t row);
-void i2c_OLED_set_line(uint8_t row);
-void i2c_OLED_send_char(unsigned char ascii);
-void i2c_OLED_send_string(const char *string);
-void i2c_OLED_clear_display(void);
-void i2c_OLED_clear_display_quick(void);
+bool ug2864hsweg01InitI2C(busDevice_t *bus);
 
+void i2c_OLED_set_xy(busDevice_t *bus, uint8_t col, uint8_t row);
+void i2c_OLED_set_line(busDevice_t *bus, uint8_t row);
+void i2c_OLED_send_char(busDevice_t *bus, unsigned char ascii);
+void i2c_OLED_send_string(busDevice_t *bus, const char *string);
+void i2c_OLED_clear_display(busDevice_t *bus);
+void i2c_OLED_clear_display_quick(busDevice_t *bus);
diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c
index 32791c76c..e29d9ff5e 100644
--- a/src/main/fc/settings.c
+++ b/src/main/fc/settings.c
@@ -53,6 +53,7 @@
 #include "flight/pid.h"
 #include "flight/servos.h"
 
+#include "io/dashboard.h"
 #include "io/gimbal.h"
 #include "io/gps.h"
 #include "io/ledstrip.h"
@@ -720,6 +721,10 @@ const clivalue_t valueTable[] = {
     { "esc_sensor_halfduplex",          VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, halfDuplex) },
 #endif
     { "led_inversion",                  VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, ((1 << STATUS_LED_NUMBER) - 1) }, PG_STATUS_LED_CONFIG, offsetof(statusLedConfig_t, inversion) },
+#ifdef USE_DASHBOARD
+    { "dashboard_i2c_bus",           VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, I2CDEV_COUNT }, PG_DASHBOARD_CONFIG, offsetof(dashboardConfig_t, device) },
+    { "dashboard_i2c_addr",          VAR_UINT8  | MASTER_VALUE, .config.minmax = { I2C_ADDR8_MIN, I2C_ADDR8_MAX }, PG_DASHBOARD_CONFIG, offsetof(dashboardConfig_t, address) },
+#endif
 };
 
 const uint16_t valueTableEntryCount = ARRAYLEN(valueTable);
diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c
index f54511fb5..a954abb9d 100644
--- a/src/main/io/dashboard.c
+++ b/src/main/io/dashboard.c
@@ -31,6 +31,7 @@
 
 #include "build/build_config.h"
 
+#include "drivers/bus.h"
 #include "drivers/display.h"
 #include "drivers/display_ug2864hsweg01.h"
 #include "drivers/time.h"
@@ -71,11 +72,20 @@
 #include "sensors/sensors.h"
 
 
+PG_REGISTER_WITH_RESET_TEMPLATE(dashboardConfig_t, dashboardConfig, PG_DASHBOARD_CONFIG, 0);
+
+PG_RESET_TEMPLATE(dashboardConfig_t, dashboardConfig,
+    .device = DASHBOARD_I2C_INSTANCE,
+    .address = DASHBOARD_I2C_ADDRESS,
+);
+
 #define MICROSECONDS_IN_A_SECOND (1000 * 1000)
 
 #define DISPLAY_UPDATE_FREQUENCY (MICROSECONDS_IN_A_SECOND / 5)
 #define PAGE_CYCLE_FREQUENCY (MICROSECONDS_IN_A_SECOND * 5)
 
+static busDevice_t bus;
+
 static uint32_t nextDisplayUpdateAt = 0;
 static bool dashboardPresent = false;
 
@@ -119,12 +129,14 @@ typedef struct pageState_s {
 
 static pageState_t pageState;
 
-void resetDisplay(void) {
-    dashboardPresent = ug2864hsweg01InitI2C();
+void resetDisplay(void)
+{
+    dashboardPresent = ug2864hsweg01InitI2C(&bus);
 }
 
-void LCDprint(uint8_t i) {
-   i2c_OLED_send_char(i);
+void LCDprint(uint8_t i)
+{
+   i2c_OLED_send_char(&bus, i);
 }
 
 void padLineBuffer(void)
@@ -170,8 +182,8 @@ void fillScreenWithCharacters()
 {
     for (uint8_t row = 0; row < SCREEN_CHARACTER_ROW_COUNT; row++) {
         for (uint8_t column = 0; column < SCREEN_CHARACTER_COLUMN_COUNT; column++) {
-            i2c_OLED_set_xy(column, row);
-            i2c_OLED_send_char('A' + column);
+            i2c_OLED_set_xy(&bus, column, row);
+            i2c_OLED_send_char(&bus, 'A' + column);
         }
     }
 }
@@ -181,22 +193,22 @@ void fillScreenWithCharacters()
 void updateTicker(void)
 {
     static uint8_t tickerIndex = 0;
-    i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 1, 0);
-    i2c_OLED_send_char(tickerCharacters[tickerIndex]);
+    i2c_OLED_set_xy(&bus, SCREEN_CHARACTER_COLUMN_COUNT - 1, 0);
+    i2c_OLED_send_char(&bus, tickerCharacters[tickerIndex]);
     tickerIndex++;
     tickerIndex = tickerIndex % TICKER_CHARACTER_COUNT;
 }
 
 void updateRxStatus(void)
 {
-    i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 2, 0);
+    i2c_OLED_set_xy(&bus, SCREEN_CHARACTER_COLUMN_COUNT - 2, 0);
     char rxStatus = '!';
     if (rxIsReceivingSignal()) {
         rxStatus = 'r';
     } if (rxAreFlightChannelsValid()) {
         rxStatus = 'R';
     }
-    i2c_OLED_send_char(rxStatus);
+    i2c_OLED_send_char(&bus, rxStatus);
 }
 
 void updateFailsafeStatus(void)
@@ -222,19 +234,19 @@ void updateFailsafeStatus(void)
             failsafeIndicator = 'r';
             break;
     }
-    i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 3, 0);
-    i2c_OLED_send_char(failsafeIndicator);
+    i2c_OLED_set_xy(&bus, SCREEN_CHARACTER_COLUMN_COUNT - 3, 0);
+    i2c_OLED_send_char(&bus, failsafeIndicator);
 }
 
 void showTitle()
 {
-    i2c_OLED_set_line(0);
-    i2c_OLED_send_string(pageState.page->title);
+    i2c_OLED_set_line(&bus, 0);
+    i2c_OLED_send_string(&bus, pageState.page->title);
 }
 
 void handlePageChange(void)
 {
-    i2c_OLED_clear_display_quick();
+    i2c_OLED_clear_display_quick(&bus);
     showTitle();
 }
 
@@ -253,7 +265,7 @@ void showRxPage(void)
 {
 
     for (uint8_t channelIndex = 0; channelIndex < rxRuntimeConfig.channelCount && channelIndex < RX_CHANNELS_PER_PAGE_COUNT; channelIndex += 2) {
-        i2c_OLED_set_line((channelIndex / 2) + PAGE_TITLE_LINE_COUNT);
+        i2c_OLED_set_line(&bus, (channelIndex / 2) + PAGE_TITLE_LINE_COUNT);
 
         drawRxChannel(channelIndex, HALF_SCREEN_CHARACTER_COLUMN_COUNT);
 
@@ -274,11 +286,11 @@ void showWelcomePage(void)
     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 
     tfp_sprintf(lineBuffer, "v%s (%s)", FC_VERSION_STRING, shortGitRevision);
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_line(&bus, rowIndex++);
+    i2c_OLED_send_string(&bus, lineBuffer);
 
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(targetName);
+    i2c_OLED_set_line(&bus, rowIndex++);
+    i2c_OLED_send_string(&bus, targetName);
 }
 
 void showArmedPage(void)
@@ -290,8 +302,8 @@ void showProfilePage(void)
     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 
     tfp_sprintf(lineBuffer, "Profile: %d", getCurrentPidProfileIndex());
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_line(&bus, rowIndex++);
+    i2c_OLED_send_string(&bus, lineBuffer);
 
     static const char* const axisTitles[3] = {"ROL", "PIT", "YAW"};
     const pidProfile_t *pidProfile = currentPidProfile;
@@ -303,14 +315,14 @@ void showProfilePage(void)
             pidProfile->pid[axis].D
         );
         padLineBuffer();
-        i2c_OLED_set_line(rowIndex++);
-        i2c_OLED_send_string(lineBuffer);
+        i2c_OLED_set_line(&bus, rowIndex++);
+        i2c_OLED_send_string(&bus, lineBuffer);
     }
 
     const uint8_t currentRateProfileIndex = getCurrentControlRateProfileIndex();
     tfp_sprintf(lineBuffer, "Rate profile: %d", currentRateProfileIndex);
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_line(&bus, rowIndex++);
+    i2c_OLED_send_string(&bus, lineBuffer);
 
     const controlRateConfig_t *controlRateConfig = controlRateProfiles(currentRateProfileIndex);
     tfp_sprintf(lineBuffer, "RCE: %d, RCR: %d",
@@ -318,8 +330,8 @@ void showProfilePage(void)
         controlRateConfig->rcRate8
     );
     padLineBuffer();
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_line(&bus, rowIndex++);
+    i2c_OLED_send_string(&bus, lineBuffer);
 
     tfp_sprintf(lineBuffer, "RR:%d PR:%d YR:%d",
         controlRateConfig->rates[FD_ROLL],
@@ -327,8 +339,8 @@ void showProfilePage(void)
         controlRateConfig->rates[FD_YAW]
     );
     padLineBuffer();
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_line(&bus, rowIndex++);
+    i2c_OLED_send_string(&bus, lineBuffer);
 }
 #define SATELLITE_COUNT (sizeof(GPS_svinfo_cno) / sizeof(GPS_svinfo_cno[0]))
 #define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2)
@@ -351,77 +363,77 @@ void showGpsPage() {
         gpsTicker = gpsTicker % TICKER_CHARACTER_COUNT;
     }
 
-    i2c_OLED_set_xy(0, rowIndex);
-    i2c_OLED_send_char(tickerCharacters[gpsTicker]);
+    i2c_OLED_set_xy(&bus, 0, rowIndex);
+    i2c_OLED_send_char(&bus, tickerCharacters[gpsTicker]);
 
-    i2c_OLED_set_xy(MAX(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
+    i2c_OLED_set_xy(&bus, MAX(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
 
     uint32_t index;
     for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; index++) {
         uint8_t bargraphOffset = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1);
         bargraphOffset = MIN(bargraphOffset, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1);
-        i2c_OLED_send_char(VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset);
+        i2c_OLED_send_char(&bus, VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset);
     }
 
 
     char fixChar = STATE(GPS_FIX) ? 'Y' : 'N';
     tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", gpsSol.numSat, fixChar);
     padLineBuffer();
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_line(&bus, rowIndex++);
+    i2c_OLED_send_string(&bus, lineBuffer);
 
     tfp_sprintf(lineBuffer, "La/Lo: %d/%d", gpsSol.llh.lat / GPS_DEGREES_DIVIDER, gpsSol.llh.lon / GPS_DEGREES_DIVIDER);
     padLineBuffer();
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_line(&bus, rowIndex++);
+    i2c_OLED_send_string(&bus, lineBuffer);
 
     tfp_sprintf(lineBuffer, "Spd: %d", gpsSol.groundSpeed);
     padHalfLineBuffer();
-    i2c_OLED_set_line(rowIndex);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_line(&bus, rowIndex);
+    i2c_OLED_send_string(&bus, lineBuffer);
 
     tfp_sprintf(lineBuffer, "GC: %d", gpsSol.groundCourse);
     padHalfLineBuffer();
-    i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_xy(&bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
+    i2c_OLED_send_string(&bus, lineBuffer);
 
     tfp_sprintf(lineBuffer, "RX: %d", GPS_packetCount);
     padHalfLineBuffer();
-    i2c_OLED_set_line(rowIndex);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_line(&bus, rowIndex);
+    i2c_OLED_send_string(&bus, lineBuffer);
 
     tfp_sprintf(lineBuffer, "ERRs: %d", gpsData.errors, gpsData.timeouts);
     padHalfLineBuffer();
-    i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_xy(&bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
+    i2c_OLED_send_string(&bus, lineBuffer);
 
     tfp_sprintf(lineBuffer, "Dt: %d", gpsData.lastMessage - gpsData.lastLastMessage);
     padHalfLineBuffer();
-    i2c_OLED_set_line(rowIndex);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_line(&bus, rowIndex);
+    i2c_OLED_send_string(&bus, lineBuffer);
 
     tfp_sprintf(lineBuffer, "TOs: %d", gpsData.timeouts);
     padHalfLineBuffer();
-    i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_xy(&bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
+    i2c_OLED_send_string(&bus, lineBuffer);
 
     strncpy(lineBuffer, gpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT);
     padHalfLineBuffer();
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_line(&bus, rowIndex++);
+    i2c_OLED_send_string(&bus, lineBuffer);
 
 #ifdef GPS_PH_DEBUG
     tfp_sprintf(lineBuffer, "Angles: P:%d R:%d", GPS_angle[PITCH], GPS_angle[ROLL]);
     padLineBuffer();
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_line(&bus, rowIndex++);
+    i2c_OLED_send_string(&bus, lineBuffer);
 #endif
 
 #if 0
     tfp_sprintf(lineBuffer, "%d %d %d %d", debug[0], debug[1], debug[2], debug[3]);
     padLineBuffer();
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_line(&bus, rowIndex++);
+    i2c_OLED_send_string(&bus, lineBuffer);
 #endif
 }
 #endif
@@ -433,11 +445,11 @@ void showBatteryPage(void)
     if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) {
         tfp_sprintf(lineBuffer, "Volts: %d.%1d Cells: %d", getBatteryVoltage() / 10, getBatteryVoltage() % 10, getBatteryCellCount());
         padLineBuffer();
-        i2c_OLED_set_line(rowIndex++);
-        i2c_OLED_send_string(lineBuffer);
+        i2c_OLED_set_line(&bus, rowIndex++);
+        i2c_OLED_send_string(&bus, lineBuffer);
 
         uint8_t batteryPercentage = calculateBatteryPercentageRemaining();
-        i2c_OLED_set_line(rowIndex++);
+        i2c_OLED_set_line(&bus, rowIndex++);
         drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, batteryPercentage);
     }
 
@@ -446,11 +458,11 @@ void showBatteryPage(void)
         int32_t amperage = getAmperage();
         tfp_sprintf(lineBuffer, "Amps: %d.%2d mAh: %d", amperage / 100, amperage % 100, getMAhDrawn());
         padLineBuffer();
-        i2c_OLED_set_line(rowIndex++);
-        i2c_OLED_send_string(lineBuffer);
+        i2c_OLED_set_line(&bus, rowIndex++);
+        i2c_OLED_send_string(&bus, lineBuffer);
 
         uint8_t capacityPercentage = calculateBatteryPercentageRemaining();
-        i2c_OLED_set_line(rowIndex++);
+        i2c_OLED_set_line(&bus, rowIndex++);
         drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, capacityPercentage);
     }
 }
@@ -460,36 +472,36 @@ void showSensorsPage(void)
     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
     static const char *format = "%s %5d %5d %5d";
 
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string("        X     Y     Z");
+    i2c_OLED_set_line(&bus, rowIndex++);
+    i2c_OLED_send_string(&bus, "        X     Y     Z");
 
     if (sensors(SENSOR_ACC)) {
         tfp_sprintf(lineBuffer, format, "ACC", acc.accSmooth[X], acc.accSmooth[Y], acc.accSmooth[Z]);
         padLineBuffer();
-        i2c_OLED_set_line(rowIndex++);
-        i2c_OLED_send_string(lineBuffer);
+        i2c_OLED_set_line(&bus, rowIndex++);
+        i2c_OLED_send_string(&bus, lineBuffer);
     }
 
     if (sensors(SENSOR_GYRO)) {
         tfp_sprintf(lineBuffer, format, "GYR", lrintf(gyro.gyroADCf[X]), lrintf(gyro.gyroADCf[Y]), lrintf(gyro.gyroADCf[Z]));
         padLineBuffer();
-        i2c_OLED_set_line(rowIndex++);
-        i2c_OLED_send_string(lineBuffer);
+        i2c_OLED_set_line(&bus, rowIndex++);
+        i2c_OLED_send_string(&bus, lineBuffer);
     }
 
 #ifdef MAG
     if (sensors(SENSOR_MAG)) {
         tfp_sprintf(lineBuffer, format, "MAG", mag.magADC[X], mag.magADC[Y], mag.magADC[Z]);
         padLineBuffer();
-        i2c_OLED_set_line(rowIndex++);
-        i2c_OLED_send_string(lineBuffer);
+        i2c_OLED_set_line(&bus, rowIndex++);
+        i2c_OLED_send_string(&bus, lineBuffer);
     }
 #endif
 
     tfp_sprintf(lineBuffer, format, "I&H", attitude.values.roll, attitude.values.pitch, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
     padLineBuffer();
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_line(&bus, rowIndex++);
+    i2c_OLED_send_string(&bus, lineBuffer);
 
     /*
     uint8_t length;
@@ -502,8 +514,8 @@ void showSensorsPage(void)
     }
     ftoa(EstG.A[Y], lineBuffer + length);
     padLineBuffer();
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_line(&bus, rowIndex++);
+    i2c_OLED_send_string(&bus, lineBuffer);
 
     ftoa(EstG.A[Z], lineBuffer);
     length = strlen(lineBuffer);
@@ -513,8 +525,8 @@ void showSensorsPage(void)
     }
     ftoa(smallAngle, lineBuffer + length);
     padLineBuffer();
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
+    i2c_OLED_set_line(&bus, rowIndex++);
+    i2c_OLED_send_string(&bus, lineBuffer);
     */
 
 }
@@ -525,8 +537,8 @@ void showTasksPage(void)
     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
     static const char *format = "%2d%6d%5d%4d%4d";
 
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string("Task max  avg mx% av%");
+    i2c_OLED_set_line(&bus, rowIndex++);
+    i2c_OLED_send_string(&bus, "Task max  avg mx% av%");
     cfTaskInfo_t taskInfo;
     for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; ++taskId) {
         getTaskInfo(taskId, &taskInfo);
@@ -536,8 +548,8 @@ void showTasksPage(void)
             const int averageLoad = (taskInfo.averageExecutionTime * taskFrequency + 5000) / 10000;
             tfp_sprintf(lineBuffer, format, taskId, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, maxLoad, averageLoad);
             padLineBuffer();
-            i2c_OLED_set_line(rowIndex++);
-            i2c_OLED_send_string(lineBuffer);
+            i2c_OLED_set_line(&bus, rowIndex++);
+            i2c_OLED_send_string(&bus, lineBuffer);
             if (rowIndex > SCREEN_CHARACTER_ROW_COUNT) {
                 break;
             }
@@ -553,8 +565,8 @@ void showDebugPage(void)
     for (int rowIndex = 0; rowIndex < 4; rowIndex++) {
         tfp_sprintf(lineBuffer, "%d = %5d", rowIndex, debug[rowIndex]);
         padLineBuffer();
-        i2c_OLED_set_line(rowIndex + PAGE_TITLE_LINE_COUNT);
-        i2c_OLED_send_string(lineBuffer);
+        i2c_OLED_set_line(&bus, rowIndex + PAGE_TITLE_LINE_COUNT);
+        i2c_OLED_send_string(&bus, lineBuffer);
     }
 }
 #endif
@@ -664,11 +676,14 @@ void dashboardUpdate(timeUs_t currentTimeUs)
 
 void dashboardInit(void)
 {
+    bus.i2c.device = I2C_CFG_TO_DEV(dashboardConfig()->device);
+    bus.i2c.address = dashboardConfig()->address;
+
     delay(200);
     resetDisplay();
     delay(200);
 
-    displayPort = displayPortOledInit();
+    displayPort = displayPortOledInit(&bus);
 #if defined(CMS)
     if (dashboardPresent) {
         cmsDisplayPortRegister(displayPort);
diff --git a/src/main/io/dashboard.h b/src/main/io/dashboard.h
index e7cc81f1f..10339369e 100644
--- a/src/main/io/dashboard.h
+++ b/src/main/io/dashboard.h
@@ -16,9 +16,26 @@
  */
 
 #include "common/time.h"
+#include "config/parameter_group.h"
+#include "drivers/bus_i2c.h"
 
 #define ENABLE_DEBUG_DASHBOARD_PAGE
 
+#ifdef OLED_I2C_INSTANCE
+#define DASHBOARD_I2C_INSTANCE  OLED_I2C_INSTANCE
+#else
+#define DASHBOARD_I2C_INSTANCE  I2CDEV_1
+#endif
+
+#define DASHBOARD_I2C_ADDRESS   0x3C     // OLED at address 0x3C in 7bit
+
+typedef struct dashboardConfig_s {
+    I2CDevice device;
+    uint8_t   address;
+} dashboardConfig_t;
+
+PG_DECLARE(dashboardConfig_t, dashboardConfig);
+
 typedef enum {
     PAGE_WELCOME,
     PAGE_ARMED,
diff --git a/src/main/io/displayport_oled.c b/src/main/io/displayport_oled.c
index fc60fed25..09b8f6aa8 100644
--- a/src/main/io/displayport_oled.c
+++ b/src/main/io/displayport_oled.c
@@ -41,8 +41,7 @@ static int oledRelease(displayPort_t *displayPort)
 
 static int oledClearScreen(displayPort_t *displayPort)
 {
-    UNUSED(displayPort);
-    i2c_OLED_clear_display_quick();
+    i2c_OLED_clear_display_quick(displayPort->device);
     return 0;
 }
 
@@ -59,17 +58,15 @@ static int oledScreenSize(const displayPort_t *displayPort)
 
 static int oledWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
 {
-    UNUSED(displayPort);
-    i2c_OLED_set_xy(x, y);
-    i2c_OLED_send_string(s);
+    i2c_OLED_set_xy(displayPort->device, x, y);
+    i2c_OLED_send_string(displayPort->device, s);
     return 0;
 }
 
 static int oledWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c)
 {
-    UNUSED(displayPort);
-    i2c_OLED_set_xy(x, y);
-    i2c_OLED_send_char(c);
+    i2c_OLED_set_xy(displayPort->device, x, y);
+    i2c_OLED_send_char(displayPort->device, c);
     return 0;
 }
 
@@ -110,8 +107,9 @@ static const displayPortVTable_t oledVTable = {
     .txBytesFree = oledTxBytesFree
 };
 
-displayPort_t *displayPortOledInit(void)
+displayPort_t *displayPortOledInit(void *device)
 {
+    oledDisplayPort.device = device;
     displayInit(&oledDisplayPort, &oledVTable);
     oledDisplayPort.rows = SCREEN_CHARACTER_ROW_COUNT;
     oledDisplayPort.cols = SCREEN_CHARACTER_COLUMN_COUNT;
diff --git a/src/main/io/displayport_oled.h b/src/main/io/displayport_oled.h
index 4daa6de1c..469b79aeb 100644
--- a/src/main/io/displayport_oled.h
+++ b/src/main/io/displayport_oled.h
@@ -17,4 +17,4 @@
 
 #pragma once
 
-displayPort_t *displayPortOledInit(void);
+displayPort_t *displayPortOledInit(void *device);

From c00ffe0427fe4b8210222606d528dba4e41699ec Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Tue, 27 Jun 2017 07:12:23 +0100
Subject: [PATCH 10/15] Changed bus to be a pointer variable

---
 src/main/io/dashboard.c | 170 ++++++++++++++++++++--------------------
 1 file changed, 86 insertions(+), 84 deletions(-)

diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c
index a954abb9d..943589b56 100644
--- a/src/main/io/dashboard.c
+++ b/src/main/io/dashboard.c
@@ -84,7 +84,7 @@ PG_RESET_TEMPLATE(dashboardConfig_t, dashboardConfig,
 #define DISPLAY_UPDATE_FREQUENCY (MICROSECONDS_IN_A_SECOND / 5)
 #define PAGE_CYCLE_FREQUENCY (MICROSECONDS_IN_A_SECOND * 5)
 
-static busDevice_t bus;
+static busDevice_t *bus;
 
 static uint32_t nextDisplayUpdateAt = 0;
 static bool dashboardPresent = false;
@@ -131,12 +131,12 @@ static pageState_t pageState;
 
 void resetDisplay(void)
 {
-    dashboardPresent = ug2864hsweg01InitI2C(&bus);
+    dashboardPresent = ug2864hsweg01InitI2C(bus);
 }
 
 void LCDprint(uint8_t i)
 {
-   i2c_OLED_send_char(&bus, i);
+   i2c_OLED_send_char(bus, i);
 }
 
 void padLineBuffer(void)
@@ -182,8 +182,8 @@ void fillScreenWithCharacters()
 {
     for (uint8_t row = 0; row < SCREEN_CHARACTER_ROW_COUNT; row++) {
         for (uint8_t column = 0; column < SCREEN_CHARACTER_COLUMN_COUNT; column++) {
-            i2c_OLED_set_xy(&bus, column, row);
-            i2c_OLED_send_char(&bus, 'A' + column);
+            i2c_OLED_set_xy(bus, column, row);
+            i2c_OLED_send_char(bus, 'A' + column);
         }
     }
 }
@@ -193,22 +193,22 @@ void fillScreenWithCharacters()
 void updateTicker(void)
 {
     static uint8_t tickerIndex = 0;
-    i2c_OLED_set_xy(&bus, SCREEN_CHARACTER_COLUMN_COUNT - 1, 0);
-    i2c_OLED_send_char(&bus, tickerCharacters[tickerIndex]);
+    i2c_OLED_set_xy(bus, SCREEN_CHARACTER_COLUMN_COUNT - 1, 0);
+    i2c_OLED_send_char(bus, tickerCharacters[tickerIndex]);
     tickerIndex++;
     tickerIndex = tickerIndex % TICKER_CHARACTER_COUNT;
 }
 
 void updateRxStatus(void)
 {
-    i2c_OLED_set_xy(&bus, SCREEN_CHARACTER_COLUMN_COUNT - 2, 0);
+    i2c_OLED_set_xy(bus, SCREEN_CHARACTER_COLUMN_COUNT - 2, 0);
     char rxStatus = '!';
     if (rxIsReceivingSignal()) {
         rxStatus = 'r';
     } if (rxAreFlightChannelsValid()) {
         rxStatus = 'R';
     }
-    i2c_OLED_send_char(&bus, rxStatus);
+    i2c_OLED_send_char(bus, rxStatus);
 }
 
 void updateFailsafeStatus(void)
@@ -234,19 +234,19 @@ void updateFailsafeStatus(void)
             failsafeIndicator = 'r';
             break;
     }
-    i2c_OLED_set_xy(&bus, SCREEN_CHARACTER_COLUMN_COUNT - 3, 0);
-    i2c_OLED_send_char(&bus, failsafeIndicator);
+    i2c_OLED_set_xy(bus, SCREEN_CHARACTER_COLUMN_COUNT - 3, 0);
+    i2c_OLED_send_char(bus, failsafeIndicator);
 }
 
 void showTitle()
 {
-    i2c_OLED_set_line(&bus, 0);
-    i2c_OLED_send_string(&bus, pageState.page->title);
+    i2c_OLED_set_line(bus, 0);
+    i2c_OLED_send_string(bus, pageState.page->title);
 }
 
 void handlePageChange(void)
 {
-    i2c_OLED_clear_display_quick(&bus);
+    i2c_OLED_clear_display_quick(bus);
     showTitle();
 }
 
@@ -265,7 +265,7 @@ void showRxPage(void)
 {
 
     for (uint8_t channelIndex = 0; channelIndex < rxRuntimeConfig.channelCount && channelIndex < RX_CHANNELS_PER_PAGE_COUNT; channelIndex += 2) {
-        i2c_OLED_set_line(&bus, (channelIndex / 2) + PAGE_TITLE_LINE_COUNT);
+        i2c_OLED_set_line(bus, (channelIndex / 2) + PAGE_TITLE_LINE_COUNT);
 
         drawRxChannel(channelIndex, HALF_SCREEN_CHARACTER_COLUMN_COUNT);
 
@@ -286,11 +286,11 @@ void showWelcomePage(void)
     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 
     tfp_sprintf(lineBuffer, "v%s (%s)", FC_VERSION_STRING, shortGitRevision);
-    i2c_OLED_set_line(&bus, rowIndex++);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_line(bus, rowIndex++);
+    i2c_OLED_send_string(bus, lineBuffer);
 
-    i2c_OLED_set_line(&bus, rowIndex++);
-    i2c_OLED_send_string(&bus, targetName);
+    i2c_OLED_set_line(bus, rowIndex++);
+    i2c_OLED_send_string(bus, targetName);
 }
 
 void showArmedPage(void)
@@ -302,8 +302,8 @@ void showProfilePage(void)
     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 
     tfp_sprintf(lineBuffer, "Profile: %d", getCurrentPidProfileIndex());
-    i2c_OLED_set_line(&bus, rowIndex++);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_line(bus, rowIndex++);
+    i2c_OLED_send_string(bus, lineBuffer);
 
     static const char* const axisTitles[3] = {"ROL", "PIT", "YAW"};
     const pidProfile_t *pidProfile = currentPidProfile;
@@ -315,14 +315,14 @@ void showProfilePage(void)
             pidProfile->pid[axis].D
         );
         padLineBuffer();
-        i2c_OLED_set_line(&bus, rowIndex++);
-        i2c_OLED_send_string(&bus, lineBuffer);
+        i2c_OLED_set_line(bus, rowIndex++);
+        i2c_OLED_send_string(bus, lineBuffer);
     }
 
     const uint8_t currentRateProfileIndex = getCurrentControlRateProfileIndex();
     tfp_sprintf(lineBuffer, "Rate profile: %d", currentRateProfileIndex);
-    i2c_OLED_set_line(&bus, rowIndex++);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_line(bus, rowIndex++);
+    i2c_OLED_send_string(bus, lineBuffer);
 
     const controlRateConfig_t *controlRateConfig = controlRateProfiles(currentRateProfileIndex);
     tfp_sprintf(lineBuffer, "RCE: %d, RCR: %d",
@@ -330,8 +330,8 @@ void showProfilePage(void)
         controlRateConfig->rcRate8
     );
     padLineBuffer();
-    i2c_OLED_set_line(&bus, rowIndex++);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_line(bus, rowIndex++);
+    i2c_OLED_send_string(bus, lineBuffer);
 
     tfp_sprintf(lineBuffer, "RR:%d PR:%d YR:%d",
         controlRateConfig->rates[FD_ROLL],
@@ -339,8 +339,8 @@ void showProfilePage(void)
         controlRateConfig->rates[FD_YAW]
     );
     padLineBuffer();
-    i2c_OLED_set_line(&bus, rowIndex++);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_line(bus, rowIndex++);
+    i2c_OLED_send_string(bus, lineBuffer);
 }
 #define SATELLITE_COUNT (sizeof(GPS_svinfo_cno) / sizeof(GPS_svinfo_cno[0]))
 #define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2)
@@ -363,77 +363,77 @@ void showGpsPage() {
         gpsTicker = gpsTicker % TICKER_CHARACTER_COUNT;
     }
 
-    i2c_OLED_set_xy(&bus, 0, rowIndex);
-    i2c_OLED_send_char(&bus, tickerCharacters[gpsTicker]);
+    i2c_OLED_set_xy(bus, 0, rowIndex);
+    i2c_OLED_send_char(bus, tickerCharacters[gpsTicker]);
 
-    i2c_OLED_set_xy(&bus, MAX(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
+    i2c_OLED_set_xy(bus, MAX(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
 
     uint32_t index;
     for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; index++) {
         uint8_t bargraphOffset = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1);
         bargraphOffset = MIN(bargraphOffset, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1);
-        i2c_OLED_send_char(&bus, VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset);
+        i2c_OLED_send_char(bus, VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset);
     }
 
 
     char fixChar = STATE(GPS_FIX) ? 'Y' : 'N';
     tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", gpsSol.numSat, fixChar);
     padLineBuffer();
-    i2c_OLED_set_line(&bus, rowIndex++);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_line(bus, rowIndex++);
+    i2c_OLED_send_string(bus, lineBuffer);
 
     tfp_sprintf(lineBuffer, "La/Lo: %d/%d", gpsSol.llh.lat / GPS_DEGREES_DIVIDER, gpsSol.llh.lon / GPS_DEGREES_DIVIDER);
     padLineBuffer();
-    i2c_OLED_set_line(&bus, rowIndex++);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_line(bus, rowIndex++);
+    i2c_OLED_send_string(bus, lineBuffer);
 
     tfp_sprintf(lineBuffer, "Spd: %d", gpsSol.groundSpeed);
     padHalfLineBuffer();
-    i2c_OLED_set_line(&bus, rowIndex);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_line(bus, rowIndex);
+    i2c_OLED_send_string(bus, lineBuffer);
 
     tfp_sprintf(lineBuffer, "GC: %d", gpsSol.groundCourse);
     padHalfLineBuffer();
-    i2c_OLED_set_xy(&bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_xy(bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
+    i2c_OLED_send_string(bus, lineBuffer);
 
     tfp_sprintf(lineBuffer, "RX: %d", GPS_packetCount);
     padHalfLineBuffer();
-    i2c_OLED_set_line(&bus, rowIndex);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_line(bus, rowIndex);
+    i2c_OLED_send_string(bus, lineBuffer);
 
     tfp_sprintf(lineBuffer, "ERRs: %d", gpsData.errors, gpsData.timeouts);
     padHalfLineBuffer();
-    i2c_OLED_set_xy(&bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_xy(bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
+    i2c_OLED_send_string(bus, lineBuffer);
 
     tfp_sprintf(lineBuffer, "Dt: %d", gpsData.lastMessage - gpsData.lastLastMessage);
     padHalfLineBuffer();
-    i2c_OLED_set_line(&bus, rowIndex);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_line(bus, rowIndex);
+    i2c_OLED_send_string(bus, lineBuffer);
 
     tfp_sprintf(lineBuffer, "TOs: %d", gpsData.timeouts);
     padHalfLineBuffer();
-    i2c_OLED_set_xy(&bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_xy(bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
+    i2c_OLED_send_string(bus, lineBuffer);
 
     strncpy(lineBuffer, gpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT);
     padHalfLineBuffer();
-    i2c_OLED_set_line(&bus, rowIndex++);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_line(bus, rowIndex++);
+    i2c_OLED_send_string(bus, lineBuffer);
 
 #ifdef GPS_PH_DEBUG
     tfp_sprintf(lineBuffer, "Angles: P:%d R:%d", GPS_angle[PITCH], GPS_angle[ROLL]);
     padLineBuffer();
-    i2c_OLED_set_line(&bus, rowIndex++);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_line(bus, rowIndex++);
+    i2c_OLED_send_string(bus, lineBuffer);
 #endif
 
 #if 0
     tfp_sprintf(lineBuffer, "%d %d %d %d", debug[0], debug[1], debug[2], debug[3]);
     padLineBuffer();
-    i2c_OLED_set_line(&bus, rowIndex++);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_line(bus, rowIndex++);
+    i2c_OLED_send_string(bus, lineBuffer);
 #endif
 }
 #endif
@@ -445,11 +445,11 @@ void showBatteryPage(void)
     if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) {
         tfp_sprintf(lineBuffer, "Volts: %d.%1d Cells: %d", getBatteryVoltage() / 10, getBatteryVoltage() % 10, getBatteryCellCount());
         padLineBuffer();
-        i2c_OLED_set_line(&bus, rowIndex++);
-        i2c_OLED_send_string(&bus, lineBuffer);
+        i2c_OLED_set_line(bus, rowIndex++);
+        i2c_OLED_send_string(bus, lineBuffer);
 
         uint8_t batteryPercentage = calculateBatteryPercentageRemaining();
-        i2c_OLED_set_line(&bus, rowIndex++);
+        i2c_OLED_set_line(bus, rowIndex++);
         drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, batteryPercentage);
     }
 
@@ -458,11 +458,11 @@ void showBatteryPage(void)
         int32_t amperage = getAmperage();
         tfp_sprintf(lineBuffer, "Amps: %d.%2d mAh: %d", amperage / 100, amperage % 100, getMAhDrawn());
         padLineBuffer();
-        i2c_OLED_set_line(&bus, rowIndex++);
-        i2c_OLED_send_string(&bus, lineBuffer);
+        i2c_OLED_set_line(bus, rowIndex++);
+        i2c_OLED_send_string(bus, lineBuffer);
 
         uint8_t capacityPercentage = calculateBatteryPercentageRemaining();
-        i2c_OLED_set_line(&bus, rowIndex++);
+        i2c_OLED_set_line(bus, rowIndex++);
         drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, capacityPercentage);
     }
 }
@@ -472,36 +472,36 @@ void showSensorsPage(void)
     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
     static const char *format = "%s %5d %5d %5d";
 
-    i2c_OLED_set_line(&bus, rowIndex++);
-    i2c_OLED_send_string(&bus, "        X     Y     Z");
+    i2c_OLED_set_line(bus, rowIndex++);
+    i2c_OLED_send_string(bus, "        X     Y     Z");
 
     if (sensors(SENSOR_ACC)) {
         tfp_sprintf(lineBuffer, format, "ACC", acc.accSmooth[X], acc.accSmooth[Y], acc.accSmooth[Z]);
         padLineBuffer();
-        i2c_OLED_set_line(&bus, rowIndex++);
-        i2c_OLED_send_string(&bus, lineBuffer);
+        i2c_OLED_set_line(bus, rowIndex++);
+        i2c_OLED_send_string(bus, lineBuffer);
     }
 
     if (sensors(SENSOR_GYRO)) {
         tfp_sprintf(lineBuffer, format, "GYR", lrintf(gyro.gyroADCf[X]), lrintf(gyro.gyroADCf[Y]), lrintf(gyro.gyroADCf[Z]));
         padLineBuffer();
-        i2c_OLED_set_line(&bus, rowIndex++);
-        i2c_OLED_send_string(&bus, lineBuffer);
+        i2c_OLED_set_line(bus, rowIndex++);
+        i2c_OLED_send_string(bus, lineBuffer);
     }
 
 #ifdef MAG
     if (sensors(SENSOR_MAG)) {
         tfp_sprintf(lineBuffer, format, "MAG", mag.magADC[X], mag.magADC[Y], mag.magADC[Z]);
         padLineBuffer();
-        i2c_OLED_set_line(&bus, rowIndex++);
-        i2c_OLED_send_string(&bus, lineBuffer);
+        i2c_OLED_set_line(bus, rowIndex++);
+        i2c_OLED_send_string(bus, lineBuffer);
     }
 #endif
 
     tfp_sprintf(lineBuffer, format, "I&H", attitude.values.roll, attitude.values.pitch, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
     padLineBuffer();
-    i2c_OLED_set_line(&bus, rowIndex++);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_line(bus, rowIndex++);
+    i2c_OLED_send_string(bus, lineBuffer);
 
     /*
     uint8_t length;
@@ -514,8 +514,8 @@ void showSensorsPage(void)
     }
     ftoa(EstG.A[Y], lineBuffer + length);
     padLineBuffer();
-    i2c_OLED_set_line(&bus, rowIndex++);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_line(bus, rowIndex++);
+    i2c_OLED_send_string(bus, lineBuffer);
 
     ftoa(EstG.A[Z], lineBuffer);
     length = strlen(lineBuffer);
@@ -525,8 +525,8 @@ void showSensorsPage(void)
     }
     ftoa(smallAngle, lineBuffer + length);
     padLineBuffer();
-    i2c_OLED_set_line(&bus, rowIndex++);
-    i2c_OLED_send_string(&bus, lineBuffer);
+    i2c_OLED_set_line(bus, rowIndex++);
+    i2c_OLED_send_string(bus, lineBuffer);
     */
 
 }
@@ -537,8 +537,8 @@ void showTasksPage(void)
     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
     static const char *format = "%2d%6d%5d%4d%4d";
 
-    i2c_OLED_set_line(&bus, rowIndex++);
-    i2c_OLED_send_string(&bus, "Task max  avg mx% av%");
+    i2c_OLED_set_line(bus, rowIndex++);
+    i2c_OLED_send_string(bus, "Task max  avg mx% av%");
     cfTaskInfo_t taskInfo;
     for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; ++taskId) {
         getTaskInfo(taskId, &taskInfo);
@@ -548,8 +548,8 @@ void showTasksPage(void)
             const int averageLoad = (taskInfo.averageExecutionTime * taskFrequency + 5000) / 10000;
             tfp_sprintf(lineBuffer, format, taskId, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, maxLoad, averageLoad);
             padLineBuffer();
-            i2c_OLED_set_line(&bus, rowIndex++);
-            i2c_OLED_send_string(&bus, lineBuffer);
+            i2c_OLED_set_line(bus, rowIndex++);
+            i2c_OLED_send_string(bus, lineBuffer);
             if (rowIndex > SCREEN_CHARACTER_ROW_COUNT) {
                 break;
             }
@@ -565,8 +565,8 @@ void showDebugPage(void)
     for (int rowIndex = 0; rowIndex < 4; rowIndex++) {
         tfp_sprintf(lineBuffer, "%d = %5d", rowIndex, debug[rowIndex]);
         padLineBuffer();
-        i2c_OLED_set_line(&bus, rowIndex + PAGE_TITLE_LINE_COUNT);
-        i2c_OLED_send_string(&bus, lineBuffer);
+        i2c_OLED_set_line(bus, rowIndex + PAGE_TITLE_LINE_COUNT);
+        i2c_OLED_send_string(bus, lineBuffer);
     }
 }
 #endif
@@ -676,14 +676,16 @@ void dashboardUpdate(timeUs_t currentTimeUs)
 
 void dashboardInit(void)
 {
-    bus.i2c.device = I2C_CFG_TO_DEV(dashboardConfig()->device);
-    bus.i2c.address = dashboardConfig()->address;
+    static busDevice_t dashBoardBus;
+    dashBoardBus.i2c.device = I2C_CFG_TO_DEV(dashboardConfig()->device);
+    dashBoardBus.i2c.address = dashboardConfig()->address;
+    bus = &dashBoardBus;
 
     delay(200);
     resetDisplay();
     delay(200);
 
-    displayPort = displayPortOledInit(&bus);
+    displayPort = displayPortOledInit(bus);
 #if defined(CMS)
     if (dashboardPresent) {
         cmsDisplayPortRegister(displayPort);

From 2d45a2e8c3d16e0bef1f5a25dcaff71c4d523feb Mon Sep 17 00:00:00 2001
From: mikeller <github@ike.ch>
Date: Wed, 28 Jun 2017 23:40:39 +1200
Subject: [PATCH 11/15] Enabled LED_STRIP on CC3D, fixed includes.

---
 src/main/target/CC3D/target.c | 1 +
 src/main/target/CC3D/target.h | 3 ++-
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c
index 53fa9c58b..56951fc47 100644
--- a/src/main/target/CC3D/target.c
+++ b/src/main/target/CC3D/target.c
@@ -22,6 +22,7 @@
 
 #include "drivers/timer.h"
 #include "drivers/timer_def.h"
+#include "drivers/dma.h"
 
 const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
 
diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h
index 23e3af7c4..fba7f6b69 100644
--- a/src/main/target/CC3D/target.h
+++ b/src/main/target/CC3D/target.h
@@ -114,9 +114,10 @@
 #undef USE_SERIALRX_SUMD       // Graupner Hott protocol
 #undef USE_SERIALRX_SUMH       // Graupner legacy protocol
 #undef USE_SERIALRX_XBUS       // JR
+#undef LED_STRIP
 #endif
 
-#undef LED_STRIP
+//#undef LED_STRIP
 #define DEFAULT_RX_FEATURE      FEATURE_RX_PPM
 
 // IO - from schematics

From 3d7145b0888faed5966337a2710544f544a3fe0a Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 28 Jun 2017 14:40:42 +0100
Subject: [PATCH 12/15] Fix as per @jflyper's comments

---
 src/main/drivers/bus_i2c.h | 3 ++-
 src/main/io/dashboard.c    | 2 +-
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h
index 0f9f0c538..d02f573e3 100644
--- a/src/main/drivers/bus_i2c.h
+++ b/src/main/drivers/bus_i2c.h
@@ -45,8 +45,9 @@ typedef enum I2CDevice {
 #define I2CDEV_COUNT 4
 #endif
 
-// Macro to convert CLI bus number to I2CDevice.
+// Macros to convert between CLI bus number and I2CDevice.
 #define I2C_CFG_TO_DEV(x)   ((x) - 1)
+#define I2C_DEV_TO_CFG(x)   ((x) + 1)
 
 // I2C device address range in 8-bit address mode
 #define I2C_ADDR8_MIN       8
diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c
index 943589b56..c3ea1b5e2 100644
--- a/src/main/io/dashboard.c
+++ b/src/main/io/dashboard.c
@@ -75,7 +75,7 @@
 PG_REGISTER_WITH_RESET_TEMPLATE(dashboardConfig_t, dashboardConfig, PG_DASHBOARD_CONFIG, 0);
 
 PG_RESET_TEMPLATE(dashboardConfig_t, dashboardConfig,
-    .device = DASHBOARD_I2C_INSTANCE,
+    .device = I2C_DEV_TO_CFG(DASHBOARD_I2C_INSTANCE),
     .address = DASHBOARD_I2C_ADDRESS,
 );
 

From 0d2239ddd63adda31956a1d49249f4ae79163e6a Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 28 Jun 2017 21:00:02 +0100
Subject: [PATCH 13/15] Dashboard tidy

---
 src/main/io/dashboard.c | 88 ++++++++++++++++++++---------------------
 1 file changed, 44 insertions(+), 44 deletions(-)

diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c
index c3ea1b5e2..df36b6b2a 100644
--- a/src/main/io/dashboard.c
+++ b/src/main/io/dashboard.c
@@ -129,7 +129,7 @@ typedef struct pageState_s {
 
 static pageState_t pageState;
 
-void resetDisplay(void)
+static void resetDisplay(void)
 {
     dashboardPresent = ug2864hsweg01InitI2C(bus);
 }
@@ -139,7 +139,7 @@ void LCDprint(uint8_t i)
    i2c_OLED_send_char(bus, i);
 }
 
-void padLineBuffer(void)
+static void padLineBuffer(void)
 {
     uint8_t length = strlen(lineBuffer);
     while (length < sizeof(lineBuffer) - 1) {
@@ -148,7 +148,8 @@ void padLineBuffer(void)
     lineBuffer[length] = 0;
 }
 
-void padHalfLineBuffer(void)
+#ifdef GPS
+static void padHalfLineBuffer(void)
 {
     uint8_t halfLineIndex = sizeof(lineBuffer) / 2;
     uint8_t length = strlen(lineBuffer);
@@ -157,9 +158,11 @@ void padHalfLineBuffer(void)
     }
     lineBuffer[length] = 0;
 }
+#endif
 
 // LCDbar(n,v) : draw a bar graph - n number of chars for width, v value in % to display
-void drawHorizonalPercentageBar(uint8_t width,uint8_t percent) {
+static void drawHorizonalPercentageBar(uint8_t width,uint8_t percent)
+{
     uint8_t i, j;
 
     if (percent > 100)
@@ -178,7 +181,7 @@ void drawHorizonalPercentageBar(uint8_t width,uint8_t percent) {
 }
 
 #if 0
-void fillScreenWithCharacters()
+static void fillScreenWithCharacters()
 {
     for (uint8_t row = 0; row < SCREEN_CHARACTER_ROW_COUNT; row++) {
         for (uint8_t column = 0; column < SCREEN_CHARACTER_COLUMN_COUNT; column++) {
@@ -190,7 +193,7 @@ void fillScreenWithCharacters()
 #endif
 
 
-void updateTicker(void)
+static void updateTicker(void)
 {
     static uint8_t tickerIndex = 0;
     i2c_OLED_set_xy(bus, SCREEN_CHARACTER_COLUMN_COUNT - 1, 0);
@@ -199,7 +202,7 @@ void updateTicker(void)
     tickerIndex = tickerIndex % TICKER_CHARACTER_COUNT;
 }
 
-void updateRxStatus(void)
+static void updateRxStatus(void)
 {
     i2c_OLED_set_xy(bus, SCREEN_CHARACTER_COLUMN_COUNT - 2, 0);
     char rxStatus = '!';
@@ -211,60 +214,57 @@ void updateRxStatus(void)
     i2c_OLED_send_char(bus, rxStatus);
 }
 
-void updateFailsafeStatus(void)
+static void updateFailsafeStatus(void)
 {
     char failsafeIndicator = '?';
     switch (failsafePhase()) {
-        case FAILSAFE_IDLE:
-            failsafeIndicator = '-';
-            break;
-        case FAILSAFE_RX_LOSS_DETECTED:
-            failsafeIndicator = 'R';
-            break;
-        case FAILSAFE_LANDING:
-            failsafeIndicator = 'l';
-            break;
-        case FAILSAFE_LANDED:
-            failsafeIndicator = 'L';
-            break;
-        case FAILSAFE_RX_LOSS_MONITORING:
-            failsafeIndicator = 'M';
-            break;
-        case FAILSAFE_RX_LOSS_RECOVERED:
-            failsafeIndicator = 'r';
-            break;
+    case FAILSAFE_IDLE:
+        failsafeIndicator = '-';
+        break;
+    case FAILSAFE_RX_LOSS_DETECTED:
+        failsafeIndicator = 'R';
+        break;
+    case FAILSAFE_LANDING:
+        failsafeIndicator = 'l';
+        break;
+    case FAILSAFE_LANDED:
+        failsafeIndicator = 'L';
+        break;
+    case FAILSAFE_RX_LOSS_MONITORING:
+        failsafeIndicator = 'M';
+        break;
+    case FAILSAFE_RX_LOSS_RECOVERED:
+        failsafeIndicator = 'r';
+        break;
     }
     i2c_OLED_set_xy(bus, SCREEN_CHARACTER_COLUMN_COUNT - 3, 0);
     i2c_OLED_send_char(bus, failsafeIndicator);
 }
 
-void showTitle()
+static void showTitle()
 {
     i2c_OLED_set_line(bus, 0);
     i2c_OLED_send_string(bus, pageState.page->title);
 }
 
-void handlePageChange(void)
+static void handlePageChange(void)
 {
     i2c_OLED_clear_display_quick(bus);
     showTitle();
 }
 
-void drawRxChannel(uint8_t channelIndex, uint8_t width)
+static void drawRxChannel(uint8_t channelIndex, uint8_t width)
 {
-    uint32_t percentage;
-
     LCDprint(rcChannelLetters[channelIndex]);
 
-    percentage = (constrain(rcData[channelIndex], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
+    const uint32_t percentage = (constrain(rcData[channelIndex], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
     drawHorizonalPercentageBar(width - 1, percentage);
 }
 
 #define RX_CHANNELS_PER_PAGE_COUNT 14
-void showRxPage(void)
+static void showRxPage(void)
 {
-
-    for (uint8_t channelIndex = 0; channelIndex < rxRuntimeConfig.channelCount && channelIndex < RX_CHANNELS_PER_PAGE_COUNT; channelIndex += 2) {
+    for (int channelIndex = 0; channelIndex < rxRuntimeConfig.channelCount && channelIndex < RX_CHANNELS_PER_PAGE_COUNT; channelIndex += 2) {
         i2c_OLED_set_line(bus, (channelIndex / 2) + PAGE_TITLE_LINE_COUNT);
 
         drawRxChannel(channelIndex, HALF_SCREEN_CHARACTER_COLUMN_COUNT);
@@ -281,7 +281,7 @@ void showRxPage(void)
     }
 }
 
-void showWelcomePage(void)
+static void showWelcomePage(void)
 {
     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 
@@ -293,11 +293,11 @@ void showWelcomePage(void)
     i2c_OLED_send_string(bus, targetName);
 }
 
-void showArmedPage(void)
+static void showArmedPage(void)
 {
 }
 
-void showProfilePage(void)
+static void showProfilePage(void)
 {
     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 
@@ -346,8 +346,8 @@ void showProfilePage(void)
 #define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2)
 
 #ifdef GPS
-void showGpsPage() {
-
+static void showGpsPage()
+{
     if (!feature(FEATURE_GPS)) {
         pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
         return;
@@ -438,7 +438,7 @@ void showGpsPage() {
 }
 #endif
 
-void showBatteryPage(void)
+static void showBatteryPage(void)
 {
     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 
@@ -467,7 +467,7 @@ void showBatteryPage(void)
     }
 }
 
-void showSensorsPage(void)
+static void showSensorsPage(void)
 {
     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
     static const char *format = "%s %5d %5d %5d";
@@ -532,7 +532,7 @@ void showSensorsPage(void)
 }
 
 #ifndef SKIP_TASK_STATISTICS
-void showTasksPage(void)
+static void showTasksPage(void)
 {
     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
     static const char *format = "%2d%6d%5d%4d%4d";
@@ -560,7 +560,7 @@ void showTasksPage(void)
 
 #ifdef ENABLE_DEBUG_DASHBOARD_PAGE
 
-void showDebugPage(void)
+static void showDebugPage(void)
 {
     for (int rowIndex = 0; rowIndex < 4; rowIndex++) {
         tfp_sprintf(lineBuffer, "%d = %5d", rowIndex, debug[rowIndex]);

From 1c7f574a55124d3533cc2b23c57e021c345392f7 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 28 Jun 2017 21:21:29 +0100
Subject: [PATCH 14/15] Displayport tidy

---
 src/main/drivers/display.c        | 13 ++++++++++++-
 src/main/drivers/display.h        |  5 ++++-
 src/main/io/displayport_max7456.c |  4 ++--
 src/main/io/displayport_msp.c     |  6 +++---
 src/main/io/displayport_oled.c    |  4 ++--
 5 files changed, 23 insertions(+), 9 deletions(-)

diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c
index 8d55c102a..978663fd6 100644
--- a/src/main/drivers/display.c
+++ b/src/main/drivers/display.c
@@ -17,6 +17,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <string.h>
 
 #include "platform.h"
 
@@ -66,13 +67,23 @@ bool displayIsGrabbed(const displayPort_t *instance)
     return (instance && instance->grabCount > 0);
 }
 
+void displaySetXY(displayPort_t *instance, uint8_t x, uint8_t y)
+{
+    instance->posX = x;
+    instance->posY = y;
+}
+
 int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, const char *s)
 {
-    return instance->vTable->write(instance, x, y, s);
+    instance->posX = x + strlen(s);
+    instance->posY = y;
+    return instance->vTable->writeString(instance, x, y, s);
 }
 
 int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c)
 {
+    instance->posX = x + 1;
+    instance->posY = y;
     return instance->vTable->writeChar(instance, x, y, c);
 }
 
diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h
index 6caf1767d..f8ae22819 100644
--- a/src/main/drivers/display.h
+++ b/src/main/drivers/display.h
@@ -23,6 +23,8 @@ typedef struct displayPort_s {
     void *device;
     uint8_t rows;
     uint8_t cols;
+    uint8_t posX;
+    uint8_t posY;
 
     // CMS state
     bool cleared;
@@ -36,7 +38,7 @@ typedef struct displayPortVTable_s {
     int (*clearScreen)(displayPort_t *displayPort);
     int (*drawScreen)(displayPort_t *displayPort);
     int (*screenSize)(const displayPort_t *displayPort);
-    int (*write)(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *text);
+    int (*writeString)(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *text);
     int (*writeChar)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c);
     bool (*isTransferInProgress)(const displayPort_t *displayPort);
     int (*heartbeat)(displayPort_t *displayPort);
@@ -59,6 +61,7 @@ bool displayIsGrabbed(const displayPort_t *instance);
 void displayClearScreen(displayPort_t *instance);
 void displayDrawScreen(displayPort_t *instance);
 int displayScreenSize(const displayPort_t *instance);
+void displaySetXY(displayPort_t *instance, uint8_t x, uint8_t y);
 int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, const char *s);
 int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c);
 bool displayIsTransferInProgress(const displayPort_t *instance);
diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c
index 5297b77ed..bd5afd4e5 100644
--- a/src/main/io/displayport_max7456.c
+++ b/src/main/io/displayport_max7456.c
@@ -95,7 +95,7 @@ static int screenSize(const displayPort_t *displayPort)
     return maxScreenSize;
 }
 
-static int write(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
+static int writeString(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
 {
     UNUSED(displayPort);
     max7456Write(x, y, s);
@@ -143,7 +143,7 @@ static const displayPortVTable_t max7456VTable = {
     .clearScreen = clearScreen,
     .drawScreen = drawScreen,
     .screenSize = screenSize,
-    .write = write,
+    .writeString = writeString,
     .writeChar = writeChar,
     .isTransferInProgress = isTransferInProgress,
     .heartbeat = heartbeat,
diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c
index 6753a3f23..2631dcbf1 100644
--- a/src/main/io/displayport_msp.c
+++ b/src/main/io/displayport_msp.c
@@ -100,7 +100,7 @@ static int screenSize(const displayPort_t *displayPort)
     return displayPort->rows * displayPort->cols;
 }
 
-static int write(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string)
+static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string)
 {
 #define MSP_OSD_MAX_STRING_LENGTH 30 // FIXME move this
     uint8_t buf[MSP_OSD_MAX_STRING_LENGTH + 4];
@@ -125,7 +125,7 @@ static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint8
 
     buf[0] = c;
     buf[1] = 0;
-    return write(displayPort, col, row, buf); //!!TODO - check if there is a direct MSP command to do this
+    return writeString(displayPort, col, row, buf); //!!TODO - check if there is a direct MSP command to do this
 }
 
 static bool isTransferInProgress(const displayPort_t *displayPort)
@@ -152,7 +152,7 @@ static const displayPortVTable_t mspDisplayPortVTable = {
     .clearScreen = clearScreen,
     .drawScreen = drawScreen,
     .screenSize = screenSize,
-    .write = write,
+    .writeString = writeString,
     .writeChar = writeChar,
     .isTransferInProgress = isTransferInProgress,
     .heartbeat = heartbeat,
diff --git a/src/main/io/displayport_oled.c b/src/main/io/displayport_oled.c
index 09b8f6aa8..b6b5d7e77 100644
--- a/src/main/io/displayport_oled.c
+++ b/src/main/io/displayport_oled.c
@@ -56,7 +56,7 @@ static int oledScreenSize(const displayPort_t *displayPort)
     return displayPort->rows * displayPort->cols;
 }
 
-static int oledWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
+static int oledWriteString(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
 {
     i2c_OLED_set_xy(displayPort->device, x, y);
     i2c_OLED_send_string(displayPort->device, s);
@@ -99,7 +99,7 @@ static const displayPortVTable_t oledVTable = {
     .clearScreen = oledClearScreen,
     .drawScreen = oledDrawScreen,
     .screenSize = oledScreenSize,
-    .write = oledWrite,
+    .writeString = oledWriteString,
     .writeChar = oledWriteChar,
     .isTransferInProgress = oledIsTransferInProgress,
     .heartbeat = oledHeartbeat,

From 8d4f838aacaae65351ac2d535c3ebeacfd56eb91 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 28 Jun 2017 21:36:33 +0100
Subject: [PATCH 15/15] Fixed test

---
 src/test/unit/unittest_displayport.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/test/unit/unittest_displayport.h b/src/test/unit/unittest_displayport.h
index ded02afe9..4e649923d 100644
--- a/src/test/unit/unittest_displayport.h
+++ b/src/test/unit/unittest_displayport.h
@@ -67,7 +67,7 @@ static int displayPortTestScreenSize(const displayPort_t *displayPort)
     return 0;
 }
 
-static int displayPortTestWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
+static int displayPortTestWriteString(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
 {
     UNUSED(displayPort);
     for (unsigned int i = 0; i < strlen(s); i++) {
@@ -112,7 +112,7 @@ static const displayPortVTable_t testDisplayPortVTable = {
     .clearScreen = displayPortTestClearScreen,
     .drawScreen = displayPortTestDrawScreen,
     .screenSize = displayPortTestScreenSize,
-    .write = displayPortTestWrite,
+    .writeString = displayPortTestWriteString,
     .writeChar = displayPortTestWriteChar,
     .isTransferInProgress = displayPortTestIsTransferInProgress,
     .heartbeat = displayPortTestHeartbeat,