Skip to content

Commit 78f81a8

Browse files
shiv-tyagishiv-tyagi
committed
add multicast functionality
1 parent 08a0ec9 commit 78f81a8

File tree

4 files changed

+147
-41
lines changed

4 files changed

+147
-41
lines changed

‎src/mavesp8266_gcs.cpp‎

Lines changed: 61 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -55,11 +55,21 @@ void
5555
MavESP8266GCS::begin(MavESP8266Bridge* forwardTo, IPAddress gcsIP)
5656
{
5757
MavESP8266Bridge::begin(forwardTo);
58-
_ip = gcsIP;
58+
5959
//-- Init variables that shouldn't change unless we reboot
60-
_udp_port = getWorld()->getParameters()->getWifiUdpHport();
6160
//-- Start UDP
62-
_udp.begin(getWorld()->getParameters()->getWifiUdpCport());
61+
switch(getWorld()->getParameters()->getWifiCastMode()) {
62+
case CAST_MODE_UNI:
63+
_ip = gcsIP;
64+
_udp_port = getWorld()->getParameters()->getWifiUdpHport();
65+
_udp.begin(getWorld()->getParameters()->getWifiUdpCport());
66+
break;
67+
case CAST_MODE_MULTI:
68+
_ip = getWorld()->getParameters()->getWifiMcastIP();
69+
_udp_port = getWorld()->getParameters()->getWifiMcastPort();
70+
_udp.beginMulticast(WiFi.localIP(), _ip, _udp_port);
71+
break;
72+
}
6373
}
6474

