diff --git a/include/common.h b/include/common.h index 8818ade9..a6929cfc 100644 --- a/include/common.h +++ b/include/common.h @@ -11,3 +11,4 @@ typedef enum extern connectionState_e connectionState; extern unsigned long bindingStart; +static const uint8_t bindingAddress[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; \ No newline at end of file diff --git a/lib/CRSF/CRSF.cpp b/lib/CRSF/CRSF.cpp new file mode 100644 index 00000000..9c847b01 --- /dev/null +++ b/lib/CRSF/CRSF.cpp @@ -0,0 +1,15 @@ +#include "CRSF.h" +#include + +GENERIC_CRC8 crsf_crc(CRSF_CRC_POLY); + +void CRSF::SetHeaderAndCrc(uint8_t *frame, crsf_frame_type_e frameType, uint8_t frameSize, crsf_addr_e destAddr) +{ + auto *header = (crsf_header_t *)frame; + header->sync_byte = destAddr; + header->frame_size = frameSize; + header->type = frameType; + + uint8_t crc = crsf_crc.calc(&frame[CRSF_FRAME_NOT_COUNTED_BYTES], frameSize - 1, 0); + frame[frameSize + CRSF_FRAME_NOT_COUNTED_BYTES - 1] = crc; +} \ No newline at end of file diff --git a/lib/CRSF/CRSF.h b/lib/CRSF/CRSF.h new file mode 100644 index 00000000..8205e3cb --- /dev/null +++ b/lib/CRSF/CRSF.h @@ -0,0 +1,8 @@ +#include +#include "crsf_protocol.h" + +class CRSF { +public: + static void SetHeaderAndCrc(uint8_t *frame, crsf_frame_type_e frameType, uint8_t frameSize, crsf_addr_e destAddr); +}; + diff --git a/lib/CrsfProtocol/crsf_protocol.h b/lib/CrsfProtocol/crsf_protocol.h index 856b877a..564b091a 100644 --- a/lib/CrsfProtocol/crsf_protocol.h +++ b/lib/CrsfProtocol/crsf_protocol.h @@ -2,10 +2,7 @@ #define CRSF_CRC_POLY 0xd5 #define CRSF_SYNC_BYTE 0xc8 - -#define CRSF_FRAMETYPE_GPS 0x02 -#define CRSF_FRAMETYPE_LINK_STATISTICS 0x14 -#define CRSF_FRAMETYPE_BATTERY_SENSOR 0x08 +#define CRSF_FRAME_NOT_COUNTED_BYTES 2 #define PACKED __attribute__((packed)) @@ -19,6 +16,57 @@ typedef struct crsf_header_s uint8_t type; // from crsf_frame_type_e } PACKED crsf_header_t; +typedef enum : uint8_t +{ + CRSF_FRAMETYPE_GPS = 0x02, + CRSF_FRAMETYPE_VARIO = 0x07, + CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, + CRSF_FRAMETYPE_BARO_ALTITUDE = 0x09, + CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, + CRSF_FRAMETYPE_OPENTX_SYNC = 0x10, + CRSF_FRAMETYPE_RADIO_ID = 0x3A, + CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, + CRSF_FRAMETYPE_ATTITUDE = 0x1E, + CRSF_FRAMETYPE_FLIGHT_MODE = 0x21, + // Extended Header Frames, range: 0x28 to 0x96 + CRSF_FRAMETYPE_DEVICE_PING = 0x28, + CRSF_FRAMETYPE_DEVICE_INFO = 0x29, + CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY = 0x2B, + CRSF_FRAMETYPE_PARAMETER_READ = 0x2C, + CRSF_FRAMETYPE_PARAMETER_WRITE = 0x2D, + + //CRSF_FRAMETYPE_ELRS_STATUS = 0x2E, ELRS good/bad packet count and status flags + + CRSF_FRAMETYPE_COMMAND = 0x32, + // KISS frames + CRSF_FRAMETYPE_KISS_REQ = 0x78, + CRSF_FRAMETYPE_KISS_RESP = 0x79, + // MSP commands + CRSF_FRAMETYPE_MSP_REQ = 0x7A, // response request using msp sequence as command + CRSF_FRAMETYPE_MSP_RESP = 0x7B, // reply with 58 byte chunked binary + CRSF_FRAMETYPE_MSP_WRITE = 0x7C, // write with 8 byte chunked binary (OpenTX outbound telemetry buffer limit) + // Ardupilot frames + CRSF_FRAMETYPE_ARDUPILOT_RESP = 0x80, +} crsf_frame_type_e; + +typedef enum : uint8_t +{ + CRSF_ADDRESS_BROADCAST = 0x00, + CRSF_ADDRESS_USB = 0x10, + CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x80, + CRSF_ADDRESS_RESERVED1 = 0x8A, + CRSF_ADDRESS_CURRENT_SENSOR = 0xC0, + CRSF_ADDRESS_GPS = 0xC2, + CRSF_ADDRESS_TBS_BLACKBOX = 0xC4, + CRSF_ADDRESS_FLIGHT_CONTROLLER = 0xC8, + CRSF_ADDRESS_RESERVED2 = 0xCA, + CRSF_ADDRESS_RACE_TAG = 0xCC, + CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA, + CRSF_ADDRESS_CRSF_RECEIVER = 0xEC, + CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE, + CRSF_ADDRESS_ELRS_LUA = 0xEF +} crsf_addr_e; + #define CRSF_MK_FRAME_T(payload) struct payload##_frame_s { crsf_header_t h; payload p; uint8_t crc; } PACKED typedef struct crsf_sensor_gps_s { diff --git a/lib/ESPNOW/ESPNOW_Helpers.cpp b/lib/ESPNOW/ESPNOW_Helpers.cpp new file mode 100644 index 00000000..d62bea41 --- /dev/null +++ b/lib/ESPNOW/ESPNOW_Helpers.cpp @@ -0,0 +1,35 @@ +#include "ESPNOW_Helpers.h" +#include "espnow.h" +#include +#include +#include "devLED.h" + +extern MSP msp; +extern connectionState_e connectionState; // from Vrx_main.cpp + +void ESPNOW::sendMSPViaEspnow(mspPacket_t *packet) +{ + // Do not send while in binding mode. The currently used firmwareOptions.uid may be garbage. + if (connectionState == binding) + return; + + uint8_t packetSize = msp.getTotalPacketSize(packet); + uint8_t nowDataOutput[packetSize]; + + uint8_t result = msp.convertToByteArray(packet, nowDataOutput); + + if (!result) + { + // packet could not be converted to array, bail out + return; + } + if (packet->function == MSP_ELRS_BIND) + { + esp_now_send((uint8_t*)bindingAddress, (uint8_t *) &nowDataOutput, packetSize); // Send Bind packet with the broadcast address + } + else + { + esp_now_send(firmwareOptions.uid, (uint8_t *) &nowDataOutput, packetSize); + } + blinkLED(); +} \ No newline at end of file diff --git a/lib/ESPNOW/ESPNOW_Helpers.h b/lib/ESPNOW/ESPNOW_Helpers.h new file mode 100644 index 00000000..48768280 --- /dev/null +++ b/lib/ESPNOW/ESPNOW_Helpers.h @@ -0,0 +1,7 @@ +#include "msp.h" +#include "msptypes.h" + +class ESPNOW { +public: + static void sendMSPViaEspnow(mspPacket_t *packet); +}; \ No newline at end of file diff --git a/lib/MAVLink/MAVLink.cpp b/lib/MAVLink/MAVLink.cpp index 2203ce29..56a536af 100644 --- a/lib/MAVLink/MAVLink.cpp +++ b/lib/MAVLink/MAVLink.cpp @@ -2,6 +2,11 @@ #include #include "MAVLink.h" #include +#include +#include +#include +#include +#include void MAVLink::ProcessMAVLinkFromTX(uint8_t c) @@ -40,6 +45,35 @@ MAVLink::ProcessMAVLinkFromTX(uint8_t c) mavlink_to_gcs_buf[mavlink_to_gcs_buf_count] = msg; mavlink_to_gcs_buf_count++; mavlink_stats.packets_downlink++; + + // Look for GPS packets - convert them to CRSF + if (msg.msgid == MAVLINK_MSG_ID_GPS_RAW_INT) + { + mavlink_gps_raw_int_t gps_int; + mavlink_msg_gps_raw_int_decode(&msg, &gps_int); + CRSF_MK_FRAME_T(crsf_sensor_gps_t) crsfgps = {0}; + + crsfgps.p.speed = htobe16(gps_int.vel * 36 / 100); + crsfgps.p.lat = htobe32(gps_int.lat); + crsfgps.p.lon = htobe32(gps_int.lon); + crsfgps.p.heading = htobe16(gps_int.cog); + crsfgps.p.satcnt = gps_int.satellites_visible; + crsfgps.p.altitude = htobe16(gps_int.alt / 1000 + 1000); + + CRSF::SetHeaderAndCrc((uint8_t *)&crsfgps, CRSF_FRAMETYPE_GPS, sizeof(crsf_sensor_gps_t), CRSF_ADDRESS_CRSF_TRANSMITTER); + + // Wrap in MSP + mspPacket_t packet; + packet.reset(); + packet.makeCommand(); + packet.function = MSP_ELRS_BACKPACK_CRSF_TLM; + for (size_t i = 0; i < sizeof(crsfgps); i++) + { + packet.addByte(((uint8_t *)&crsfgps)[i]); + } + // Send it out ESPNOW + ESPNOW::sendMSPViaEspnow(&packet); + } } } diff --git a/src/Tx_main.cpp b/src/Tx_main.cpp index 6748794a..f3017127 100644 --- a/src/Tx_main.cpp +++ b/src/Tx_main.cpp @@ -11,6 +11,7 @@ #include "msp.h" #include "msptypes.h" +#include "ESPNOW_Helpers.h" #include "logging.h" #include "config.h" #include "common.h" @@ -28,8 +29,6 @@ /////////// GLOBALS /////////// -uint8_t bindingAddress[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; - const uint8_t version[] = {LATEST_VERSION}; connectionState_e connectionState = starting; @@ -61,11 +60,6 @@ mspPacket_t cachedHTPacket; MAVLink mavlink; #endif -/////////// FUNCTION DEFS /////////// - -void sendMSPViaEspnow(mspPacket_t *packet); - -///////////////////////////////////// #if defined(PLATFORM_ESP32) // This seems to need to be global, as per this page, @@ -200,7 +194,7 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet) DBG(""); // Extra line for serial output readability config.Commit(); // delay(500); // delay may not be required - sendMSPViaEspnow(packet); + ESPNOW::sendMSPViaEspnow(packet); // delay(500); // delay may not be required rebootTime = millis(); // restart to set SetSoftMACAddress } @@ -212,11 +206,11 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet) cachedVTXPacket = *packet; cacheFull = true; // transparently forward MSP packets via espnow to any subscribers - sendMSPViaEspnow(packet); + ESPNOW::sendMSPViaEspnow(packet); break; case MSP_ELRS_SET_VRX_BACKPACK_WIFI_MODE: DBGLN("Processing MSP_ELRS_SET_VRX_BACKPACK_WIFI_MODE..."); - sendMSPViaEspnow(packet); + ESPNOW::sendMSPViaEspnow(packet); break; case MSP_ELRS_SET_TX_BACKPACK_WIFI_MODE: DBGLN("Processing MSP_ELRS_SET_TX_BACKPACK_WIFI_MODE..."); @@ -230,13 +224,13 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet) DBGLN("Processing MSP_ELRS_BACKPACK_SET_HEAD_TRACKING..."); cachedHTPacket = *packet; cacheFull = true; - sendMSPViaEspnow(packet); + ESPNOW::sendMSPViaEspnow(packet); break; case MSP_ELRS_BACKPACK_CRSF_TLM: DBGLN("Processing MSP_ELRS_BACKPACK_CRSF_TLM..."); if (config.GetTelemMode() != BACKPACK_TELEM_MODE_OFF) { - sendMSPViaEspnow(packet); + ESPNOW::sendMSPViaEspnow(packet); } break; case MSP_ELRS_BACKPACK_CONFIG: @@ -245,36 +239,11 @@ void ProcessMSPPacketFromTX(mspPacket_t *packet) break; default: // transparently forward MSP packets via espnow to any subscribers - sendMSPViaEspnow(packet); + ESPNOW::sendMSPViaEspnow(packet); break; } } -void sendMSPViaEspnow(mspPacket_t *packet) -{ - uint8_t packetSize = msp.getTotalPacketSize(packet); - uint8_t nowDataOutput[packetSize]; - - uint8_t result = msp.convertToByteArray(packet, nowDataOutput); - - if (!result) - { - // packet could not be converted to array, bail out - return; - } - - if (packet->function == MSP_ELRS_BIND) - { - esp_now_send(bindingAddress, (uint8_t *) &nowDataOutput, packetSize); // Send Bind packet with the broadcast address - } - else - { - esp_now_send(firmwareOptions.uid, (uint8_t *) &nowDataOutput, packetSize); - } - - blinkLED(); -} - void SendCachedMSP() { if (!cacheFull) @@ -285,11 +254,11 @@ void SendCachedMSP() if (cachedVTXPacket.type != MSP_PACKET_UNKNOWN) { - sendMSPViaEspnow(&cachedVTXPacket); + ESPNOW::sendMSPViaEspnow(&cachedVTXPacket); } if (cachedHTPacket.type != MSP_PACKET_UNKNOWN) { - sendMSPViaEspnow(&cachedHTPacket); + ESPNOW::sendMSPViaEspnow(&cachedHTPacket); } } diff --git a/src/Vrx_main.cpp b/src/Vrx_main.cpp index 7215b6b9..35768394 100644 --- a/src/Vrx_main.cpp +++ b/src/Vrx_main.cpp @@ -12,6 +12,7 @@ #include "msp.h" #include "msptypes.h" +#include "ESPNOW_Helpers.h" #include "logging.h" #include "helpers.h" #include "common.h" @@ -125,7 +126,6 @@ VrxBackpackConfig config; /////////// FUNCTION DEFS /////////// void ProcessMSPPacket(mspPacket_t *packet); -void sendMSPViaEspnow(mspPacket_t *packet); void resetBootCounter(); void SetupEspNow(); @@ -340,30 +340,10 @@ void RequestVTXPacket() packet.addByte(0); // empty byte blinkLED(); - sendMSPViaEspnow(&packet); + ESPNOW::sendMSPViaEspnow(&packet); #endif } -void sendMSPViaEspnow(mspPacket_t *packet) -{ - // Do not send while in binding mode. The currently used firmwareOptions.uid may be garbage. - if (connectionState == binding) - return; - - uint8_t packetSize = msp.getTotalPacketSize(packet); - uint8_t nowDataOutput[packetSize]; - - uint8_t result = msp.convertToByteArray(packet, nowDataOutput); - - if (!result) - { - // packet could not be converted to array, bail out - return; - } - - esp_now_send(firmwareOptions.uid, (uint8_t *) &nowDataOutput, packetSize); -} - void resetBootCounter() { config.SetBootCount(0);