MAVLink

#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;

}