6575
//---------------------------------------------------------------------------------
@@ -120,48 +130,50 @@ MavESP8266GCS::_readMessage()
120130
if(msgReceived) {
121131
//-- We no longer need to broadcast
122132
_status.packets_received++;
123-
if(_ip[3] == 255) {
124-
_ip = _udp.remoteIP();
125-
getWorld()->getLogger()->log("Response from GCS. Setting GCS IP to: %s\n", _ip.toString().c_str());
126-
}
127-
//-- First packets
128-
if(!_heard_from) {
129-
if(_message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
130-
//-- We no longer need DHCP
131-
if(getWorld()->getParameters()->getWifiMode() == WIFI_MODE_AP) {
132-
wifi_softap_dhcps_stop();
133+
if(getWorld()->getParameters()->getWifiCastMode() == CAST_MODE_UNI) {
134+
if (_ip[3] == 255) {
135+
_ip = _udp.remoteIP();
136+
getWorld()->getLogger()->log("Response from GCS. Setting GCS IP to: %s\n", _ip.toString().c_str());
137+
}
138+
//-- First packets
139+
if(!_heard_from) {
140+
if(_message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
141+
//-- We no longer need DHCP
142+
if(getWorld()->getParameters()->getWifiMode() == WIFI_MODE_AP) {
143+
wifi_softap_dhcps_stop();
144+
}
145+
_heard_from = true;
146+
_system_id = _message.sysid;
147+
_component_id = _message.compid;
148+
_seq_expected = _message.seq + 1;
149+
_last_heartbeat = millis();
133150
}
134-
_heard_from = true;
135-
_system_id = _message.sysid;
136-
_component_id = _message.compid;
137-
_seq_expected = _message.seq + 1;
138-
_last_heartbeat = millis();
151+
} else {
152+
if(_message.msgid == MAVLINK_MSG_ID_HEARTBEAT)
153+
_last_heartbeat = millis();
154+
_checkLinkErrors(&_message);
139155
}
140-
} else {
141-
if(_message.msgid == MAVLINK_MSG_ID_HEARTBEAT)
142-
_last_heartbeat = millis();
143-
_checkLinkErrors(&_message);
144-
}
145156

146-
if (msgReceived == MAVLINK_FRAMING_BAD_CRC) {
147-
// we don't process messages locally with bad CRC,
148-
// but we do forward them, so when new messages
149-
// are added we can bridge them
150-
break;
151-
}
157+
if (msgReceived == MAVLINK_FRAMING_BAD_CRC) {
158+
// we don't process messages locally with bad CRC,
159+
// but we do forward them, so when new messages
160+
// are added we can bridge them
161+
break;
162+
}
152163

153164
#ifdef MAVLINK_FRAMING_BAD_SIGNATURE
154-
if (msgReceived == MAVLINK_FRAMING_BAD_SIGNATURE) {
155-
break;
156-
}
165+
if (msgReceived == MAVLINK_FRAMING_BAD_SIGNATURE) {
166+
break;
167+
}
157168
#endif
158-
159-
//-- Check for message we might be interested
160-
if(getWorld()->getComponent()->handleMessage(this, &_message)){
161-
//-- Eat message (don't send it to FC)
162-
memset(&_message, 0, sizeof(_message));
163-
msgReceived = false;
164-
continue;
169+
170+
//-- Check for message we might be interested
171+
if(getWorld()->getComponent()->handleMessage(this, &_message)){
172+
//-- Eat message (don't send it to FC)
173+
memset(&_message, 0, sizeof(_message));
174+
msgReceived = false;
175+
continue;
176+
}
165177
}
166178

167179

@@ -171,7 +183,7 @@ MavESP8266GCS::_readMessage()
171183
}
172184
}
173185
}
174-
if(!msgReceived) {
186+
if(!msgReceived && getWorld()->getParameters()->getWifiCastMode() == CAST_MODE_UNI) {
175187
if(_heard_from && (millis() - _last_heartbeat) > HEARTBEAT_TIMEOUT) {
176188
//-- Restart DHCP and start broadcasting again
177189
if(getWorld()->getParameters()->getWifiMode() == WIFI_MODE_AP) {
@@ -223,6 +235,15 @@ MavESP8266GCS::sendMessage(mavlink_message_t* message) {
223235
int
224236
MavESP8266GCS::sendMessageRaw(uint8_t *buffer, int len)
225237
{
238+
switch(getWorld()->getParameters()->getWifiCastMode()) {
239+
case CAST_MODE_UNI:
240+
_udp.beginPacket(_ip, _udp_port);
241+
break;
242+
243+
case CAST_MODE_MULTI:
244+
_udp.beginPacketMulticast(_ip, _udp_port, WiFi.localIP());
245+
break;
246+
}
226247
_udp.beginPacket(_ip, _udp_port);
227248
size_t sent = _udp.write(buffer, len);
228249
_udp.endPacket();

‎src/mavesp8266_httpd.cpp‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -66,6 +66,9 @@ const char* kDEBUG = "debug";
6666
const char* kREBOOT = "reboot";
6767
const char* kPOSITION = "position";
6868
const char* kMODE = "mode";
69+
const char* kCASTMODE = "castmode";
70+
const char* kMCASTIP = "mcastip";
71+
const char* kMCASTPORT = "mcastport";
6972

7073
const char* kFlashMaps[7] = {
7174
"512KB (256/256)",
@@ -325,6 +328,29 @@ static void handle_setup()
325328
message += "<input type='text' name='cport' value='";
326329
message += getWorld()->getParameters()->getWifiUdpCport();
327330
message += "'><br>";
331+
332+
message += "Casting Mode:&nbsp;";
333+
message += "<input type='radio' name='castmode' value='0'";
334+
if (getWorld()->getParameters()->getWifiCastMode() == CAST_MODE_UNI) {
335+
message += " checked";
336+
}
337+
message += ">Unicast\n";
338+
message += "<input type='radio' name='castmode' value='1'";
339+
if (getWorld()->getParameters()->getWifiCastMode() == CAST_MODE_MULTI) {
340+
message += " checked";
341+
}
342+
message += ">Multicast<br>\n";
343+
344+
message += "Multicast IP:&nbsp;";
345+
message += "<input type='text' name='mcastip' value='";
346+
IP = getWorld()->getParameters()->getWifiMcastIP();
347+
message += IP.toString();
348+
message += "'><br>";
349+
350+
message += "Multicast Port:&nbsp;";
351+
message += "<input type='text' name='mcastport' value='";
352+
message += getWorld()->getParameters()->getWifiMcastPort();
353+
message += "'><br>";
328354

329355
message += "Baudrate:&nbsp;";
330356
message += "<input type='text' name='baud' value='";
@@ -537,6 +563,19 @@ void handle_setParameters()
537563
ok = true;
538564
reboot = webServer.arg(kREBOOT) == "1";
539565
}
566+
if(webServer.hasArg(kCASTMODE)) {
567+
ok = true;
568+
getWorld()->getParameters()->setWifiCastMode(webServer.arg(kCASTMODE).toInt());
569+
}
570+
if(webServer.hasArg(kMCASTIP)) {
571+
IPAddress ip;
572+
ip.fromString(webServer.arg(kMCASTIP).c_str());
573+
getWorld()->getParameters()->setWifiMcastIP(ip);
574+
}
575+
if(webServer.hasArg(kMCASTPORT)) {
576+
ok = true;
577+
getWorld()->getParameters()->setWifiMcastPort(webServer.arg(kMCASTPORT).toInt());
578+
}
540579
if(ok) {
541580
getWorld()->getParameters()->saveAllToEeprom();
542581
//-- Send new parameters back

‎src/mavesp8266_parameters.cpp‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -65,6 +65,9 @@ uint32_t _wifi_subnetsta;
6565
uint32_t _uart_baud_rate;
6666
uint32_t _flash_left;
6767
int8_t _raw_enable;
68+
int8_t _wifi_cast_mode;
69+
uint32_t _wifi_mcast_ip;
70+
uint16_t _wifi_mcast_port;
6871

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

102108
//---------------------------------------------------------------------------------
@@ -154,6 +160,9 @@ uint32_t MavESP8266Parameters::getWifiStaGateway () { return _wifi_gatewaysta
154160
uint32_t MavESP8266Parameters::getWifiStaSubnet () { return _wifi_subnetsta; }
155161
uint32_t MavESP8266Parameters::getUartBaudRate () { return _uart_baud_rate; }
156162
int8_t MavESP8266Parameters::getRawEnable () { return _raw_enable; }
163+
int8_t MavESP8266Parameters::getWifiCastMode () { return _wifi_cast_mode; }
164+
uint32_t MavESP8266Parameters::getWifiMcastIP () { return _wifi_mcast_ip; }
165+
uint16_t MavESP8266Parameters::getWifiMcastPort () { return _wifi_mcast_port; }
157166

158167
//---------------------------------------------------------------------------------
159168
//-- Reset all to defaults
@@ -170,6 +179,9 @@ MavESP8266Parameters::resetToDefaults()
170179
_wifi_ipsta = 0;
171180
_wifi_gatewaysta = 0;
172181
_wifi_subnetsta = 0;
182+
_wifi_cast_mode = 0;
183+
_wifi_mcast_ip = DEFAULT_MCAST_IP;
184+
_wifi_mcast_port = DEFAULT_UDP_HPORT;
173185
strncpy(_wifi_ssid, kDEFAULT_SSID, sizeof(_wifi_ssid));
174186
strncpy(_wifi_password, kDEFAULT_PASSWORD, sizeof(_wifi_password));
175187
strncpy(_wifi_ssidsta, kDEFAULT_SSID, sizeof(_wifi_ssidsta));
@@ -412,3 +424,24 @@ MavESP8266Parameters::setUartBaudRate(uint32_t baud)
412424
{
413425
_uart_baud_rate = baud;
414426
}
427+
428+
//---------------------------------------------------------------------------------
429+
void
430+
MavESP8266Parameters::setWifiCastMode(int8_t enabled)
431+
{
432+
_wifi_cast_mode = enabled;
433+
}
434+
435+
//---------------------------------------------------------------------------------
436+
void
437+
MavESP8266Parameters::setWifiMcastIP(uint32_t ip)
438+
{
439+
_wifi_mcast_ip = ip;
440+
}
441+
442+
//---------------------------------------------------------------------------------
443+
void
444+
MavESP8266Parameters::setWifiMcastPort(uint16_t port)
445+
{
446+
_wifi_mcast_port = port;
447+
}

‎src/mavesp8266_parameters.h‎

Lines changed: 14 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -41,12 +41,16 @@
4141
#define WIFI_MODE_AP 0
4242
#define WIFI_MODE_STA 1
4343

44+
#define CAST_MODE_UNI 0
45+
#define CAST_MODE_MULTI 1
46+
4447
//-- Constants
4548
#define DEFAULT_WIFI_MODE WIFI_MODE_AP
4649
#define DEFAULT_UART_SPEED 921600
4750
#define DEFAULT_WIFI_CHANNEL 11
4851
#define DEFAULT_UDP_HPORT 14550
4952
#define DEFAULT_UDP_CPORT 14555
53+
#define DEFAULT_MCAST_IP 848429039
5054

5155
struct stMavEspParameters {
5256
char id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
@@ -90,7 +94,10 @@ class MavESP8266Parameters {
9094
ID_SUBNETSTA,
9195
ID_UART,
9296
ID_RAW_ENABLE,
93-
ID_COUNT
97+
ID_CAST_MODE,
98+
ID_MCAST_IP,
99+
ID_MCAST_PORT,
100+
ID_COUNT,
94101
};
95102

96103
void begin ();
@@ -114,6 +121,9 @@ class MavESP8266Parameters {
114121
uint32_t getWifiStaSubnet ();
115122
uint32_t getUartBaudRate ();
116123
int8_t getRawEnable ();
124+
int8_t getWifiCastMode ();
125+
uint32_t getWifiMcastIP ();
126+
uint16_t getWifiMcastPort ();
117127

118128
void setDebugEnabled (int8_t enabled);
119129
void setWifiMode (int8_t mode);
@@ -129,6 +139,9 @@ class MavESP8266Parameters {
129139
void setWifiStaSubnet (uint32_t addr);
130140
void setUartBaudRate (uint32_t baud);
131141
void setLocalIPAddress (uint32_t ipAddress);
142+
void setWifiCastMode (int8_t mode);
143+
void setWifiMcastIP (uint32_t ip);
144+
void setWifiMcastPort (uint16_t port);
132145

133146
stMavEspParameters* getAt (int index);
134147

0 commit comments

Comments
 (0)