src/main/tracker/mavlink.c

   1 /*
   2  * This file is part of u360gts, aka amv-open360tracker 32bits:
   3  * https://github.com/raul-ortega/amv-open360tracker-32bits
   4  *
   5  * The code below is an adaptation by Raúl Ortega of the original code written by Samuel Brucksch:
   6  * https://github.com/SamuelBrucksch/open360tracker
   7  *
   8  * u360gts is free software: you can redistribute it and/or modify
   9  * it under the terms of the GNU General Public License as published by
  10  * the Free Software Foundation, either version 3 of the License, or
  11  * (at your option) any later version.
  12  *
  13  * u360gts is distributed in the hope that it will be useful,
  14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16  * GNU General Public License for more details.
  17  *
  18  * You should have received a copy of the GNU General Public License
  19  * along with u360gts.  If not, see <http://www.gnu.org/licenses/>.
  20  *
  21  */
  22
  23 #include "config.h"
  24 #include "telemetry.h"
  25 #include "telemetry/mavlink/common/mavlink.h"
  26 #include "Arduino.h"
  27
  28 #define MAVLINK_MAX_PAYLOAD_LEN 36
  29
  30 void mavlink_handleMessage(mavlink_message_t* msg) {
  31   switch (msg->msgid) {
  32     case MAVLINK_MSG_ID_GPS_RAW_INT:
  33       telemetry_lat = mavlink_msg_gps_raw_int_get_lat(msg) / 10;
  34       telemetry_lon = mavlink_msg_gps_raw_int_get_lon(msg) / 10;
  35       telemetry_sats = mavlink_msg_gps_raw_int_get_satellites_visible(msg);
  36       telemetry_alt = (int16_t)(mavlink_msg_gps_raw_int_get_alt(msg) / 1000);
  37       telemetry_fixtype = mavlink_msg_gps_raw_int_get_fix_type(msg);
  38       telemetry_speed = mavlink_msg_gps_raw_int_get_vel(msg) / 100;
  39       gotAlt = true;
  40
  41       // fix_type: GPS lock 0-1=no fix, 2=2D, 3=3D
  42       if (telemetry_fixtype > 1) {
  43         gotFix = true;
  44       } else {
  45         gotFix = false;
  46       }
  47       break;
  48     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
  49       telemetry_lat = mavlink_msg_global_position_int_get_lat(msg) / 10;
  50       telemetry_lon = mavlink_msg_global_position_int_get_lon(msg) / 10;
  51       telemetry_alt = (int16_t)(mavlink_msg_global_position_int_get_alt(msg) / 1000);
  52       gotAlt = true;
  53
  54       if (telemetry_lat != 0 || telemetry_lon != 0) {
  55         gotFix = true;
  56       } else {
  57         gotFix = false;
  58       }
  59       break;
  60     case MAVLINK_MSG_ID_ATTITUDE:
  61                 {
  62                         telemetry_pitch = mavlink_msg_attitude_get_pitch(msg);
  63                         telemetry_roll = mavlink_msg_attitude_get_roll(msg);
  64                         telemetry_yaw = mavlink_msg_attitude_get_yaw(msg);
  65                         telemetry_course = degrees(telemetry_yaw);
  66                 }
  67                 break;
  68   }
  69 }
  70
  71 void mavlink_encodeTargetData(uint8_t c) {
  72   mavlink_status_t status;
  73   mavlink_message_t msg;
  74   if (mavlink_parse_char(0, c, &msg, &status))
  75     mavlink_handleMessage(&msg);
  76   else
  77         telemetry_failed_cs += status.packet_rx_drop_count;
  78 }
  79