Skip to content

Add conservative multicast mode for location sharing #12

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
shiv-tyagi wants to merge 2 commits into from
Closed

Add conservative multicast mode for location sharing #12

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
78f81a8
add multicast functionality
shiv-tyagi Jul 24, 2023
90fc067
add conservative multicast mode for location sharing
shiv-tyagi Jul 24, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 21 additions & 0 deletions src/mavesp8266.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -201,3 +201,24 @@ void MavESP8266Bridge::handle_non_mavlink(uint8_t b, bool msgReceived)
}
_last_parse_state = _rxstatus.parse_state;
}

/*
helper to check if a message is restricted to pass through the bridge
*/
bool MavESP8266Bridge::restricted_message(mavlink_message_t* message)
{
if (getWorld()->getParameters()->getWifiCastMode() != CAST_MODE_CONS_MULTI) {
// restrict messages only in conservative multicast mode
return false;
}

// allowed messages
switch(message->msgid) {
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
case MAVLINK_MSG_ID_FOLLOW_TARGET:
return false;
}

// restrict all other messages
return true;
}
1 change: 1 addition & 0 deletions src/mavesp8266.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -121,6 +121,7 @@ class MavESP8266Bridge {
mavlink_status_t _rxstatus;

void handle_non_mavlink(uint8_t b, bool msgReceived);
bool restricted_message(mavlink_message_t* message);
uint8_t _non_mavlink_buffer[270];
uint16_t _non_mavlink_len;
mavlink_parse_state_t _last_parse_state;
Expand Down
105 changes: 64 additions & 41 deletions src/mavesp8266_gcs.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,11 +55,21 @@ void
MavESP8266GCS::begin(MavESP8266Bridge* forwardTo, IPAddress gcsIP)
{
MavESP8266Bridge::begin(forwardTo);
_ip = gcsIP;

//-- Init variables that shouldn't change unless we reboot
_udp_port = getWorld()->getParameters()->getWifiUdpHport();
//-- Start UDP
_udp.begin(getWorld()->getParameters()->getWifiUdpCport());
switch(getWorld()->getParameters()->getWifiCastMode()) {
case CAST_MODE_UNI:
_ip = gcsIP;
_udp_port = getWorld()->getParameters()->getWifiUdpHport();
_udp.begin(getWorld()->getParameters()->getWifiUdpCport());
break;
case CAST_MODE_FULL_MULTI ... CAST_MODE_CONS_MULTI:
_ip = getWorld()->getParameters()->getWifiMcastIP();
_udp_port = getWorld()->getParameters()->getWifiMcastPort();
_udp.beginMulticast(WiFi.localIP(), _ip, _udp_port);
break;
}
}

//---------------------------------------------------------------------------------
Expand All @@ -70,7 +80,9 @@ MavESP8266GCS::readMessage()
//-- Read UDP
if(_readMessage()) {
//-- If we have a message, forward it
_forwardTo->sendMessage(&_message);
if(!restricted_message(&_message)) {
_forwardTo->sendMessage(&_message);
}
memset(&_message, 0, sizeof(_message));
}
uint32_t now = millis();
Expand Down Expand Up @@ -120,48 +132,50 @@ MavESP8266GCS::_readMessage()
if(msgReceived) {
//-- We no longer need to broadcast
_status.packets_received++;
if(_ip[3] == 255) {
_ip = _udp.remoteIP();
getWorld()->getLogger()->log("Response from GCS. Setting GCS IP to: %s\n", _ip.toString().c_str());
}
//-- First packets
if(!_heard_from) {
if(_message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
//-- We no longer need DHCP
if(getWorld()->getParameters()->getWifiMode() == WIFI_MODE_AP) {
wifi_softap_dhcps_stop();
if(getWorld()->getParameters()->getWifiCastMode() == CAST_MODE_UNI) {
if (_ip[3] == 255) {
_ip = _udp.remoteIP();
getWorld()->getLogger()->log("Response from GCS. Setting GCS IP to: %s\n", _ip.toString().c_str());
}
//-- First packets
if(!_heard_from) {
if(_message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
//-- We no longer need DHCP
if(getWorld()->getParameters()->getWifiMode() == WIFI_MODE_AP) {
wifi_softap_dhcps_stop();
}
_heard_from = true;
_system_id = _message.sysid;
_component_id = _message.compid;
_seq_expected = _message.seq + 1;
_last_heartbeat = millis();
}
_heard_from = true;
_system_id = _message.sysid;
_component_id = _message.compid;
_seq_expected = _message.seq + 1;
_last_heartbeat = millis();
} else {
if(_message.msgid == MAVLINK_MSG_ID_HEARTBEAT)
_last_heartbeat = millis();
_checkLinkErrors(&_message);
}
} else {
if(_message.msgid == MAVLINK_MSG_ID_HEARTBEAT)
_last_heartbeat = millis();
_checkLinkErrors(&_message);
}

if (msgReceived == MAVLINK_FRAMING_BAD_CRC) {
// we don't process messages locally with bad CRC,
// but we do forward them, so when new messages
// are added we can bridge them
break;
}
if (msgReceived == MAVLINK_FRAMING_BAD_CRC) {
// we don't process messages locally with bad CRC,
// but we do forward them, so when new messages
// are added we can bridge them
break;
}

#ifdef MAVLINK_FRAMING_BAD_SIGNATURE
if (msgReceived == MAVLINK_FRAMING_BAD_SIGNATURE) {
break;
}
if (msgReceived == MAVLINK_FRAMING_BAD_SIGNATURE) {
break;
}
#endif

//-- Check for message we might be interested
if(getWorld()->getComponent()->handleMessage(this, &_message)){
//-- Eat message (don't send it to FC)
memset(&_message, 0, sizeof(_message));
msgReceived = false;
continue;

//-- Check for message we might be interested
if(getWorld()->getComponent()->handleMessage(this, &_message)){
//-- Eat message (don't send it to FC)
memset(&_message, 0, sizeof(_message));
msgReceived = false;
continue;
}
}


Expand All @@ -171,7 +185,7 @@ MavESP8266GCS::_readMessage()
}
}
}
if(!msgReceived) {
if(!msgReceived && getWorld()->getParameters()->getWifiCastMode() == CAST_MODE_UNI) {
if(_heard_from && (millis() - _last_heartbeat) > HEARTBEAT_TIMEOUT) {
//-- Restart DHCP and start broadcasting again
if(getWorld()->getParameters()->getWifiMode() == WIFI_MODE_AP) {
Expand Down Expand Up @@ -223,6 +237,15 @@ MavESP8266GCS::sendMessage(mavlink_message_t* message) {
int
MavESP8266GCS::sendMessageRaw(uint8_t *buffer, int len)
{
switch(getWorld()->getParameters()->getWifiCastMode()) {
case CAST_MODE_UNI:
_udp.beginPacket(_ip, _udp_port);
break;

case CAST_MODE_FULL_MULTI ... CAST_MODE_CONS_MULTI:
_udp.beginPacketMulticast(_ip, _udp_port, WiFi.localIP());
break;
}
_udp.beginPacket(_ip, _udp_port);
size_t sent = _udp.write(buffer, len);
_udp.endPacket();
Expand Down
44 changes: 44 additions & 0 deletions src/mavesp8266_httpd.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,9 @@ const char* kDEBUG = "debug";
const char* kREBOOT = "reboot";
const char* kPOSITION = "position";
const char* kMODE = "mode";
const char* kCASTMODE = "castmode";
const char* kMCASTIP = "mcastip";
const char* kMCASTPORT = "mcastport";

const char* kFlashMaps[7] = {
"512KB (256/256)",
Expand Down Expand Up @@ -325,6 +328,34 @@ static void handle_setup()
message += "<input type='text' name='cport' value='";
message += getWorld()->getParameters()->getWifiUdpCport();
message += "'><br>";

message += "Casting Mode:&nbsp;";
message += "<input type='radio' name='castmode' value='0'";
if (getWorld()->getParameters()->getWifiCastMode() == CAST_MODE_UNI) {
message += " checked";
}
message += ">Unicast\n";
message += "<input type='radio' name='castmode' value='1'";
if (getWorld()->getParameters()->getWifiCastMode() == CAST_MODE_FULL_MULTI) {
message += " checked";
}
message += ">Full Multicast\n";
message += "<input type='radio' name='castmode' value='2'";
if (getWorld()->getParameters()->getWifiCastMode() == CAST_MODE_CONS_MULTI) {
message += " checked";
}
message += ">Conservative Multicast<br>\n";

message += "Multicast IP:&nbsp;";
message += "<input type='text' name='mcastip' value='";
IP = getWorld()->getParameters()->getWifiMcastIP();
message += IP.toString();
message += "'><br>";

message += "Multicast Port:&nbsp;";
message += "<input type='text' name='mcastport' value='";
message += getWorld()->getParameters()->getWifiMcastPort();
message += "'><br>";

message += "Baudrate:&nbsp;";
message += "<input type='text' name='baud' value='";
Expand Down Expand Up @@ -537,6 +568,19 @@ void handle_setParameters()
ok = true;
reboot = webServer.arg(kREBOOT) == "1";
}
if(webServer.hasArg(kCASTMODE)) {
ok = true;
getWorld()->getParameters()->setWifiCastMode(webServer.arg(kCASTMODE).toInt());
}
if(webServer.hasArg(kMCASTIP)) {
IPAddress ip;
ip.fromString(webServer.arg(kMCASTIP).c_str());
getWorld()->getParameters()->setWifiMcastIP(ip);
}
if(webServer.hasArg(kMCASTPORT)) {
ok = true;
getWorld()->getParameters()->setWifiMcastPort(webServer.arg(kMCASTPORT).toInt());
}
if(ok) {
getWorld()->getParameters()->saveAllToEeprom();
//-- Send new parameters back
Expand Down
33 changes: 33 additions & 0 deletions src/mavesp8266_parameters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,9 @@ uint32_t _wifi_subnetsta;
uint32_t _uart_baud_rate;
uint32_t _flash_left;
int8_t _raw_enable;
int8_t _wifi_cast_mode;
uint32_t _wifi_mcast_ip;
uint16_t _wifi_mcast_port;

//-- Parameters
// No string support in parameters so we stash a char[16] into 4 uint32_t
Expand Down Expand Up @@ -97,6 +100,9 @@ int8_t _raw_enable;
{"WIFI_SUBNET_STA", &_wifi_subnetsta, MavESP8266Parameters::ID_SUBNETSTA, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false},
{"UART_BAUDRATE", &_uart_baud_rate, MavESP8266Parameters::ID_UART, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false},
{"RAW_ENABLE", &_raw_enable, MavESP8266Parameters::ID_RAW_ENABLE,sizeof(int8_t), MAV_PARAM_TYPE_INT8, false},
{"WIFI_CAST_MODE", &_wifi_cast_mode, MavESP8266Parameters::ID_CAST_MODE, sizeof(int8_t), MAV_PARAM_TYPE_INT8, false},
{"WIFI_MCAST_IP", &_wifi_mcast_ip, MavESP8266Parameters::ID_MCAST_IP, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false},
{"WIFI_MCAST_PORT", &_wifi_mcast_port, MavESP8266Parameters::ID_MCAST_PORT,sizeof(uint16_t), MAV_PARAM_TYPE_UINT16, false},
};

//---------------------------------------------------------------------------------
Expand Down Expand Up @@ -154,6 +160,9 @@ uint32_t MavESP8266Parameters::getWifiStaGateway () { return _wifi_gatewaysta
uint32_t MavESP8266Parameters::getWifiStaSubnet () { return _wifi_subnetsta; }
uint32_t MavESP8266Parameters::getUartBaudRate () { return _uart_baud_rate; }
int8_t MavESP8266Parameters::getRawEnable () { return _raw_enable; }
int8_t MavESP8266Parameters::getWifiCastMode () { return _wifi_cast_mode; }
uint32_t MavESP8266Parameters::getWifiMcastIP () { return _wifi_mcast_ip; }
uint16_t MavESP8266Parameters::getWifiMcastPort () { return _wifi_mcast_port; }

//---------------------------------------------------------------------------------
//-- Reset all to defaults
Expand All @@ -170,6 +179,9 @@ MavESP8266Parameters::resetToDefaults()
_wifi_ipsta = 0;
_wifi_gatewaysta = 0;
_wifi_subnetsta = 0;
_wifi_cast_mode = 0;
_wifi_mcast_ip = DEFAULT_MCAST_IP;
_wifi_mcast_port = DEFAULT_UDP_HPORT;
strncpy(_wifi_ssid, kDEFAULT_SSID, sizeof(_wifi_ssid));
strncpy(_wifi_password, kDEFAULT_PASSWORD, sizeof(_wifi_password));
strncpy(_wifi_ssidsta, kDEFAULT_SSID, sizeof(_wifi_ssidsta));
Expand Down Expand Up @@ -412,3 +424,24 @@ MavESP8266Parameters::setUartBaudRate(uint32_t baud)
{
_uart_baud_rate = baud;
}

//---------------------------------------------------------------------------------
void
MavESP8266Parameters::setWifiCastMode(int8_t enabled)
{
_wifi_cast_mode = enabled;
}

//---------------------------------------------------------------------------------
void
MavESP8266Parameters::setWifiMcastIP(uint32_t ip)
{
_wifi_mcast_ip = ip;
}

//---------------------------------------------------------------------------------
void
MavESP8266Parameters::setWifiMcastPort(uint16_t port)
{
_wifi_mcast_port = port;
}
16 changes: 15 additions & 1 deletion src/mavesp8266_parameters.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,12 +41,17 @@
#define WIFI_MODE_AP 0
#define WIFI_MODE_STA 1

#define CAST_MODE_UNI 0
#define CAST_MODE_FULL_MULTI 1
#define CAST_MODE_CONS_MULTI 2

//-- Constants
#define DEFAULT_WIFI_MODE WIFI_MODE_AP
#define DEFAULT_UART_SPEED 921600
#define DEFAULT_WIFI_CHANNEL 11
#define DEFAULT_UDP_HPORT 14550
#define DEFAULT_UDP_CPORT 14555
#define DEFAULT_MCAST_IP 848429039

struct stMavEspParameters {
char id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
Expand Down Expand Up @@ -90,7 +95,10 @@ class MavESP8266Parameters {
ID_SUBNETSTA,
ID_UART,
ID_RAW_ENABLE,
ID_COUNT
ID_CAST_MODE,
ID_MCAST_IP,
ID_MCAST_PORT,
ID_COUNT,
};

void begin ();
Expand All @@ -114,6 +122,9 @@ class MavESP8266Parameters {
uint32_t getWifiStaSubnet ();
uint32_t getUartBaudRate ();
int8_t getRawEnable ();
int8_t getWifiCastMode ();
uint32_t getWifiMcastIP ();
uint16_t getWifiMcastPort ();

void setDebugEnabled (int8_t enabled);
void setWifiMode (int8_t mode);
Expand All @@ -129,6 +140,9 @@ class MavESP8266Parameters {
void setWifiStaSubnet (uint32_t addr);
void setUartBaudRate (uint32_t baud);
void setLocalIPAddress (uint32_t ipAddress);
void setWifiCastMode (int8_t mode);
void setWifiMcastIP (uint32_t ip);
void setWifiMcastPort (uint16_t port);

stMavEspParameters* getAt (int index);

Expand Down
4 changes: 3 additions & 1 deletion src/mavesp8266_vehicle.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,9 @@ void
MavESP8266Vehicle::readMessage()
{
if (_readMessage()) {
_forwardTo->sendMessage(&_msg);
if(!restricted_message(&_msg)) {
_forwardTo->sendMessage(&_msg);
}
}
//-- Update radio status (1Hz)
if(_heard_from && (millis() - _last_status_time > 1000)) {
Expand Down