Skip to content

Commit

Permalink
Cleanup, and add specific targets for crossbow
Browse files Browse the repository at this point in the history
  • Loading branch information
wvarty committed Aug 7, 2024
1 parent 6bb37f9 commit 8d8599d
Show file tree
Hide file tree
Showing 10 changed files with 389 additions and 264 deletions.
29 changes: 29 additions & 0 deletions hardware/targets.json
Original file line number Diff line number Diff line change
Expand Up @@ -548,5 +548,34 @@
"platform": "esp8285"
}
}
},
"mfd-crossbow": {
"name": "MFD Crossbow Antenna Tracker Backpack",
"vrx": {
"esp82": {
"product_name": "Generic ESP8285 Receiver",
"firmware": "MFD_Crossbow_ESP8285_Backpack",
"upload_methods": ["uart", "wifi"],
"platform": "esp8285"
},
"esp32": {
"product_name": "Generic ESP32 Receiver",
"firmware": "MFD_Crossbow_ESP32_Backpack",
"upload_methods": ["uart", "wifi"],
"platform": "esp32"
},
"esp32c3": {
"product_name": "Generic ESP32C3 Receiver",
"firmware": "MFD_Crossbow_ESP32C3_Backpack",
"upload_methods": ["uart", "wifi"],
"platform": "esp32-c3"
},
"esp32s3": {
"product_name": "Generic ESP32S3 Receiver",
"firmware": "MFD_Crossbow_ESP32S3_Backpack",
"upload_methods": ["uart", "wifi"],
"platform": "esp32-s3"
}
}
}
}
2 changes: 1 addition & 1 deletion lib/MSP/msp.h
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
// dynamically allocate array length based on the payload size
// Hardcoding payload size to 64 bytes for now, to allow enough space
// for custom OSD text.
#define MSP_PORT_INBUF_SIZE 128
#define MSP_PORT_INBUF_SIZE 64

