@@ -41,14 +41,13 @@ void read_mavlink(){
4141 } else {
4242 crlf_count = 0 ;
4343 }
44- // if (crlf_count == 3) {
45- // uploadFont();
46- // }
44+ if (crlf_count == 3 ) {
45+ uploadFont ();
46+ }
4747 }
4848
4949 // trying to grab msg
5050 if (mavlink_parse_char (MAVLINK_COMM_0, c, &msg, &status)) {
51- lastMAVBeat = millis ();
5251 mavlink_active = 1 ;
5352 // handle msg
5453 switch (msg.msgid ) {
@@ -62,14 +61,14 @@ void read_mavlink(){
6261 osd_mode = (uint8_t )mavlink_msg_heartbeat_get_custom_mode (&msg);
6362 // Mode (arducoper armed/disarmed)
6463 base_mode = mavlink_msg_heartbeat_get_base_mode (&msg);
65- // if(getBit(base_mode,7)) motor_armed = 1;
66- // else motor_armed = 0;
64+ if (getBit (base_mode,7 )) motor_armed = 1 ;
65+ else motor_armed = 0 ;
6766
6867 osd_nav_mode = 0 ;
69- /* lastMAVBeat = millis();
68+ lastMAVBeat = millis ();
7069 if (waitingMAVBeats == 1 ){
7170 enable_mav_request = 1 ;
72- }*/
71+ }
7372 }
7473 break ;
7574 case MAVLINK_MSG_ID_SYS_STATUS:
@@ -89,8 +88,6 @@ void read_mavlink(){
8988 osd_lon = mavlink_msg_gps_raw_int_get_lon (&msg) / 10000000 .0f ;
9089 osd_fix_type = mavlink_msg_gps_raw_int_get_fix_type (&msg);
9190 osd_satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible (&msg);
92- osd_cog = mavlink_msg_gps_raw_int_get_cog (&msg);
93- eph = mavlink_msg_gps_raw_int_get_eph (&msg);
9491 }
9592 break ;
9693 case MAVLINK_MSG_ID_VFR_HUD:
@@ -117,8 +114,8 @@ void read_mavlink(){
117114// nav_bearing = mavlink_msg_nav_controller_output_get_nav_bearing(&msg);
118115 wp_target_bearing = mavlink_msg_nav_controller_output_get_target_bearing (&msg);
119116 wp_dist = mavlink_msg_nav_controller_output_get_wp_dist (&msg);
120- alt_error = mavlink_msg_nav_controller_output_get_alt_error (&msg);
121- aspd_error = mavlink_msg_nav_controller_output_get_aspd_error (&msg);
117+ // alt_error = mavlink_msg_nav_controller_output_get_alt_error(&msg);
118+ // aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(&msg);
122119 xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error (&msg);
123120 }
124121 break ;
@@ -142,23 +139,16 @@ void read_mavlink(){
142139 break ;
143140 case MAVLINK_MSG_ID_WIND:
144141 {
145- // if (osd_climb < 1.66 && osd_climb > -1.66){
146- osd_winddirection = mavlink_msg_wind_get_direction (&msg); // 0..360 deg, 0=north
147- osd_windspeed = mavlink_msg_wind_get_speed (&msg); // m/s
148- // osd_windspeedz = mavlink_msg_wind_get_speed_z(&msg); //m/s
149- // }
142+ osd_winddirection = abs (mavlink_msg_wind_get_direction (&msg)); // 0..360 deg, 0=north
143+ osd_windspeed = mavlink_msg_wind_get_speed (&msg); // m/s
144+ // osd_windspeedz = mavlink_msg_wind_get_speed_z(&msg); //m/s
150145 }
151146 break ;
152147 case MAVLINK_MSG_ID_SCALED_PRESSURE:
153148 {
154149 temperature = mavlink_msg_scaled_pressure_get_temperature (&msg);
155150 }
156151 break ;
157- case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
158- {
159- osd_home_alt = osd_alt - (mavlink_msg_global_position_int_get_relative_alt (&msg)*0.001 );
160- }
161- break ;
162152 default :
163153 // Do nothing
164154 break ;
0 commit comments