src/lib/MAVLink/MAVLink.cpp

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