src/lib/MAVLink/MAVLink.cpp

   1 #include "MAVLink.h"
   2 #if !defined(PLATFORM_STM32)
   3     #include "ardupilot_protocol.h"
   4 #endif
   5
   6 void convert_mavlink_to_crsf_telem(uint8_t *CRSFinBuffer, uint8_t count, Handset *handset)
   7 {
   8 #if !defined(PLATFORM_STM32)
   9     // Store the relative altitude for GPS altitude
  10     static int32_t relative_alt = 0;
  11
  12     for (uint8_t i = 0; i < count; i++)
  13     {
  14         mavlink_message_t msg;
  15         mavlink_status_t status;
  16         bool have_message = mavlink_frame_char(MAVLINK_COMM_0, CRSFinBuffer[CRSF_FRAME_NOT_COUNTED_BYTES + i], &msg, &status);
  17         // convert mavlink messages to CRSF messages
  18         if (have_message)
  19         {
  20             // Only parse heartbeats from the autopilot (not GCS)
  21             if (msg.compid != MAV_COMP_ID_AUTOPILOT1)
  22             {
  23                 continue;
  24             }
  25             switch (msg.msgid)
  26             {
  27             case MAVLINK_MSG_ID_BATTERY_STATUS: {
  28                 mavlink_battery_status_t battery_status;
  29                 mavlink_msg_battery_status_decode(&msg, &battery_status);
  30                 if (battery_status.id != 0) {
  31                     break;
  32                 }
  33                 CRSF_MK_FRAME_T(crsf_sensor_battery_t)
  34                 crsfbatt = {0};
  35                 // mV -> mv*100
  36                 crsfbatt.p.voltage = htobe16(battery_status.voltages[0] / 100);
  37                 // cA -> mA*100
  38                 crsfbatt.p.current = htobe16(battery_status.current_battery / 10);
  39                 crsfbatt.p.capacity = htobe32(battery_status.current_consumed) & 0x0FFF;
  40                 crsfbatt.p.remaining = battery_status.battery_remaining;
  41                 CRSF::SetHeaderAndCrc((uint8_t *)&crsfbatt, CRSF_FRAMETYPE_BATTERY_SENSOR, CRSF_FRAME_SIZE(sizeof(crsf_sensor_battery_t)), CRSF_ADDRESS_CRSF_TRANSMITTER);
  42                 handset->sendTelemetryToTX((uint8_t *)&crsfbatt);
  43                 break;
  44             }
  45             case MAVLINK_MSG_ID_GPS_RAW_INT: {
  46                 mavlink_gps_raw_int_t gps_int;
  47                 mavlink_msg_gps_raw_int_decode(&msg, &gps_int);
  48                 CRSF_MK_FRAME_T(crsf_sensor_gps_t)
  49                 crsfgps = {0};
  50 // We use altitude relative to home for GPS altitude, by default, but we can also use GPS altitude if USE_MAVLINK_GPS_ALTITUDE is defined
  51 #if defined(USE_MAVLINK_GPS_ALTITUDE)
  52                 // mm -> meters + 1000
  53                 crsfgps.p.altitude = htobe16(gps_int.alt / 1000 + 1000);
  54 #else
  55                 crsfgps.p.altitude = htobe16((uint16_t)(relative_alt / 1000 + 1000));
  56 #endif
  57                 // cm/s -> km/h / 10
  58                 crsfgps.p.groundspeed = htobe16(gps_int.vel * 36 / 100);
  59                 crsfgps.p.latitude = htobe32(gps_int.lat);
  60                 crsfgps.p.longitude = htobe32(gps_int.lon);
  61                 crsfgps.p.gps_heading = htobe16(gps_int.cog);
  62                 crsfgps.p.satellites_in_use = gps_int.satellites_visible;
  63                 CRSF::SetHeaderAndCrc((uint8_t *)&crsfgps, CRSF_FRAMETYPE_GPS, CRSF_FRAME_SIZE(sizeof(crsf_sensor_gps_t)), CRSF_ADDRESS_CRSF_TRANSMITTER);
  64                 handset->sendTelemetryToTX((uint8_t *)&crsfgps);
  65                 break;
  66             }
  67             case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
  68                 mavlink_global_position_int_t global_pos;
  69                 mavlink_msg_global_position_int_decode(&msg, &global_pos);
  70                 CRSF_MK_FRAME_T(crsf_sensor_vario_t)
  71                 crsfvario = {0};
  72                 // store relative altitude for GPS Alt so we don't have 2 Alt sensors
  73                 relative_alt = global_pos.relative_alt;
  74                 crsfvario.p.verticalspd = htobe16(-global_pos.vz); // MAVLink vz is positive down
  75                 CRSF::SetHeaderAndCrc((uint8_t *)&crsfvario, CRSF_FRAMETYPE_VARIO, CRSF_FRAME_SIZE(sizeof(crsf_sensor_vario_t)), CRSF_ADDRESS_CRSF_TRANSMITTER);
  76                 handset->sendTelemetryToTX((uint8_t *)&crsfvario);
  77                 break;
  78             }
  79             case MAVLINK_MSG_ID_ATTITUDE: {
  80                 mavlink_attitude_t attitude;
  81                 mavlink_msg_attitude_decode(&msg, &attitude);
  82                 CRSF_MK_FRAME_T(crsf_sensor_attitude_t)
  83                 crsfatt = {0};
  84                 crsfatt.p.pitch = htobe16(attitude.pitch * 10000); // in Betaflight & INAV, CRSF positive pitch is nose down, but in Ardupilot, it's nose up - we follow Ardupilot
  85                 crsfatt.p.roll = htobe16(attitude.roll * 10000);
  86                 crsfatt.p.yaw = htobe16(attitude.yaw * 10000);
  87                 CRSF::SetHeaderAndCrc((uint8_t *)&crsfatt, CRSF_FRAMETYPE_ATTITUDE, CRSF_FRAME_SIZE(sizeof(crsf_sensor_attitude_t)), CRSF_ADDRESS_CRSF_TRANSMITTER);
  88                 handset->sendTelemetryToTX((uint8_t *)&crsfatt);
  89                 break;
  90             }
  91             case MAVLINK_MSG_ID_HEARTBEAT: {
  92                 mavlink_heartbeat_t heartbeat;
  93                 mavlink_msg_heartbeat_decode(&msg, &heartbeat);
  94                 CRSF_MK_FRAME_T(crsf_flight_mode_t)
  95                 crsffm = {0};
  96                 ap_flight_mode_name4(crsffm.p.flight_mode, ap_vehicle_from_mavtype(heartbeat.type), heartbeat.custom_mode);
  97                 // if we have a good flight mode, and we're armed, suffix the flight mode with a * - see Ardupilot's AP_CRSF_Telem::calc_flight_mode()
  98                 size_t len = strnlen(crsffm.p.flight_mode, sizeof(crsffm.p.flight_mode));
  99                 if (len > 0 && (len + 1 < sizeof(crsffm.p.flight_mode)) && (heartbeat.base_mode & MAV_MODE_FLAG_SAFETY_ARMED)) {
 100                     crsffm.p.flight_mode[len] = '*';
 101                     crsffm.p.flight_mode[len + 1] = '\0';
 102                 }
 103                 CRSF::SetHeaderAndCrc((uint8_t *)&crsffm, CRSF_FRAMETYPE_FLIGHT_MODE, CRSF_FRAME_SIZE(sizeof(crsffm)), CRSF_ADDRESS_CRSF_TRANSMITTER);
 104                 handset->sendTelemetryToTX((uint8_t *)&crsffm);
 105                 break;
 106             }
 107             }
 108         }
 109     }
 110 #endif
 111 }
 112
 113 bool isThisAMavPacket(uint8_t *buffer, uint16_t bufferSize)
 114 {
 115 #if !defined(PLATFORM_STM32)
 116     for (uint8_t i = 0; i < bufferSize; ++i)
 117     {
 118         uint8_t c = buffer[i];
 119
 120         mavlink_message_t msg;
 121         mavlink_status_t status;
 122
 123         // Try parse a mavlink message
 124         if (mavlink_frame_char(MAVLINK_COMM_0, c, &msg, &status))
 125         {
 126             // Message decoded successfully
 127             return true;
 128         }
 129     }
 130 #endif
 131     return false;
 132 }
 133
 134 uint16_t buildMAVLinkELRSModeChange(uint8_t mode, uint8_t *buffer)
 135 {
 136 #if !defined(PLATFORM_STM32)
 137     constexpr uint8_t ELRS_MODE_CHANGE = 0x8;
 138     mavlink_command_int_t commandMsg;
 139     commandMsg.target_system = 255;
 140     commandMsg.target_component = MAV_COMP_ID_UDP_BRIDGE;
 141     commandMsg.command = MAV_CMD_USER_1;
 142     commandMsg.param1 = ELRS_MODE_CHANGE;
 143     commandMsg.param2 = mode;
 144     mavlink_message_t msg;
 145     mavlink_msg_command_int_encode(255, MAV_COMP_ID_TELEMETRY_RADIO, &msg, &commandMsg);
 146     uint16_t len = mavlink_msg_to_send_buffer(buffer, &msg);
 147     return len;
 148 #else
 149     return 0;
 150 #endif
 151 }