Browse Source

AP_Hott_Telem: fixed GPS display for mz-32

thanks to Ralf Helbing
zr-v5.1
Andrew Tridgell 5 years ago
parent
commit
f721fb1743
  1. 5
      libraries/AP_Hott_Telem/AP_Hott_Telem.cpp

5
libraries/AP_Hott_Telem/AP_Hott_Telem.cpp

@ -208,7 +208,7 @@ void AP_Hott_Telem::send_GPS(void) @@ -208,7 +208,7 @@ void AP_Hott_Telem::send_GPS(void)
uint16_t climbrate; //#24 m/s 0.01m/s resolution. Value of 30000 = 0.00 m/s
uint8_t climbrate3s; //#26 climbrate in m/3s resolution, value of 120 = 0 m/3s
uint8_t gps_satelites; //#27 sat count
uint8_t free_char3; //#28 ???
uint8_t gps_fix_char; //#28 GPS fix character. display, 'D' = DGPS, '2' = 2D, '3' = 3D, '-' = no fix
uint8_t home_direction; //#29 direction from starting point to Model position (2 degree steps)
int16_t vel_north; //#30 velocity north mm/s
uint8_t speed_acc; //#32 speed accuracy cm/s
@ -220,7 +220,7 @@ void AP_Hott_Telem::send_GPS(void) @@ -220,7 +220,7 @@ void AP_Hott_Telem::send_GPS(void)
uint8_t horiz_acc; //#39 horizontal accuracy
uint8_t free_char1; //#40 displayed to right of home
uint8_t free_char2; //#41
uint8_t gps_fix_char; //#42 GPS fix character. display, 'D' = DGPS, '2' = 2D, '3' = 3D, '-' = no fix
uint8_t free_char3; //#42 GPS fix character. display, 'D' = DGPS, '2' = 2D, '3' = 3D, '-' = no fix
uint8_t version = 1; //#43 0: GPS Graupner #33600, 1: ArduPilot
uint8_t stop_byte = 0x7d; //#44
} msg {};
@ -287,6 +287,7 @@ void AP_Hott_Telem::send_GPS(void) @@ -287,6 +287,7 @@ void AP_Hott_Telem::send_GPS(void)
msg.gps_fix_char = '3';
break;
}
msg.free_char3 = msg.gps_fix_char;
msg.home_direction = degrees(atan2f(home_vec.y, home_vec.x)) * 0.5 + 0.5;

Loading…
Cancel
Save