#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"
#include "include/mavlink/v1.0/mavlink_types.h"
#include "include/mavlink/v1.0/common/mavlink.h"
static int packet_drops = 0;
static int parse_error = 0;
void request_mavlink_rates()
{
//serial debug entfernt
}
break;
case MAVLINK_MSG_ID_SYS_STATUS:
{
// AQ sends (0,0,0,idle %, batt, batt remaining, drops, 0,0,0,0)
m2h_vbat_A = (mavlink_msg_sys_status_get_voltage_battery(&msg) / 100.0f); // It will arive in mV, but Hott uses V * 100
//m2h_battery_remaining_A = mavlink_msg_sys_status_get_battery_remaining(&msg); // not used in HoTT
m2h_abat = mavlink_msg_sys_status_get_current_battery(&msg);
}
break;
/*
* @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
* @param lat Latitude in 1E7 degrees
* @param lon Longitude in 1E7 degrees
* @param alt Altitude in 1E3 meters (millimeters) above MSL
* @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
* @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
* @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535
* @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
* @param satellites_visible Number of satellites visible. If unknown, set to 255
*/
case MAVLINK_MSG_ID_GPS_RAW_INT: // is send by AutoQad
{
m2h_fix_type = mavlink_msg_gps_raw_int_get_fix_type(&msg); //1
m2h_gps_lon = mavlink_msg_gps_raw_int_get_lon(&msg); // latitude 2
m2h_gps_lat = mavlink_msg_gps_raw_int_get_lat(&msg); // longitude 3
m2h_gps_alt = mavlink_msg_gps_raw_int_get_alt(&msg); // gps altitude 4
m2h_gps_vel = mavlink_msg_gps_raw_int_get_vel(&msg); // speed in cm/s 5
m2h_gps_cog = mavlink_msg_gps_raw_int_get_cog(&msg); // direction of movement 6
m2h_satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(&msg);
}
break;
case MAVLINK_MSG_ID_ATTITUDE:
{
m2h_pitch = ToDeg(mavlink_msg_attitude_get_pitch(&msg));
m2h_roll = ToDeg(mavlink_msg_attitude_get_roll(&msg));
m2h_yaw = ToDeg(mavlink_msg_attitude_get_yaw(&msg));
}
break;
/**
* @brief Pack a scaled_pressure message on a channel
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message was sent over
* @param msg The MAVLink message to compress the data into
* @param time_boot_ms Timestamp (microseconds since UNIX epoch or microseconds since system boot) -- AutoQuad
* @param press_abs Absolute pressure (hectopascal) -- AutoQuad AQ_PRESSURE*0.01f
* @param press_diff Differential pressure 1 (hectopascal) -- AutoQuad 0.0f
* @param temperature Temperature measurement (0.01 degrees celsius) -- AutoQuad
* @return length of the message in bytes (excluding serial stream start sign)
*/
case MAVLINK_MSG_ID_SCALED_PRESSURE:
{
m2h_abs = mavlink_msg_scaled_pressure_get_press_abs(&msg); // absolute presure
m2h_temp = mavlink_msg_scaled_pressure_get_temperature(&msg); // internal pressure sensor temp
}
break;
/**
* @return RC channel 1 value, in microseconds
*/
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
{
throttle = mavlink_msg_rc_channels_raw_get_chan3_raw(&msg);
}
break;
/**
* @param target_system System ID
* @param latitude global position * 1E7
* @param longitude global position * 1E7
* @param altitude global position * 1000
*/
case MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT:
{
m2h_gps_lat_home = mavlink_msg_set_global_position_setpoint_int_get_latitude(&msg);
m2h_gps_lon_home = mavlink_msg_set_global_position_setpoint_int_get_longitude(&msg);
m2h_ceiling = mavlink_msg_set_global_position_setpoint_int_get_altitude(&msg);
if ( (m2h_gps_lat_home !=0) && (m2h_gps_lon_home !=0)) m2h_got_home = 1;
}
break;
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
m2h_rel_alt = mavlink_msg_global_position_int_get_relative_alt(&msg);
}
break;
/* I know, totaly misused this message with other variables but the Android app doesn't take any other 'alien' message */
case MAVLINK_MSG_ID_VFR_HUD:
{
m2h_gps_lat_home = mavlink_msg_vfr_hud_get_airspeed(&msg);
m2h_gps_lon_home = mavlink_msg_vfr_hud_get_groundspeed(&msg);
m2h_ceiling = mavlink_msg_vfr_hud_get_alt(&msg);
m2h_heading = mavlink_msg_vfr_hud_get_heading(&msg);
if ( (m2h_gps_lat_home !=0) && (m2h_gps_lon_home !=0)) m2h_got_home = 1;
}
break;
case MAVLINK_MSG_ID_STATUSTEXT:
{
#ifdef SERDB
DPL(mavlink_msg_statustext_get_severity(&msg));
#endif
}
break;
// * * * end messages * * *
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;
}