Commit 599360a9 authored by Daniil Kukushkin's avatar Daniil Kukushkin

Support of multiple ground station connections

parent f3d01e48
This diff is collapsed.
......@@ -39,15 +39,16 @@
#include "stm_ota_udp_receiver.h"
#include "settings_manager.h"
#include "Color.h"
#include "UdpPacketSender.h"
extern "C" {
#include<user_interface.h>
}
namespace NanopixCore {
//#define DEBUG
#define DEBUG 0
#ifdef DEBUG
#if DEBUG
#define UART_BAUDRATE 230400
#else
#define UART_BAUDRATE 460800
......@@ -56,15 +57,17 @@ namespace NanopixCore {
#define STM32_OTA_UDP_PORT 5700
#define STM32_OTA_SERVER_UDP_PORT 5705
#define RESET_SSID_PASS_SECONDS 60*5
#define RESET_SSID_PASS_SECONDS (60*5)
#define CMD_LOOP_FREQ 19
#define CMD_UDP_RX_FREQ 20
#define RETRANSMIT_TIMER_FREQ 100 // retransmit data timer from STM to App/GS
#define RETRANSMIT_TIMER_FREQ 100 // HZ, retransmit data timer from STM to App/GS
#define PIX_RECEIVER_BUFFER_SIZE 256
#define PIX_RECEIVER_BUFFER_SIZE 2048
#define MAX_UART_READ_SIZE 128
#define MAVLINK_TX_BUFFER_SIZE 256
class MainController {
public:
enum TelemetryPacketType {Custom6AxisIMU = 1, Standart9AxisIMU, AttitudePacket, DistanceSensor, LocalPosition};
......@@ -75,44 +78,25 @@ private:
mavlink_status_t mavStatus, mavStatusFC;
mavlink_manual_control_t manualControlMsg;
uint8_t mavSendBuffer[256];
uint8_t mavSendBuffer[MAVLINK_TX_BUFFER_SIZE];
AttitudeControlMode attitudeControlMode;
UdpPacketSender udpSender;
AsyncUDP Udp;
int localUdpPort = 14555;
/* For user/pass reset: */
bool isSomeConnectedBefore = false;
int noConnectSeconds = 0;
byte incomingPacket[255];
/* -------------------- */
unsigned long currentMillis;
unsigned long prevMillis = 0;
unsigned long measureIntervalMillis = 1000; // measure once per second
uint16_t loopFrequency = 0;
uint16_t udpReadFrequency = 0;
bool isRemoteInit = false;
int MTU = 128;
IPAddress ip;
IPAddress gateway;
IPAddress subnet;
IPAddress controllerIP;
IPAddress multicastIP;
IPAddress localIP;
int localUdpPort = 14555;
int controllerUdpListenerPort = 14550;
int localEspUdpPort = 5610;
int controllerEspUdpListenerPort = 5605;
uint8_t uart_read_size = 0;
uint8_t pixReceiverBuffer[PIX_RECEIVER_BUFFER_SIZE];
uint8_t pixReceiverCount = 0;
uint32_t uartBytesInSecRX = 0;
SettingsManager settingsManager;
......@@ -125,27 +109,24 @@ private:
float currentRoll, currentPitch, currentYaw;
int rcRoll, rcPitch, rcThrottle, rcYaw;
int optimalWifiChannel;
int wifiChannel;
///////////////////////////////////////////////////////////////////////////////////////////
////////////////////// Methods: //////////////////////////////////////////////////////////
byte selectOptimalChannel();
friend void udpOnPacketHandler(AsyncUDPPacket packet);
friend void WiFiEvent(WiFiEvent_t event);
void client_status();
void serialFlush();
void setParam(const char* name, float value);
void sendCommandIntMavMsg(uint8_t msgType, float param1, float param2, float param3, float param4, float param5, float param6, float param7);
void switchTakeoffLand(bool takeoff);
void switchArmDisarm(bool armState);
void sendMavlinkPacket(mavlink_message_t* msg, int port, IPAddress* ipAddr);
int sendMavlinkPacket(mavlink_message_t* msg, bool multicast = false);
void openAsyncUDP();
void closeAsyncUDP();
void mainServiceRoutine();
friend void retransmitTimerCallback(void *pArg);
uint8_t* grabSerialAndRetransmit();
void grabSerialAndRetransmit(); // extract data from packets and retransmit them to GS
public:
MainController();
......@@ -163,7 +144,7 @@ public:
void setAttitudeControlMode(AttitudeControlMode attitudeCtrlMode);
void requestTelemetryDataStream(TelemetryPacketType streamType, int streamRateHz, bool startStopFlag);
int getAltitude(); // in sm
int getAltitude(); // in cm
int getRC_Roll();
int getRC_Pitch();
int getRC_Throttle();
......@@ -179,7 +160,9 @@ public:
void land();
void rebootFlightController();
void sendLogMsg(const char* str, IPAddress* ipAddr);
int sendLogMsg(const char* str, bool multicast = false);
int sendLogMsg(int value, bool multicast = false);
int sendLogMsg(float value, bool multicast = false);
void setLEDsColors(uint8_t forwardR, uint8_t forwardG, uint8_t forwardB, uint8_t backwardR, uint8_t backwardG, uint8_t backwardB);
void setLEDsColors(Color forwardColor, Color backwardColor);
......
/*
Copyright (C) 2017 Nanopix
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the
distribution.
* Neither the name of Nanopix nor the names
of its contributors may be used to endorse or promote products
derived from this software without specific prior written
permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
SUCH DAMAGE.
*/
#include "UdpPacketSender.h"
#include "nanopix_config.h"
namespace NanopixCore {
/* =========================================== */
/* =============== Constructors ============== */
/* =========================================== */
UdpPacketSender::UdpPacketSender()
{
ip = IPAddress(192, 168, 43, 140);
gateway = IPAddress(192, 168, 43, 1);
subnet = IPAddress(255, 255, 255, 0);
multicastIP = IPAddress(192, 168, 43, 255);
for (int i = 0; i < controllersCount; i++)
controllerIP[i] = IPAddress(192, 168, 43, 255);
}
int UdpPacketSender::initialize(AsyncUDP * udp)
{
int ret = NANOPIX_STATUS_OK;
if (udp == NULL)
return NANOPIX_STATUS_NULL_ARGUMENT;
this->udp = udp;
return ret;
}
/* =========================================== */
/* ================ Operations =============== */
/* =========================================== */
int UdpPacketSender::sendToConnected(uint8_t * data, int size, SenderSourceType sourceType) const
{
int ret = NANOPIX_STATUS_OK;
if(data == NULL)
return NANOPIX_STATUS_NULL_ARGUMENT;
if(size <= 0)
return NANOPIX_STATUS_INVALID_ARGUMENT;
if(controllersCount == 0)
return NANOPIX_STATUS_NO_DATA;
int port = resolvePortBySourceType(sourceType);
for(int i = 0; i < controllersCount; i++)
{
udp->writeTo(data, size, controllerIP[i], port);
}
return ret;
}
int UdpPacketSender::sendMulticast(uint8_t * data, int size, SenderSourceType sourceType) const
{
int ret = NANOPIX_STATUS_OK;
if (data == NULL)
return NANOPIX_STATUS_NULL_ARGUMENT;
if (size <= 0)
return NANOPIX_STATUS_INVALID_ARGUMENT;
int port = resolvePortBySourceType(sourceType);
udp->writeTo(data, size, multicastIP, port);
return ret;
}
int UdpPacketSender::registerController(IPAddress addr)
{
int ret = NANOPIX_STATUS_OK;
if(controllersCount == MAX_CONTROLLERS)
return NANOPIX_STATUS_FULL;
controllerIP[controllersCount] = addr;
controllersCount++;
return ret;
}
/* =========================================== */
/* ================ Accessors ================ */
/* =========================================== */
int UdpPacketSender::connectedCount() const
{
return controllersCount;
}
bool UdpPacketSender::hasController(IPAddress addr) const
{
for (int i = 0; i < controllersCount; i++)
if (addr == controllerIP[i])
return true;
return false;
}
int UdpPacketSender::resolvePortBySourceType(SenderSourceType sourceType) const
{
switch (sourceType)
{
case SenderSourceType::ResendFromFC:
return CONTROLLER_UDP_PORT;
break;
case SenderSourceType::OwnMessage:
return ESP_LISTENER_UDP_PORT;
break;
default:
return 0;
}
}
}
/*
Copyright (C) 2017 Nanopix
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the
distribution.
* Neither the name of Nanopix nor the names
of its contributors may be used to endorse or promote products
derived from this software without specific prior written
permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
SUCH DAMAGE.
*/
#ifndef UDP_PACKET_SENDER_H
#define UDP_PACKET_SENDER_H
#include <ESP8266WiFi.h>
#include <ESP8266mDNS.h>
#include "ESPAsyncUDP.h"
namespace NanopixCore {
#define MAX_CONTROLLERS 4
enum SenderSourceType { ResendFromFC = 0, OwnMessage };
class UdpPacketSender {
public:
/* =========================================== */
/* =============== Constructors ============== */
/* =========================================== */
UdpPacketSender();
int initialize(AsyncUDP * udp);
/* =========================================== */
/* ================ Operations =============== */
/* =========================================== */
int registerController(IPAddress addr);
int sendToConnected(uint8_t * data, int size, SenderSourceType sourceType = SenderSourceType::ResendFromFC) const;
int sendMulticast(uint8_t * data, int size, SenderSourceType sourceType = SenderSourceType::ResendFromFC) const;
/* =========================================== */
/* ================ Accessors ================ */
/* =========================================== */
bool hasController(IPAddress addr) const;
int connectedCount() const;
private:
int resolvePortBySourceType(SenderSourceType sourceType) const;
static const int CONTROLLER_UDP_PORT = 14550; // listener for resended packets
static const int ESP_LISTENER_UDP_PORT = 5605; // listener for ESP own (internal, like wifi auth) packets
IPAddress controllerIP[MAX_CONTROLLERS];
IPAddress multicastIP;
IPAddress localIP;
AsyncUDP * udp;
int controllersCount = 0;
public:
IPAddress ip;
IPAddress gateway;
IPAddress subnet;
};
}
#endif
......@@ -26,26 +26,23 @@
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
SUCH DAMAGE.
*/
#include "NanopixCore.h"
using namespace NanopixCore;
using namespace NanopixCore;
MainController mainController;
MainController mainController;
void setup() {
mainController.initializeSystem();
void setup() {
mainController.initializeSystem();
}
void flightScript()
{
// здесь вы можете написать скрипт для Nanopix
}
void loop() {
mainController.mainRoutine();
//if(mainController.isArmCommandReceived())
if(mainController.getRC_Throttle() < 1200 && mainController.getRC_Yaw() < 1200)
{
mainController.requestTelemetryDataStream(MainController::LocalPosition, 20, true);
......@@ -57,4 +54,3 @@ void loop() {
mainController.requestTelemetryDataStream(MainController::AttitudePacket, 20, false);
}
}
/*
Copyright (C) 2017 Nanopix
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the
distribution.
* Neither the name of Nanopix nor the names
of its contributors may be used to endorse or promote products
derived from this software without specific prior written
permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
SUCH DAMAGE.
*/
#ifndef NANOPIX_CONFIG
#define NANOPIX_CONFIG
#define NANOPIX_STATUS_OK 0
#define NANOPIX_STATUS_NULL_ARGUMENT 1
#define NANOPIX_STATUS_ALREADY_EXIST 2
#define NANOPIX_STATUS_NO_DATA 3
#define NANOPIX_STATUS_INVALID_ARGUMENT 4
#define NANOPIX_STATUS_FULL 5
#define NANOPIX_STATUS_NO_MEM 6
#define NANOPIX_STATUS_UNSUPP 7
#define NANOPIX_STATUS_UNINITIALIZED 8
#define NP_CHECK(x){\
int macro_opRes = (x);\
if(macro_opRes != NANOPIX_STATUS_OK){\
ret = macro_opRes;\
goto out;\
}}
#endif
\ No newline at end of file
......@@ -35,6 +35,7 @@
#include <WiFiUdp.h>
#include <EEPROM.h>
#include "mavlink\common\mavlink.h"
#include "UdpPacketSender.h"
namespace NanopixCore {
......@@ -47,22 +48,24 @@ class SettingsManager {
#define DEFAULT_PASSWORD "12345678"
WiFiUDP espUDP;
IPAddress controllerIP;
unsigned int localESP_UdpPort = 5610;
unsigned int stationESP_ListenerUdpPort = 5605;
mavlink_message_t rxMessage, txMessage;
mavlink_status_t mavStatus;
mavlink_statustext_t statusTextMsg;
uint8_t buffer[256];
UdpPacketSender * udpSender;
public:
String SSID, PASS;
SettingsManager() {
controllerIP = IPAddress(192, 168, 43, 255);
}
void initAll()
void initAll(UdpPacketSender * udpSender)
{
this->udpSender = udpSender;
EEPROM.begin(512);
//EEPROM.write(0,0);
......@@ -72,11 +75,6 @@ class SettingsManager {
randomSeed(ESP.getCycleCount());
}
void setControllerIP(IPAddress IP)
{
controllerIP = IP;
}
void parceMavPackets()
{
int packetSize = espUDP.parsePacket();
......@@ -143,9 +141,7 @@ class SettingsManager {
mavlink_msg_param_value_pack(0, 0, &txMsg, ESP_SOFTWARE_VERSION_ID, ESP_SOFTWARE_VERSION, MAVLINK_TYPE_FLOAT, paramsCount, paramIndex);
int txSize = mavlink_msg_to_send_buffer(buffer, &txMsg);
espUDP.beginPacket(controllerIP, stationESP_ListenerUdpPort);
espUDP.write(buffer, txSize);
espUDP.endPacket();
udpSender->sendToConnected(buffer, txSize, SenderSourceType::OwnMessage);
return true;
}
return false;
......@@ -187,9 +183,7 @@ class SettingsManager {
{
mavlink_msg_statustext_pack(0,0,&txMessage, 0, str);
int16_t txSize = mavlink_msg_to_send_buffer(buffer, &txMessage);
espUDP.beginPacket(controllerIP, stationESP_ListenerUdpPort);
espUDP.write(buffer, txSize);
espUDP.endPacket();
udpSender->sendToConnected(buffer, txSize, SenderSourceType::OwnMessage);
}
void sendString(String str)
......
#include "utils.h"
#include <ESP8266WiFi.h>
#include <ESP8266mDNS.h>
#include <WiFiUdp.h>
#include <ArduinoOTA.h>
#include "ESPAsyncUDP.h"
extern "C" {
#include<user_interface.h>
}
uint8_t selectOptimalWifiChannel()
{
const int maxChannel = 12;
int rssiPerChannel[maxChannel + 2];
for (int i = 0; i < maxChannel + 2; i++)
rssiPerChannel[i] = 0;
int n = WiFi.scanNetworks();
for (int i = 0; i < n; i++) {
byte chan = WiFi.channel(i);
int rssi = 255 - WiFi.RSSI(i);
#if DEBUG
Serial.print("SSID : ");
Serial.println(WiFi.SSID(i));
Serial.print("Channel : ");
Serial.print(chan);
Serial.print(" RSSI : ");
Serial.println(rssi);
#endif
if (chan > maxChannel)
chan = maxChannel;
if (chan < 1)
chan = 1;
rssiPerChannel[chan - 1] += rssi / 2;
rssiPerChannel[chan] += rssi;
rssiPerChannel[chan + 1] += rssi / 2;
}
#if DEBUG
for (int i = 0; i < maxChannel + 2; i++) {
Serial.print(rssiPerChannel[i]);
Serial.print(" ");
}
#endif
uint8_t min = 255;
uint8_t minIndex = 0;
for (int i = 1; i < maxChannel + 1; i++) {
int sum = rssiPerChannel[i - 1] + rssiPerChannel[i] + rssiPerChannel[i + 1];
if (sum < min) {
min = sum;
minIndex = i;
}
}
return minIndex;
}
void printClientsStatus()
{
IPAddress accessIP = WiFi.softAPIP();
Serial.print("ESP AccessPoint IP address: ");
Serial.println(accessIP);
unsigned char number_client;
struct station_info *stat_info;
struct ip_addr *IPaddress;
IPAddress address;
int i = 1;
number_client = wifi_softap_get_station_num(); // Count of stations which are connected to ESP8266 soft-AP
stat_info = wifi_softap_get_station_info();
Serial.print(" Total connected_client are = ");
Serial.println(number_client);
while (stat_info != NULL)
{
IPaddress = &stat_info->ip;
address = IPaddress->addr;
Serial.print("client= ");
Serial.print(i);
Serial.print(" ip adress is = ");
Serial.print((address));
Serial.print(" with mac adress is = ");
Serial.print(stat_info->bssid[0], HEX);
Serial.print(stat_info->bssid[1], HEX);
Serial.print(stat_info->bssid[2], HEX);
Serial.print(stat_info->bssid[3], HEX);
Serial.print(stat_info->bssid[4], HEX);
Serial.print(stat_info->bssid[5], HEX);
stat_info = STAILQ_NEXT(stat_info, next);
i++;
Serial.println();
}
}
\ No newline at end of file
/*
Copyright (C) 2017 Nanopix
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the
distribution.
* Neither the name of Nanopix nor the names
of its contributors may be used to endorse or promote products
derived from this software without specific prior written
permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
SUCH DAMAGE.
*/
#ifndef UTILS_H
#define UTILS_H
#include "stdint.h"
uint8_t selectOptimalWifiChannel();
void printClientsStatus();
#endif
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment