diff --git a/sw/airborne/modules/display/max7456.c b/sw/airborne/modules/display/max7456.c
index 7604c10c7c..6ed2f392f5 100644
--- a/sw/airborne/modules/display/max7456.c
+++ b/sw/airborne/modules/display/max7456.c
@@ -28,6 +28,8 @@
 #include "std.h"
 //#include "stdio.h"
 
+#include "inter_mcu.h"
+
 #include "mcu_periph/sys_time.h"
 #include "mcu_periph/gpio.h"
 #include "mcu_periph/spi.h"
@@ -99,12 +101,12 @@ enum max7456_osd_status_codes {
   OSD_FINISHED,
 };
 
-enum osd_attributes{
-     BLINK = OSD_BLINK_CHAR,
-     INVERT = OSD_INVERT_PIXELS,
-     L_JUST = 0x00,
-     R_JUST = 0x01,
-     C_JUST = 0x02,
+enum osd_attributes {
+  BLINK = OSD_BLINK_CHAR,
+  INVERT = OSD_INVERT_PIXELS,
+  L_JUST = 0x00,
+  R_JUST = 0x01,
+  C_JUST = 0x02,
 
 };
 
@@ -167,7 +169,7 @@ static float home_direction(void)
 
   Home_Position.fx = WaypointY(WP_HOME);
   Home_Position.fy = WaypointX(WP_HOME);
-  Home_Position.fz = waypoints[WP_HOME].a;
+  Home_Position.fz = ground_alt;
 
   /* distance between plane and object */
   vSubtractVectors(&Home_PositionForPlane, Home_Position, svPlanePosition);
@@ -187,7 +189,7 @@ static float home_direction(void)
 
   /* DEFAULT ORIENTATION IS 0 = FRONT, 90 = RIGHT, 180 = BACK, -90 = LEFT
    *
-   * WHEN home_dir =  (float)(atan2(Home_PositionForPlane2.fy, (Home_PositionForPlane2.fx))); 
+   * WHEN home_dir =  (float)(atan2(Home_PositionForPlane2.fy, (Home_PositionForPlane2.fx)));
    *
    *   plane front
    *
@@ -236,27 +238,27 @@ static float home_direction(void)
             -90˚  -> antenna looks backwards
   */
   /* fixed to the plane*/
-  home_dir =  (float)(atan2(Home_PositionForPlane2.fy, (Home_PositionForPlane2.fx)));
+  home_dir = (float)(atan2(Home_PositionForPlane2.fy, (Home_PositionForPlane2.fx)));
   home_dir = DegOfRad(home_dir);
-  if(home_dir < 0){ home_dir += 360; }
+  if (home_dir < 0) { home_dir += 360; }
 
-return(home_dir);
+  return (home_dir);
 }
 
 static void check_osd_status(void)
 {
 
-osd_stat_reg_valid = FALSE;
+  osd_stat_reg_valid = FALSE;
 
-if (max7456_osd_status == OSD_IDLE){
-   max7456_trans.output_length = 1;
-   max7456_trans.input_length = 1;
-   max7456_trans.output_buf[0] = OSD_STAT_REG;
-   max7456_osd_status = OSD_READ_STATUS;
-   spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
-}
+  if (max7456_osd_status == OSD_IDLE) {
+    max7456_trans.output_length = 1;
+    max7456_trans.input_length = 1;
+    max7456_trans.output_buf[0] = OSD_STAT_REG;
+    max7456_osd_status = OSD_READ_STATUS;
+    spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
+  }
 
-return;
+  return;
 }
 
 static char ascii_to_osd_c(char c)
@@ -265,225 +267,220 @@ static char ascii_to_osd_c(char c)
 #if defined USE_MATEK_TYPE_OSD_CHIP && USE_MATEK_TYPE_OSD_CHIP == 1
 #warning OSD USES THE CUSTOM MATEK TYPE OSD CHIP
 
-return(c);
+  return (c);
 
 #else
 
-if (c >= '0' && c <= '9'){
-   if (c == '0'){ c -= 38; }else{  c -= 48; }
-}
-else
-if (c >= 'A' && c <= 'Z'){
+  if (c >= '0' && c <= '9') {
+    if (c == '0') { c -= 38; } else {  c -= 48; }
+  } else if (c >= 'A' && c <= 'Z') {
     c -= 54;
-}
-else
-if (c >= 'a' && c <= 'z'){
-   c -= 60;
+  } else if (c >= 'a' && c <= 'z') {
+    c -= 60;
 
-}else{
-       switch (c){
-              case('('): c = 0x3f; break;
-              case(')'): c = 0x40; break;
-              case('.'): c = 0x41; break;
-              case('?'): c = 0x42; break;
-              case(';'): c = 0x43; break;
-              case(':'): c = 0x44; break;
-              case(','): c = 0x45; break;
-              //case('''): c = 0x46; break;
-              case('/'): c = 0x47; break;
-              case('"'): c = 0x48; break;
-              case('-'): c = 0x49; break;
-              case('<'): c = 0x4A; break;
-              case('>'): c = 0x4B; break;
-              case('@'): c = 0x4C; break;
-              case(' '): c = 0x00; break;
-              case('\0'): c = 0xFF; break;
-              default  : break;
-       }
-     }
+  } else {
+    switch (c) {
+      case ('('): c = 0x3f; break;
+      case (')'): c = 0x40; break;
+      case ('.'): c = 0x41; break;
+      case ('?'): c = 0x42; break;
+      case (';'): c = 0x43; break;
+      case (':'): c = 0x44; break;
+      case (','): c = 0x45; break;
+      //case('''): c = 0x46; break;
+      case ('/'): c = 0x47; break;
+      case ('"'): c = 0x48; break;
+      case ('-'): c = 0x49; break;
+      case ('<'): c = 0x4A; break;
+      case ('>'): c = 0x4B; break;
+      case ('@'): c = 0x4C; break;
+      case (' '): c = 0x00; break;
+      case ('\0'): c = 0xFF; break;
+      default  : break;
+    }
+  }
 
-return(c);
+  return (c);
 
 #endif
-}                       
+}
 
 static void osd_put_s(char *string, uint8_t attributes, uint8_t char_nb, uint8_t row, uint8_t column)
 {
 
-int8_t x = 0, idx = 0, post_offset = 0, aft_offset = 0, string_len = 0;
-char osd_buf[OSD_STRING_SIZE];
+  int8_t x = 0, idx = 0, post_offset = 0, aft_offset = 0, string_len = 0;
+  char osd_buf[OSD_STRING_SIZE];
 
-if (row > 15){ column = 15; }
-if (column > 29){ column = 29; }
+  if (row > 15) { column = 15; }
+  if (column > 29) { column = 29; }
 
 // translate the string and put it to the "osd_string" '\0' = 0xff
-x = 0;
-while (*(string+x) != '\0'){ osd_string[x] = ascii_to_osd_c(*(string+x)); x++; }
-osd_string[x] = 0xff;
-string_len = x;
-idx = x;
+  x = 0;
+  while (*(string + x) != '\0') { osd_string[x] = ascii_to_osd_c(*(string + x)); x++; }
+  osd_string[x] = 0xff;
+  string_len = x;
+  idx = x;
 
-if (attributes & C_JUST){
-   if(char_nb%2 == 0){ char_nb++; }
-   post_offset = (char_nb - string_len)/2;
-   aft_offset = char_nb - string_len;
-   if( ((int8_t)column-(char_nb/2)) >= 0){ column -= (char_nb/2); }
-   for(x=0; x<30; x++){ osd_buf[x] = 0; } // FILL WITH SPACES
-   // COPY THE ORIGINAL STRING TO ITS NEW POSITION
-   for(x=0; x<string_len; x++){ osd_buf[post_offset+x] = osd_string[x]; }
-   osd_buf[string_len+aft_offset] = 0xFF; // TERMINATE THE MODIFIED STRING
-   // COPY THE MODIFIED STRING TO MAIN OSD STRING
-   x = 0;
-   do{ osd_string[x] = osd_buf[x]; }while(osd_buf[x++] != 0xFF);
-}
-else
-if (attributes & R_JUST){
-   //if(x){ x -= 1; }
-   //if (char_nb < string_len){ char_nb = string_len; }
-   if( ((int8_t)column-char_nb) >= 0){ column -= char_nb; }
-   if (((int8_t)char_nb-string_len) >= 0){ post_offset = char_nb-string_len; }else{post_offset = 0; }
-   //ADD LEADING SPACES
-   //First shift right the string and then add spaces at the beggining
-   while(idx >= 0){ osd_string[idx+post_offset] = osd_string[idx]; idx--; }
-   idx = 0;
-   while(idx < post_offset){ osd_string[idx] = 0; idx++; }
-   //osd_string[idx] = 0xff;
+  if (attributes & C_JUST) {
+    if (char_nb % 2 == 0) { char_nb++; }
+    post_offset = (char_nb - string_len) / 2;
+    aft_offset = char_nb - string_len;
+    if (((int8_t)column - (char_nb / 2)) >= 0) { column -= (char_nb / 2); }
+    for (x = 0; x < 30; x++) { osd_buf[x] = 0; } // FILL WITH SPACES
+    // COPY THE ORIGINAL STRING TO ITS NEW POSITION
+    for (x = 0; x < string_len; x++) { osd_buf[post_offset + x] = osd_string[x]; }
+    osd_buf[string_len + aft_offset] = 0xFF; // TERMINATE THE MODIFIED STRING
+    // COPY THE MODIFIED STRING TO MAIN OSD STRING
+    x = 0;
+    do { osd_string[x] = osd_buf[x]; } while (osd_buf[x++] != 0xFF);
+  } else if (attributes & R_JUST) {
+    //if(x){ x -= 1; }
+    //if (char_nb < string_len){ char_nb = string_len; }
+    if (((int8_t)column - char_nb) >= 0) { column -= char_nb; }
+    if (((int8_t)char_nb - string_len) >= 0) { post_offset = char_nb - string_len; } else {post_offset = 0; }
+    //ADD LEADING SPACES
+    //First shift right the string and then add spaces at the beggining
+    while (idx >= 0) { osd_string[idx + post_offset] = osd_string[idx]; idx--; }
+    idx = 0;
+    while (idx < post_offset) { osd_string[idx] = 0; idx++; }
+    //osd_string[idx] = 0xff;
 
-}else{
-        //Adjust for the reserved character number.
-        for (x=0; x < (int8_t)(sizeof(osd_string)); x++){ if(osd_string[x] == 0xFF){ break; } }
-        for (; x< char_nb; x++){  osd_string[x] = 0; }
-        osd_string[x] = 0xff;
-     }
+  } else {
+    //Adjust for the reserved character number.
+    for (x = 0; x < (int8_t)(sizeof(osd_string)); x++) { if (osd_string[x] == 0xFF) { break; } }
+    for (; x < char_nb; x++) {  osd_string[x] = 0; }
+    osd_string[x] = 0xff;
+  }
 
-osd_char_address = ((uint16_t)row*30) + column;
-osd_attr = (attributes & (BLINK|INVERT));
+  osd_char_address = ((uint16_t)row * 30) + column;
+  osd_attr = (attributes & (BLINK | INVERT));
 //TRIGGER THE SPI TRANSFERS. The rest of the spi transfers occur in the "max7456_event" function.
-if (max7456_osd_status == OSD_IDLE){
+  if (max7456_osd_status == OSD_IDLE) {
     max7456_trans.output_length = 2;
     max7456_trans.output_buf[0] = OSD_DMAH_REG;
-    max7456_trans.output_buf[1] = (uint8_t)((osd_char_address>>8) & 0x0001);
+    max7456_trans.output_buf[1] = (uint8_t)((osd_char_address >> 8) & 0x0001);
     max7456_osd_status = OSD_S_STEP1;
     spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
 
-}
+  }
 
-return;
+  return;
 }
 
 
