Robotics Institute 

University of Michigan, Ann Arbor

© 2017 by Bruce Personal Website. 

Updated on 8/21/2017

MAVLink

January 31, 2016

#include "../GCS_MAVLink/include/mavlink/v1.0/mavlink_types.h"
#include "../GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink.h"

 

// 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 read_mavlink(){
    mavlink_message_t msg; 
    mavlink_status_t status;

    //grabing data 
    while(Serial.available() > 0) { 
        uint8_t c = Serial.read();
        if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
            lastMAVBeat = millis();
            mavlink_active = 1;
            switch(msg.msgid) {
            case MAVLINK_MSG_ID_HEARTBEAT:
                {
                
                    mavbeat = 1;
                    apm_mav_system    = msg.sysid;
                    apm_mav_component = msg.compid;
                    osd_mode = (uint8_t)mavlink_msg_heartbeat_get_custom_mode(&msg);
                    base_mode = mavlink_msg_heartbeat_get_base_mode(&msg);
                    motor_armed = getBit(base_mode,7);

                    osd_nav_mode = 0;  
                }
                break;
            case MAVLINK_MSG_ID_SYS_STATUS:
                {

                    osd_vbat_A = (mavlink_msg_sys_status_get_voltage_battery(&msg) / 1000.0f); //Battery voltage, in millivolts (1 = 1 millivolt)
                    osd_curr_A = mavlink_msg_sys_status_get_current_battery(&msg); //Battery current, in 10*milliamperes (1 = 10 milliampere)         
                    osd_battery_remaining_A = mavlink_msg_sys_status_get_battery_remaining(&msg); //Remaining battery energy: (0%: 0, 100%: 100)
                }
                break;

            case MAVLINK_MSG_ID_GPS_RAW_INT:
                {
                    osd_alt_gps = mavlink_msg_gps_raw_int_get_alt(&msg) / 1000.0f;
                    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);
                    osd_cog = mavlink_msg_gps_raw_int_get_cog(&msg);
                    eph = mavlink_msg_gps_raw_int_get_eph(&msg);
                    idle_Count=0;
                    if(bGotHome == 0 && osd_fix_type > 1)
                     {
                      osd_home_lat = osd_lat;
                      osd_home_lon = osd_lon;
                      bGotHome = 1;
                     }
                    if(bGotHome==1)
                     {
                      float dstlon,dstlat;
                      float rads = fabs(osd_home_lat) * 0.0174532925;
                      double scaleLongDown = cos(rads);
                      double scaleLongUp   = 1.0f/cos(rads);
                      dstlat = fabs(osd_home_lat - osd_lat) * 111319.5;
                      dstlon = fabs(osd_home_lon - osd_lon) * 111319.5 * scaleLongDown;
                      osd_home_distance = sqrt(sq(dstlat) + sq(dstlon)); 
                     } 
                }
                break;             
            case MAVLINK_MSG_ID_VFR_HUD:
                {
                    osd_airspeed = mavlink_msg_vfr_hud_get_airspeed(&msg);
                    osd_groundspeed = mavlink_msg_vfr_hud_get_groundspeed(&msg);
                    osd_heading = mavlink_msg_vfr_hud_get_heading(&msg); // 0..360 deg, 0=north
                    osd_throttle = (uint8_t)mavlink_msg_vfr_hud_get_throttle(&msg);
                    osd_alt_rel = mavlink_msg_vfr_hud_get_alt(&msg);
                    osd_climb = mavlink_msg_vfr_hud_get_climb(&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;
                
            case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
                {

                  wp_target_bearing = mavlink_msg_nav_controller_output_get_target_bearing(&msg);
                  wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(&msg);

                  xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(&msg);
                }
                break;
            case MAVLINK_MSG_ID_MISSION_CURRENT:
                {
                    wp_number = (uint8_t)mavlink_msg_mission_current_get_seq(&msg);
                }
                break;
            case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
                {
                    chan1_raw = mavlink_msg_rc_channels_raw_get_chan1_raw(&msg);
                    chan2_raw = mavlink_msg_rc_channels_raw_get_chan2_raw(&msg);
                    chan3_raw = mavlink_msg_rc_channels_raw_get_chan3_raw(&msg);
                    chan4_raw = mavlink_msg_rc_channels_raw_get_chan4_raw(&msg);
                    chan5_raw = mavlink_msg_rc_channels_raw_get_chan5_raw(&msg);
                    chan6_raw = mavlink_msg_rc_channels_raw_get_chan6_raw(&msg);
                    chan7_raw = mavlink_msg_rc_channels_raw_get_chan7_raw(&msg);
                    chan8_raw = mavlink_msg_rc_channels_raw_get_chan8_raw(&msg);
                    osd_rssi = mavlink_msg_rc_channels_raw_get_rssi(&msg);
                }
                break;           
            case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: 
                { 
                    //osd_home_alt = osd_alt - (mavlink_msg_global_position_int_get_relative_alt(&msg)*0.001); 
                    //Commented because it seems that we only get relative alt when we have GPS lock.
                    //That shouldn't be because we may rely only on baro. So using vfr hud alt (testing)
                    //osd_alt_rel = (mavlink_msg_global_position_int_get_relative_alt(&msg)*0.001);
                }
                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;

}

Please reload