void MavlinkProcessor::receiveTelemetry() {
tryToConnectToAPM();
while(MavLinkSerial.available()) {
uint8_t c = MavLinkSerial.read();
if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
switch(msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: // 0
if (msg.sysid == 1) {
gathered_telemetry.base_mode = mavlink_msg_heartbeat_get_base_mode(&msg);
gathered_telemetry.custom_mode = mavlink_msg_heartbeat_get_custom_mode(&msg);
connection_timer = millis();
if(!is_connected); {
heartbeat_count++;
if((heartbeat_count) > 10) { // If received > 10 heartbeats from MavLink then we are connected
is_connected = true;
heartbeat_count = 0;
digitalWrite(alive_led_pin, HIGH); // LED will be ON when connected to MavLink, else it will slowly blink
}
}
}
break;
case MAVLINK_MSG_ID_SYS_STATUS : // 1
gathered_telemetry.battery_voltage = mavlink_msg_sys_status_get_voltage_battery(&msg) / 10; //
gathered_telemetry.battery_current = mavlink_msg_sys_status_get_current_battery(&msg) / 10; //
gathered_telemetry.battery_remaining = mavlink_msg_sys_status_get_battery_remaining(&msg);
break;
case MAVLINK_MSG_ID_GPS_RAW_INT: // 24
gathered_telemetry.gps_fixtype = mavlink_msg_gps_raw_int_get_fix_type(&msg);// 0 = No GPS, 1 =No Fix, 2 = 2D Fix, 3 = 3D Fix
gathered_telemetry.gps_satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(&msg); // numbers of visible satelites
if(gathered_telemetry.gps_fixtype == 3) {
gathered_telemetry.gps_hdop = mavlink_msg_gps_raw_int_get_eph(&msg); // hdop * 100
gathered_telemetry.gps_latitude = mavlink_msg_gps_raw_int_get_lat(&msg);
gathered_telemetry.gps_longitude = mavlink_msg_gps_raw_int_get_lon(&msg);
gathered_telemetry.gps_altitude = mavlink_msg_gps_raw_int_get_alt(&msg); // 1m =1000
gathered_telemetry.gps_speed = mavlink_msg_gps_raw_int_get_vel(&msg);
}
else {
gathered_telemetry.gps_hdop = 9999;
gathered_telemetry.gps_latitude = 0L;
gathered_telemetry.gps_longitude = 0l;
gathered_telemetry.gps_altitude = 0L;
gathered_telemetry.gps_speed = 0L;
}
break;
case MAVLINK_MSG_ID_VFR_HUD: // 74
gathered_telemetry.groundspeed = mavlink_msg_vfr_hud_get_groundspeed(&msg); // 100 = 1m/s
gathered_telemetry.heading = mavlink_msg_vfr_hud_get_heading(&msg) ; // 100 = 100 deg
gathered_telemetry.throttle = mavlink_msg_vfr_hud_get_throttle(&msg); // 100 = 100%
gathered_telemetry.bar_altitude = mavlink_msg_vfr_hud_get_alt(&msg); // m
gathered_telemetry.climb_rate = mavlink_msg_vfr_hud_get_climb(&msg) * 100; // m/s
break;
case MAVLINK_MSG_ID_STATUSTEXT:
mavlink_msg_statustext_decode(&msg, &gathered_telemetry.status_text);
frsky_send_text_message(gathered_telemetry.status_text.text);
break;
default:
break;
}
}
}
}
void MavlinkProcessor::receiveTelemetry(Telemetry& gathered_telemetry) {
uint32_t next_timeout = millis() + 6;
tryToConnectToAPM();
while(MavLinkSerial.available() && millis() < next_timeout){
if (mavlink_parse_char(MAVLINK_COMM_0, MavLinkSerial.read(), &msg, &status)) {
switch(msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: // 0
if (msg.sysid == 1) {
CHECK_CHANGED(gathered_telemetry.base_mode, mavlink_msg_heartbeat_get_base_mode(&msg), BASE_MODE);
CHECK_CHANGED(gathered_telemetry.custom_mode, mavlink_msg_heartbeat_get_custom_mode(&msg), CUSTOM_MODE);
CHECK_CHANGED(gathered_telemetry.system_status, mavlink_msg_heartbeat_get_system_status(&msg), SYSTEM_STATUS);
CHECK_CHANGED(gathered_telemetry.mav_type, mavlink_msg_heartbeat_get_type(&msg), SlowParameters::MAV_TYPE);
connection_timer = millis();
if (!is_connected);
{
heartbeat_count++;
if ((heartbeat_count) > 10) { // If received > 10 heartbeats from MavLink then we are connected
is_connected = true;
heartbeat_count = 0;
digitalWrite(alive_led_pin, HIGH); // LED will be ON when connected to MavLink, else it will slowly blink
}
}
}
break;
case MAVLINK_MSG_ID_SYS_STATUS: // 1
gathered_telemetry.battery_voltage = mavlink_msg_sys_status_get_voltage_battery(&msg) / 10; //
gathered_telemetry.battery_current = mavlink_msg_sys_status_get_current_battery(&msg) / 10; //
CHECK_CHANGED(gathered_telemetry.battery_remaining, mavlink_msg_sys_status_get_battery_remaining(&msg), BATTERY_REMAINING);
break;
case MAVLINK_MSG_ID_GPS_RAW_INT: // 24
CHECK_CHANGED(gathered_telemetry.gps_fixtype, mavlink_msg_gps_raw_int_get_fix_type(&msg), GPS_FIX_TYPE_AND_SAT_VISIBLE); // 0 = No GPS, 1 =No Fix, 2 = 2D Fix, 3 = 3D Fix
CHECK_CHANGED(gathered_telemetry.gps_satellites_visible, mavlink_msg_gps_raw_int_get_satellites_visible(&msg), GPS_FIX_TYPE_AND_SAT_VISIBLE); // numbers of visible satelites
if (gathered_telemetry.gps_fixtype >= 3) {
CHECK_CHANGED(gathered_telemetry.gps_hdop, mavlink_msg_gps_raw_int_get_eph(&msg), HDOP_EX); // hdop * 100
gathered_telemetry.gps_latitude = mavlink_msg_gps_raw_int_get_lat(&msg);
gathered_telemetry.gps_longitude = mavlink_msg_gps_raw_int_get_lon(&msg);
gathered_telemetry.gps_altitude = mavlink_msg_gps_raw_int_get_alt(&msg); // 1m =1000
gathered_telemetry.gps_speed = mavlink_msg_gps_raw_int_get_vel(&msg);
}
else {
CHECK_CHANGED(gathered_telemetry.gps_hdop, mavlink_msg_gps_raw_int_get_eph(&msg), HDOP_EX); // hdop * 100
gathered_telemetry.gps_latitude = 0L;
gathered_telemetry.gps_longitude = 0l;
gathered_telemetry.gps_altitude = 0L;
gathered_telemetry.gps_speed = 0L;
}
break;
case MAVLINK_MSG_ID_VFR_HUD: // 74
gathered_telemetry.groundspeed = mavlink_msg_vfr_hud_get_groundspeed(&msg); // 100 = 1m/s
gathered_telemetry.heading = mavlink_msg_vfr_hud_get_heading(&msg); // 100 = 100 deg
gathered_telemetry.throttle = mavlink_msg_vfr_hud_get_throttle(&msg); // 100 = 100%
gathered_telemetry.bar_altitude = mavlink_msg_vfr_hud_get_alt(&msg); // m
gathered_telemetry.climb_rate = mavlink_msg_vfr_hud_get_climb(&msg) * 100; // m/s
break;
case MAVLINK_MSG_ID_STATUSTEXT:
mavlink_msg_statustext_get_text(&msg, gathered_telemetry.text);
break;
case MAVLINK_MSG_ID_ATTITUDE:
gathered_telemetry.pitch = mavlink_msg_attitude_get_pitch(&msg) * 180 / M_PI;
gathered_telemetry.roll = mavlink_msg_attitude_get_roll(&msg) * 180 / M_PI;
break;
default:
break;
}
}
}
}