-static bool _osd_sprintf(char* buffer, char* string, float value)
+static bool _osd_sprintf(char *buffer, char *string, float value)
 {
 
-uint8_t param_start = 0;
-uint8_t param_end = 0;
-uint8_t frac_nb = 0;
-uint8_t digit = 0;
-uint8_t x = 0, y = 0, z = 0;
+  uint8_t param_start = 0;
+  uint8_t param_end = 0;
+  uint8_t frac_nb = 0;
+  uint8_t digit = 0;
+  uint8_t x = 0, y = 0, z = 0;
 
-uint16_t i_dec = 0;
-uint16_t i_frac = 0;
+  uint16_t i_dec = 0;
+  uint16_t i_frac = 0;
 
-char to_asc[10] = {48,48,48,48,48,48,48,48,48,48};
-char string_buf[OSD_STRING_SIZE];
+  char to_asc[10] = {48, 48, 48, 48, 48, 48, 48, 48, 48, 48};
+  char string_buf[OSD_STRING_SIZE];
 
 // Clear the osd string.
-for (x=0; x < sizeof(osd_string); x++){ osd_string[x] = 0; }
-for (x=0; x < sizeof(string_buf); x++){ string_buf[x] = 0; }
+  for (x = 0; x < sizeof(osd_string); x++) { osd_string[x] = 0; }
+  for (x = 0; x < sizeof(string_buf); x++) { string_buf[x] = 0; }
 
 //copy the string passed as parameter to a buffer
-for (x=0; x < sizeof(string_buf); x++){ string_buf[x] = *(string+x); if(string_buf[x] == '\0') break; }
-x = 0;
-param_start = 0;
-param_end = 0;
+  for (x = 0; x < sizeof(string_buf); x++) { string_buf[x] = *(string + x); if (string_buf[x] == '\0') { break; } }
+  x = 0;
+  param_start = 0;
+  param_end = 0;
 //do {
-      //Now check for any special character
-      while (string_buf[x] != '\0'){
-      // EXAMPLE: in "%160c"x is '%' x+4 = 'c' and x+1='1', x+2='6' and x+3='0'
-      if (string_buf[x] == '%'){ if(string_buf[x+4] == 'c'){ (param_start = x+1); param_end = x+3; break; } }
-      x++;
-      }
-      if (param_end-param_start){
-         //load the special character value where the % character was
-         string_buf[x] = ((string_buf[param_start]-48)*100) + ((string_buf[param_start+1]-48)*10) + (string_buf[param_start+2]-48);
-         x++; // increment x to the next character which should be the first special character's digit
-         //Move the rest of the buffer forward so only the special character remains,
-         // for example in %170c '%' now has the special character's code and x now points to '1'
-         // which will be overwritten with the rest of the string after the 'c'
-         for (y=(x+4); y<=sizeof(string_buf); y++){ string_buf[x++] = string_buf[y]; }
-      }
+  //Now check for any special character
+  while (string_buf[x] != '\0') {
+    // EXAMPLE: in "%160c"x is '%' x+4 = 'c' and x+1='1', x+2='6' and x+3='0'
+    if (string_buf[x] == '%') { if (string_buf[x + 4] == 'c') { (param_start = x + 1); param_end = x + 3; break; } }
+    x++;
+  }
+  if (param_end - param_start) {
+    //load the special character value where the % character was
+    string_buf[x] = ((string_buf[param_start] - 48) * 100) + ((string_buf[param_start + 1] - 48) * 10) +
+                    (string_buf[param_start + 2] - 48);
+    x++; // increment x to the next character which should be the first special character's digit
+    //Move the rest of the buffer forward so only the special character remains,
+    // for example in %170c '%' now has the special character's code and x now points to '1'
+    // which will be overwritten with the rest of the string after the 'c'
+    for (y = (x + 4); y <= sizeof(string_buf); y++) { string_buf[x++] = string_buf[y]; }
+  }
 
 //}while((param_end-param_start > 0));
 
 // RESET THE USED VARIABLES JUST TO BE SAFE.
-x = 0;
-y = 0;
-param_start = 0;
-param_end = 0;
+  x = 0;
+  y = 0;
+  param_start = 0;
+  param_end = 0;
 // Search for the prameter start and stop positions.
-while ( string_buf[x] != '\0'){
-      if ( string_buf[x] == '%'){
-         param_start = x;
+  while (string_buf[x] != '\0') {
+    if (string_buf[x] == '%') {
+      param_start = x;
 
-      }else if ( string_buf[x] == 'f'){ param_end = x; break; }
-      x++;
-}
-if (param_end - param_start){
-   // find and bound the precision specified.
-   frac_nb =  string_buf[param_end-1] - 48; // Convert to number, ASCII 48 = '0'
-   if(frac_nb > 3){ frac_nb = 3; }       // Bound value.
+    } else if (string_buf[x] == 'f') { param_end = x; break; }
+    x++;
+  }
+  if (param_end - param_start) {
+    // find and bound the precision specified.
+    frac_nb =  string_buf[param_end - 1] - 48; // Convert to number, ASCII 48 = '0'
+    if (frac_nb > 3) { frac_nb = 3; }     // Bound value.
 
-   y = (sizeof(to_asc)- 1); // Point y to the end of the array.
-   i_dec = abs((int16_t)value);
-   // Fist we will deal with the fractional part if specified.
-   if (frac_nb > 0 && frac_nb <= 3){
+    y = (sizeof(to_asc) - 1); // Point y to the end of the array.
+    i_dec = abs((int16_t)value);
+    // Fist we will deal with the fractional part if specified.
+    if (frac_nb > 0 && frac_nb <= 3) {
       i_frac = abs((int16_t)((value - (int16_t)value) * 1000)); // Max precision is 3 digits.
       x = 100;
       z = frac_nb;
-      do{                            // Example if frac_nb=2 then 952 will show as .95
-           z--;
-           digit = (i_frac / x);
-           to_asc[y+z] = digit + 48; // Convert to ASCII
-           i_frac -= digit * x;      // Calculate the remainder.
-           x /= 10;                  // 952-(9*100) = 52, 52-(10*5)=2 etc.
+      do {                           // Example if frac_nb=2 then 952 will show as .95
+        z--;
+        digit = (i_frac / x);
+        to_asc[y + z] = digit + 48; // Convert to ASCII
+        i_frac -= digit * x;      // Calculate the remainder.
+        x /= 10;                  // 952-(9*100) = 52, 52-(10*5)=2 etc.
 
-      }while(z > 0);
+      } while (z > 0);
 
       y -= frac_nb;     // set y to point where the dot must be placed.
       to_asc[y] = '.';
       y--;             // Set y to point where the rest of the numbers must be written.
 
-   }  // if (frac_nb > 0 && frac_nb <= 3){
+    }  // if (frac_nb > 0 && frac_nb <= 3){
 
-   // Now it is time for the integer part. "y" already points to the position just before the dot.
-   do{
-        to_asc[y] = (i_dec % 10) + 48;            //Write at least one digit even if value is zero.
-        i_dec /= 10;
-        if (i_dec <= 0){                          // This way the leading zero is ommited.
-           if(value < 0){ y--; to_asc[y] = '-'; } // Place the minus sign if needed.
-           break;
+    // Now it is time for the integer part. "y" already points to the position just before the dot.
+    do {
+      to_asc[y] = (i_dec % 10) + 48;            //Write at least one digit even if value is zero.
+      i_dec /= 10;
+      if (i_dec <= 0) {                         // This way the leading zero is ommited.
+        if (value < 0) { y--; to_asc[y] = '-'; } // Place the minus sign if needed.
+        break;
 
-        }else{ y--; }
+      } else { y--; }
 
-   }while(1);
+    } while (1);
 
-   // Fill the buffer with the characters in the beggining of the string if any.
-   for (x=0; x<param_start; x++){ *(buffer+x) = string_buf[x]; }
+    // Fill the buffer with the characters in the beggining of the string if any.
+    for (x = 0; x < param_start; x++) { *(buffer + x) = string_buf[x]; }
 
-   // x is now pointing to the next character in osd_string.
-   // y is already pointing to the first digit or negative sign in "to_asc" array.
-   while (y < sizeof(to_asc)){ *(buffer + x) = to_asc[y]; x++; y++; }
-   // x is now pointing to the next character in osd_string.
-   // "param_end" is pointing to the last format character in the string.
-   do{
-        param_end++;
-        *(buffer + x++) = string_buf[param_end];
+    // x is now pointing to the next character in osd_string.
+    // y is already pointing to the first digit or negative sign in "to_asc" array.
+    while (y < sizeof(to_asc)) { *(buffer + x) = to_asc[y]; x++; y++; }
+    // x is now pointing to the next character in osd_string.
+    // "param_end" is pointing to the last format character in the string.
+    do {
+      param_end++;
+      *(buffer + x++) = string_buf[param_end];
 
-   }while(string_buf[param_end] != '\0'); //Write the rest of the string including the terminating char.
+    } while (string_buf[param_end] != '\0'); //Write the rest of the string including the terminating char.
 
-} // End of if (param_end - param_start)
+  } // End of if (param_end - param_start)
 
-return(0);
+  return (0);
 }
 
 void max7456_init(void)