#define CHECK_PACKET_PARSING() \
if (packet->readError) {\
Expand Down
21 changes: 1 addition & 20 deletions src/Vrx_main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -74,8 +74,6 @@ bool gotInitialPacket = false;
bool headTrackingEnabled = false;
uint32_t lastSentRequest = 0;

uint32_t lastSentGPS = 0;

device_t *ui_devices[] = {
#ifdef PIN_LED
&LED_device,
Expand Down Expand Up @@ -104,9 +102,6 @@ MSP msp;
ELRS_EEPROM eeprom;
VrxBackpackConfig config;

uint8_t cachedGPS[64];
uint8_t cachedLen = 0;

#ifdef RAPIDFIRE_BACKPACK
Rapidfire vrxModule;
#elif defined(RX5808_BACKPACK)
Expand Down Expand Up @@ -535,24 +530,10 @@ void loop()
if (!gotInitialPacket && now - VRX_BOOT_DELAY < 5000 && now - lastSentRequest > 1000 && connectionState != binding)
{
DBGLN("RequestVTXPacket...");
// RequestVTXPacket();
RequestVTXPacket();
lastSentRequest = now;
}

// spam out GPS msgs
if (cachedLen > 0 && now - lastSentGPS > 100 && connectionState != binding)
{
DBGLN("Send cached GPS...");
Serial.write(cachedGPS, cachedLen);
lastSentGPS = now;
DBGLN("*** GPS CACHE ***");
for(int i = 0; i < cachedLen; i++)
{
DBG("%x,", cachedGPS[i]); // Debug prints
}
DBGLN(""); // Extra line for serial output readability
}

#if !defined(NO_AUTOBIND)
// Power cycle must be done within 30s. Long timeout to allow goggles to boot and shutdown correctly e.g. Orqa.
if (now > BINDING_TIMEOUT && config.GetBootCount() > 0)
Expand Down
40 changes: 8 additions & 32 deletions src/mfd_crossbow.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,31 +12,17 @@ uint8_t system_state = 4; // 0 = unknown, 3 = standby, 4 = active
uint32_t upTime = 0; // System uptime, usually set to 0 for cases where it doesn't matter

// Flight parameters
float roll = 0; // Roll angle in degrees
float pitch = 0; // Pitch angle in degrees
float yaw = 0; // Yaw angle in degrees
int16_t heading = 0; // Geographical heading angle in degrees
float lat = 0.0; // GPS latitude in degrees (example: 47.123456)
float lon = 0.0; // GPS longitude in degrees
float alt = 0.0; // Relative flight altitude in m
float lat_temp = 0.0; //Temp values for GPS coords
float lon_temp = 0.0;
float groundspeed = 0.0; // Groundspeed in m/s
float airspeed = 0.0; // Airspeed in m/s
float climbrate = 0.0; // Climb rate in m/s, currently not working
float throttle = 0.0; // Throttle percentage

// GPS parameters
int16_t gps_sats = 0; // Number of visible GPS satellites
int32_t gps_alt = 0.0; // GPS altitude (Altitude above MSL)
float gps_hdop = 100.0; // GPS HDOP
uint8_t fixType = 0; // GPS fix type. 0-1: no fix, 2: 2D fix, 3: 3D fix

// Battery parameters
float battery_remaining = 0.0; // Remaining battery percentage
float voltage_battery = 0.0; // Battery voltage in V
float current_battery = 0.0; // Battery current in A

uint8_t fixType = 3; // GPS fix type. 0-1: no fix, 2: 2D fix, 3: 3D fix

void
MFDCrossbow::send_heartbeat(uint8_t system_id, uint8_t component_id, uint8_t system_type, uint8_t autopilot_type, uint8_t system_mode, uint32_t custom_mode, uint8_t system_state)
Expand All @@ -46,7 +32,7 @@ MFDCrossbow::send_heartbeat(uint8_t system_id, uint8_t component_id, uint8_t sys
uint8_t buf[MAVLINK_MAX_PACKET_LEN];

// Pack the message
mavlink_msg_heartbeat_pack(system_id,component_id, &msg, system_type, autopilot_type, system_mode, custom_mode, system_state);
mavlink_msg_heartbeat_pack(system_id, component_id, &msg, system_type, autopilot_type, system_mode, custom_mode, system_state);

// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
Expand Down Expand Up @@ -80,6 +66,7 @@ MFDCrossbow::send_gps_raw_int(int8_t system_id, int8_t component_id, int32_t upT
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

// Send the message
m_port->write(buf, len);
}

Expand Down Expand Up @@ -112,29 +99,18 @@ MFDCrossbow::SendGpsTelemetry(crsf_packet_gps_t *packet)
lat = static_cast<double>(rawLat);
lon = static_cast<double>(rawLon);

// Convert from CRSF scales to mavlink scales
groundspeed = be16toh(packet->p.speed) / 36.0 * 100.0;
heading = be16toh(packet->p.heading);
alt = (be16toh(packet->p.altitude) - 1000) * 1000.0;
gps_sats = packet->p.satcnt;

// m_port->println("GPS lat: ");
// m_port->println(lat);
// m_port->println("GPS lon: ");
// m_port->println(lon);
// m_port->println("GPS speed: ");
// m_port->println(groundspeed);
// m_port->println("GPS heading: ");
// m_port->println(heading);
// m_port->println("GPS altitude: ");
// m_port->println(alt);
// m_port->println("GPS satellites: ");
// m_port->println(gps_sats);

// Send heartbeat and GPS mavlink messages to the tracker
send_heartbeat(system_id, component_id, system_type, autopilot_type, system_mode, custom_mode, system_state);
send_gps_raw_int(system_id, component_id, upTime, fixType, lat, lon, alt, gps_alt, heading, groundspeed, gps_hdop, gps_sats);
send_global_position_int(system_id, component_id, upTime, lat, lon, alt, gps_alt, heading);

// We have received gps values, so we are ok to spam these out at 10hz
// Log the last time we received new GPS coords
gpsLastUpdated = millis();
}

Expand All @@ -143,7 +119,7 @@ MFDCrossbow::Loop(uint32_t now)
{
ModuleBase::Loop(now);

// If the GPS has been updated in the last 10 seconds, keep spamming it out at 10hz
// If the GPS data is <= 10 seconds old, keep spamming it out at 10hz
bool gpsIsValid = (now < gpsLastUpdated + 10000) && gps_sats > 0;

if (now > gpsLastSent + 100 && gpsIsValid)
Expand All @@ -154,4 +130,4 @@ MFDCrossbow::Loop(uint32_t now)

gpsLastSent = now;
}
}
}
Loading

0 comments on commit 8d8599d

Please sign in to comment.