#define MAVLINK_COMM_NUM_BUFFERS 1
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS

// this code was moved from libraries/GCS_MAVLink to allow compile
// time selection of MAVLink 1.0
BetterStream	*mavlink_comm_0_port;
BetterStream	*mavlink_comm_1_port;

mavlink_system_t mavlink_system = {12,1,0,0};

#include "Mavlink_compat.h"

#ifdef MAVLINK10
#include "../GCS_MAVLink/include/mavlink/v1.0/mavlink_types.h"
#include "../GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink.h"
#else
#include "../GCS_MAVLink/include/mavlink/v0.9/mavlink_types.h"
#include "../GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/mavlink.h"
#endif

// true when we have received at least 1 MAVLink packet
static bool mavlink_active;
static uint8_t crlf_count = 0;

static int packet_drops = 0;
static int parse_error = 0;

void request_mavlink_rates()
{
  const int  maxStreams = 6;
  const uint8_t MAVStreams[maxStreams] = {MAV_DATA_STREAM_RAW_SENSORS,
					  MAV_DATA_STREAM_EXTENDED_STATUS,
                                          MAV_DATA_STREAM_RC_CHANNELS,
					  MAV_DATA_STREAM_POSITION,
                                          MAV_DATA_STREAM_EXTRA1, 
                                          MAV_DATA_STREAM_EXTRA2};
  const uint16_t MAVRates[maxStreams] = {0x02, 0x02, 0x05, 0x02, 0x05, 0x02};
  for (int i=0; i < maxStreams; i++) {
    	  mavlink_msg_request_data_stream_send(MAVLINK_COMM_0,
					       apm_mav_system, apm_mav_component,
					       MAVStreams[i], MAVRates[i], 1);
  }
}

void read_mavlink(){
  mavlink_message_t msg; 
  mavlink_status_t status;
  
  //grabing data 
  while(Serial.available() > 0) { 
    uint8_t c = Serial.read();
    
            /* allow CLI to be started by hitting enter 3 times, if no
           heartbeat packets have been received */
        if (mavlink_active == 0 && millis() < 20000) {
            if (c == '\n' || c == '\r') {
                crlf_count++;
            } else {
                crlf_count = 0;
            }
            if (crlf_count == 3) {
              uploadFont();
            }
        }
    
    //trying to grab msg  
    if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
       mavlink_active = 1;
      //handle msg
      switch(msg.msgid) {
        case MAVLINK_MSG_ID_HEARTBEAT:
          {
            mavbeat = 1;
	    apm_mav_system    = msg.sysid;
	    apm_mav_component = msg.compid;
            apm_mav_type      = mavlink_msg_heartbeat_get_type(&msg);
#ifdef MAVLINK10             
            osd_mode = mavlink_msg_heartbeat_get_custom_mode(&msg);
            osd_nav_mode = 0;
#endif            
            lastMAVBeat = millis();
            if(waitingMAVBeats == 1){
              enable_mav_request = 1;
            }
          }
          break;
        case MAVLINK_MSG_ID_SYS_STATUS:
          {
#ifndef MAVLINK10            
            osd_vbat_A = (mavlink_msg_sys_status_get_vbat(&msg) / 1000.0f);
            osd_mode = mavlink_msg_sys_status_get_mode(&msg);
            osd_nav_mode = mavlink_msg_sys_status_get_nav_mode(&msg);
#else
            osd_vbat_A = (mavlink_msg_sys_status_get_voltage_battery(&msg) / 1000.0f);
#endif            
            osd_battery_remaining_A = mavlink_msg_sys_status_get_battery_remaining(&msg);
            //osd_mode = apm_mav_component;//Debug
            //osd_nav_mode = apm_mav_system;//Debug
          }
          break;
#ifndef MAVLINK10 
        case MAVLINK_MSG_ID_GPS_RAW:
          {
            osd_lat = mavlink_msg_gps_raw_get_lat(&msg);
            osd_lon = mavlink_msg_gps_raw_get_lon(&msg);
            osd_fix_type = mavlink_msg_gps_raw_get_fix_type(&msg);
          }
          break;
        case MAVLINK_MSG_ID_GPS_STATUS:
          {
            osd_satellites_visible = mavlink_msg_gps_status_get_satellites_visible(&msg);
          }
          break;
#else
        case MAVLINK_MSG_ID_GPS_RAW_INT:
          {
            osd_lat = mavlink_msg_gps_raw_int_get_lat(&msg) / 10000000.0f;
            osd_lon = mavlink_msg_gps_raw_int_get_lon(&msg) / 10000000.0f;
            osd_fix_type = mavlink_msg_gps_raw_int_get_fix_type(&msg);
            osd_satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(&msg);
          }
          break;
#endif          

        case MAVLINK_MSG_ID_VFR_HUD:
          {
            osd_groundspeed = mavlink_msg_vfr_hud_get_groundspeed(&msg);
            osd_heading = mavlink_msg_vfr_hud_get_heading(&msg);// * 3.60f;//0-100% of 360
            osd_throttle = mavlink_msg_vfr_hud_get_throttle(&msg);
            if(osd_throttle > 100 && osd_throttle < 150) osd_throttle = 100;//Temporary fix for ArduPlane 2.28
            if(osd_throttle < 0 || osd_throttle > 150) osd_throttle = 0;//Temporary fix for ArduPlane 2.28
            osd_alt = mavlink_msg_vfr_hud_get_alt(&msg);
          }
          break;
        case MAVLINK_MSG_ID_ATTITUDE:
          {
            osd_pitch = ToDeg(mavlink_msg_attitude_get_pitch(&msg));
            osd_roll = ToDeg(mavlink_msg_attitude_get_roll(&msg));
            osd_yaw = ToDeg(mavlink_msg_attitude_get_yaw(&msg));
          }
          break;
        default:
          //Do nothing
          break;
      }
    }
    delayMicroseconds(138);
    //next one
  }
  // Update global packet drops counter
  packet_drops += status.packet_rx_drop_count;
  parse_error += status.parse_error;

}