@@ -513,12 +510,12 @@ void max7456_init(void)
 void max7456_periodic(void)
 {
 
- float temp = 0;
-struct FloatEulers* att = stateGetNedToBodyEulers_f();
-struct EnuCoor_f* pos = stateGetPositionEnu_f();
+  float temp = 0;
+  struct FloatEulers *att = stateGetNedToBodyEulers_f();
+  struct EnuCoor_f *pos = stateGetPositionEnu_f();
 
-float ph_x = waypoints[WP_HOME].x - pos->x;
-float ph_y = waypoints[WP_HOME].y - pos->y;
+  float ph_x = waypoints[WP_HOME].x - pos->x;
+  float ph_y = waypoints[WP_HOME].y - pos->y;
 //This code is executed always and checks if the "osd_enable" var has been changed by telemetry.
 //If yes then it commands a reset but this time turns on or off the osd overlay, not the video.
   if (max7456_osd_status == OSD_IDLE) {
@@ -548,145 +545,145 @@ float ph_y = waypoints[WP_HOME].y - pos->y;
     max7456_osd_status = OSD_INIT3;
     spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
   } else if (max7456_osd_status == OSD_IDLE && osd_enable > 0) {
-            switch (step){
-              case (0):
-                osd_put_s("HDG", FALSE, 3, 1, 14);
-                step = 1;
-                break;
-              case (1):
+    switch (step) {
+      case (0):
+        osd_put_s("HDG", FALSE, 3, 1, 14);
+        step = 1;
+        break;
+      case (1):
 #if !defined USE_MATEK_TYPE_OSD_CHIP || USE_MATEK_TYPE_OSD_CHIP == 0
-                osd_put_s("DISTANCE", FALSE, 8, 14, 12);
+        osd_put_s("DISTANCE", FALSE, 8, 14, 12);
 #endif
-                step = 10;
-                break;
-              case (10):
-	        osd_put_s("( )", FALSE, 3, 8, 14);
-                step = 20;
-                break;
-              case (20):
-                temp = ((float)electrical.vsupply);
-                osd_sprintf(osd_string, "%.1fV", temp );
-                if (temp > LOW_BAT_LEVEL){
-                   osd_put_s(osd_string, L_JUST, 5, 1, 1);
+        step = 10;
+        break;
+      case (10):
+        osd_put_s("( )", FALSE, 3, 8, 14);
+        step = 20;
+        break;
+      case (20):
+        temp = ((float)electrical.vsupply);
+        osd_sprintf(osd_string, "%.1fV", temp);
+        if (temp > LOW_BAT_LEVEL) {
+          osd_put_s(osd_string, L_JUST, 5, 1, 1);
 
-                }else{ osd_put_s(osd_string, L_JUST|BLINK|INVERT, 5, 1, 1); }
-                step = 30;
-                break;
-              case (30):
+        } else { osd_put_s(osd_string, L_JUST | BLINK | INVERT, 5, 1, 1); }
+        step = 30;
+        break;
+      case (30):
 #if OSD_USE_MAG_COMPASS && !defined(SITL)
 #pragma message "OSD USES THE MAGNETIC HEADING"
-                temp = DegOfRad(MAG_Heading);
-                if (temp < 0){ temp += 360; }
+        temp = DegOfRad(MAG_Heading);
+        if (temp < 0) { temp += 360; }
 #else
 #pragma message "OSD USES THE GPS HEADING"
-                temp = DegOfRad(state.h_speed_dir_f);
-                if (temp < 0){ temp += 360; }
+        temp = DegOfRad(state.h_speed_dir_f);
+        if (temp < 0) { temp += 360; }
 #endif
-                osd_sprintf(osd_string, "%.0f", temp);
-                osd_put_s(osd_string, C_JUST, 3, 1, 15);
-                step = 40;
-                break;
-              case (40):
-                osd_sprintf(osd_string, "%.0f KM", (state.h_speed_norm_f*3.6));
-                osd_put_s(osd_string, R_JUST, 6, 1, 29);
-                step = 42;
-                break;
-              case (42):
-                osd_sprintf(osd_string, "%.0fTHR", (((float)ap_state->commands[COMMAND_THROTTLE]/MAX_PPRZ)*100));
-                osd_put_s(osd_string, R_JUST, 5, 2, 29);
-                step = 50;
-                break;
-              case (50):
+        osd_sprintf(osd_string, "%.0f", temp);
+        osd_put_s(osd_string, C_JUST, 3, 1, 15);
+        step = 40;
+        break;
+      case (40):
+        osd_sprintf(osd_string, "%.0f KM", (state.h_speed_norm_f * 3.6));
+        osd_put_s(osd_string, R_JUST, 6, 1, 29);
+        step = 42;
+        break;
+      case (42):
+        osd_sprintf(osd_string, "%.0fTHR", (((float)ap_state->commands[COMMAND_THROTTLE] / MAX_PPRZ) * 100));
+        osd_put_s(osd_string, R_JUST, 5, 2, 29);
+        step = 50;
+        break;
+      case (50):
 #if OSD_USE_BARO_ALTITUDE && !defined(SITL)
 #pragma message "OSD ALTITUDE IS COMING FROM BAROMETER"
 #if defined BARO_ALTITUDE_VAR
-                osd_sprintf(osd_string, "%.0fM", BARO_ALTITUDE_VAR );
+        osd_sprintf(osd_string, "%.0fM", BARO_ALTITUDE_VAR);
 #else
 #warning OSD USES THE DEFAULT BARO ALTITUDE VARIABLE
-                osd_sprintf(osd_string, "%.0fM", baro_alt );
+        osd_sprintf(osd_string, "%.0fM", baro_alt);
 #endif
 #else
 #pragma message "ALTITUDE IS COMING FROM GPS"
-                osd_sprintf(osd_string, "%.0fM", GetPosAlt() );
+        osd_sprintf(osd_string, "%.0fM", GetPosAlt());
 #endif
-                osd_put_s(osd_string, L_JUST, 6, 14, 1); // "FALSE = L_JUST
-                step = 52;
-                break;
-              case (52):
+        osd_put_s(osd_string, L_JUST, 6, 14, 1); // "FALSE = L_JUST
+        step = 52;
+        break;
+      case (52):
 #if defined USE_MATEK_TYPE_OSD_CHIP && USE_MATEK_TYPE_OSD_CHIP == 1
-                // ANY SPECIAL CHARACTER CODE MUST BE A 3 DIGIT NUMBER WITH THE LEADING ZEROS!!!!
-                // THE SPECIAL CHARACTER CAN BE PLACED BEFORE OR AFTER THE FLOAT OR ANY OTHER CHARACTER
-                osd_sprintf(osd_string, "%191c%.0f", home_direction() );
-                osd_put_s(osd_string, C_JUST, 4, 2, 15); // "FALSE = L_JUST
+        // ANY SPECIAL CHARACTER CODE MUST BE A 3 DIGIT NUMBER WITH THE LEADING ZEROS!!!!
+        // THE SPECIAL CHARACTER CAN BE PLACED BEFORE OR AFTER THE FLOAT OR ANY OTHER CHARACTER
+        osd_sprintf(osd_string, "%191c%.0f", home_direction());
+        osd_put_s(osd_string, C_JUST, 4, 2, 15); // "FALSE = L_JUST
 #else
-                osd_sprintf(osd_string, "H%.0f", home_direction() );
-                osd_put_s(osd_string, C_JUST, 4, 2, 15); // "FALSE = L_JUST
+        osd_sprintf(osd_string, "H%.0f", home_direction());
+        osd_put_s(osd_string, C_JUST, 4, 2, 15); // "FALSE = L_JUST
 
 #endif
-                step = 60;
-                break;
+        step = 60;
+        break;
 
-              case (60):
+      case (60):
 #if defined USE_MATEK_TYPE_OSD_CHIP && USE_MATEK_TYPE_OSD_CHIP == 1
-                // ANY SPECIAL CHARACTER CODE MUST BE A 3 DIGIT NUMBER WITH THE LEADING ZEROS!!!!
-                // THE SPECIAL CHARACTER CAN BE PLACED BEFORE OR AFTER THE FLOAT OR ANY OTHER CHARACTER
-                osd_sprintf(osd_string, "%160c%.0fM", (float)(sqrt(ph_x*ph_x + ph_y *ph_y)));
-                osd_put_s(osd_string, C_JUST, 6, 14, 15);
+        // ANY SPECIAL CHARACTER CODE MUST BE A 3 DIGIT NUMBER WITH THE LEADING ZEROS!!!!
+        // THE SPECIAL CHARACTER CAN BE PLACED BEFORE OR AFTER THE FLOAT OR ANY OTHER CHARACTER
+        osd_sprintf(osd_string, "%160c%.0fM", (float)(sqrt(ph_x * ph_x + ph_y * ph_y)));
+        osd_put_s(osd_string, C_JUST, 6, 14, 15);
 #else
-                osd_sprintf(osd_string, "%.0fM", (float)(sqrt(ph_x*ph_x + ph_y *ph_y)));
-                osd_put_s(osd_string, C_JUST, 6, 14, 15);
+        osd_sprintf(osd_string, "%.0fM", (float)(sqrt(ph_x * ph_x + ph_y * ph_y)));
+        osd_put_s(osd_string, C_JUST, 6, 14, 15);
 #endif
-                step = 70;
-                break;
-              case (70):
-                osd_sprintf(osd_string, "%.1fVZ", stateGetSpeedEnu_f()->z);
-                osd_put_s(osd_string, R_JUST, 9, 14, 29);
-                step = 80;
-                break;
-              // A Text PFD as graphics are not the strong point of the MAX7456
-              // In order to level the aircraft while fpving
-              // just move the stick to the opposite direction from the angles shown on the osd
-              // and that's why positive pitch (UP) is shown below the OSD center
-              case (80):
-                if (DegOfRad(att->theta) > 2){
-                   osd_sprintf(osd_string, "%.0f", DegOfRad(att->theta));
-                   osd_put_s(osd_string, C_JUST, 5, 6, 15);
- 
-                }else{ osd_put_s("    ", C_JUST, 5, 6, 15); }
-                step = 90;
-                break;
-              case (90):
-                if (DegOfRad(att->theta) < -2){
-                   osd_sprintf(osd_string, "%.0f", DegOfRad(att->theta));
-                   osd_put_s(osd_string, C_JUST, 5, 10, 15);
- 
-                }else{ osd_put_s("   ", C_JUST, 5, 10, 15); }
-                step = 100;
-                break;
-              case (100):
-                if (DegOfRad(att->phi) > 2){
-                   osd_sprintf(osd_string, "%.0f>", DegOfRad(att->phi));
-                   osd_put_s(osd_string, FALSE, 5, 8, 18);
- 
-                }else{ osd_put_s("     ", FALSE, 5, 8, 18); }
-                step = 110;
-                break;
-              case (110):
-                if (DegOfRad(att->phi) < -2){
-                   osd_sprintf(osd_string, "<%.0f", DegOfRad(fabs(att->phi)) );
-                   osd_put_s(osd_string, R_JUST, 5, 8, 13);
- 
-                }else{ osd_put_s("     ", R_JUST, 5, 8, 13); }
-                step = 120;
-                break;
+        step = 70;
+        break;
+      case (70):
+        osd_sprintf(osd_string, "%.1fVZ", stateGetSpeedEnu_f()->z);
+        osd_put_s(osd_string, R_JUST, 9, 14, 29);
+        step = 80;
+        break;
+      // A Text PFD as graphics are not the strong point of the MAX7456
+      // In order to level the aircraft while fpving
+      // just move the stick to the opposite direction from the angles shown on the osd
+      // and that's why positive pitch (UP) is shown below the OSD center
+      case (80):
+        if (DegOfRad(att->theta) > 2) {
+          osd_sprintf(osd_string, "%.0f", DegOfRad(att->theta));
+          osd_put_s(osd_string, C_JUST, 5, 6, 15);
 
-              case (120):
-                check_osd_status();
-                step = 10;
-                break;
+        } else { osd_put_s("    ", C_JUST, 5, 6, 15); }
+        step = 90;
+        break;
+      case (90):
+        if (DegOfRad(att->theta) < -2) {
+          osd_sprintf(osd_string, "%.0f", DegOfRad(att->theta));
+          osd_put_s(osd_string, C_JUST, 5, 10, 15);
 
-              default: step = 10; break;
-   }  // End of switch statement.
+        } else { osd_put_s("   ", C_JUST, 5, 10, 15); }
+        step = 100;
+        break;
+      case (100):
+        if (DegOfRad(att->phi) > 2) {
+          osd_sprintf(osd_string, "%.0f>", DegOfRad(att->phi));
+          osd_put_s(osd_string, FALSE, 5, 8, 18);
+
+        } else { osd_put_s("     ", FALSE, 5, 8, 18); }
+        step = 110;
+        break;
+      case (110):
+        if (DegOfRad(att->phi) < -2) {
+          osd_sprintf(osd_string, "<%.0f", DegOfRad(fabs(att->phi)));
+          osd_put_s(osd_string, R_JUST, 5, 8, 13);
+
+        } else { osd_put_s("     ", R_JUST, 5, 8, 13); }
+        step = 120;
+        break;
+
+      case (120):
+        check_osd_status();
+        step = 10;
+        break;
+
+      default: step = 10; break;
+    }  // End of switch statement.
   }  // End of if (max7456_osd_status == OSD_UNINIT)
   return;
 }
@@ -721,13 +718,13 @@ void max7456_event(void)
         max7456_osd_status = OSD_FINISHED;
         spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
         break;
-/*
-      case (OSD_READ_STATUS):
-        osd_stat_reg = max7456_trans.input_buf[0];
-        osd_stat_reg_valid = true;
-        max7456_osd_status = OSD_FINISHED;
-        break;
-*/
+      /*
+            case (OSD_READ_STATUS):
+              osd_stat_reg = max7456_trans.input_buf[0];
+              osd_stat_reg_valid = true;
+              max7456_osd_status = OSD_FINISHED;
+              break;
+      */
       case (OSD_S_STEP1):
         max7456_trans.output_length = 2;
         max7456_trans.output_buf[0] = OSD_DMAL_REG;
@@ -755,10 +752,10 @@ void max7456_event(void)
         }
         break;
       case (OSD_READ_STATUS):
-               osd_stat_reg = max7456_trans.input_buf[0];
-               osd_stat_reg_valid = TRUE;
-               max7456_trans.status = SPITransDone;
-               max7456_osd_status = OSD_IDLE;
+        osd_stat_reg = max7456_trans.input_buf[0];
+        osd_stat_reg_valid = TRUE;
+        max7456_trans.status = SPITransDone;
+        max7456_osd_status = OSD_IDLE;
         break;
       case (OSD_FINISHED):
         osd_attr = 0;