├── flashing ├── mac_addresses.txt ├── MacFlash.batch ├── README.md ├── WindowsFlash.bat └── esptool_output.txt ├── .gitmodules ├── .gitignore ├── src ├── led_manager.h ├── mavesp8266_httpd.h ├── mavesp8266_gcs.h ├── mavesp8266_vehicle.h ├── mavesp8266_component.h ├── led_manager.cpp ├── mavesp8266_parameters.h ├── crc.h ├── mavesp8266.cpp ├── mavesp8266.h ├── mavesp8266_vehicle.cpp ├── main.cpp ├── mavesp8266_gcs.cpp ├── mavesp8266_component.cpp ├── mavesp8266_parameters.cpp └── mavesp8266_httpd.cpp ├── .travis.yml ├── platformio_prebuild.py ├── platformio.ini ├── LICENSE.md ├── README.md ├── HTTP.md └── PARAMETERS.md /flashing/mac_addresses.txt: -------------------------------------------------------------------------------- 1 | 24:d7:eb:f3:a0:27 2 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "lib/mavlink"] 2 | path = lib/mavlink 3 | url = https://github.com/mavlink/c_library_v2.git 4 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .pioenvs 2 | platformio.sublime* 3 | platformio.pro* 4 | tests 5 | research 6 | .pio 7 | .vscode 8 | .vs 9 | builds 10 | -------------------------------------------------------------------------------- /flashing/MacFlash.batch: -------------------------------------------------------------------------------- 1 | while true; do 2 | python3 /Users/samknox/.platformio/packages/tool-esptoolpy/esptool.py --after soft_reset --chip esp8266 --port /dev/cu.usbserial-AQ035JK3 --baud 3000000 write_flash 0x0 /Users/samknox/Documents/Kahuna/.pio/build/esp07s/mavesp-esp07s-2.4.0.bin 3 | done -------------------------------------------------------------------------------- /src/led_manager.h: -------------------------------------------------------------------------------- 1 | #include "mavesp8266.h" 2 | 3 | #ifndef LED_MANAGER_H 4 | #define LED_MANAGER_H 5 | 6 | class LEDManager 7 | { 8 | public: 9 | enum Led 10 | { 11 | wifi = 4, 12 | air = 12, 13 | gcs = 5 14 | }; 15 | enum LedStatus 16 | { 17 | off, 18 | on, 19 | blink, 20 | doubleBlink 21 | }; 22 | void setLED(Led selectedLed, LedStatus status); 23 | void blinkLED(); 24 | void doubleBlinkLED(); 25 | 26 | private: 27 | unsigned long _timeNextBlink = 0; // Time at which the next change in the status light is due 28 | unsigned long _timeNextDoubleBlink = 0; // Time at which the next double blink is due 29 | bool _ledsToBlink = false; 30 | LedStatus _gcsLedStatus = off; 31 | LedStatus _wifiLedStatus = off; 32 | LedStatus _airLedStatus = off; 33 | int _gcsValue = LOW; 34 | int _wifiValue = LOW; 35 | int _airValue = LOW; 36 | int _cycleTime = 600; 37 | // bool _doubleBlinkFlag = false; 38 | int _doubleBlinkCount = 0; 39 | }; 40 | #endif -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # Continuous Integration (CI) is the practice, in software 2 | # engineering, of merging all developer working copies with a shared mainline 3 | # several times a day < http://docs.platformio.org/en/latest/ci/index.html > 4 | # 5 | # Documentation: 6 | # 7 | # * Travis CI Embedded Builds with PlatformIO 8 | # < https://docs.travis-ci.com/user/integration/platformio/ > 9 | # 10 | # * PlatformIO integration with Travis CI 11 | # < http://docs.platformio.org/en/latest/ci/travis.html > 12 | # 13 | # * User Guide for `platformio ci` command 14 | # < http://docs.platformio.org/en/latest/userguide/cmd_ci.html > 15 | # 16 | # 17 | # Please choice one of the following templates (proposed below) and uncomment 18 | # it (remove "# " before each line) or use own configuration according to the 19 | # Travis CI documentation (see above). 20 | # 21 | 22 | 23 | language: python 24 | python: 25 | - "2.7" 26 | 27 | sudo: false 28 | cache: 29 | directories: 30 | - "~/.platformio" 31 | 32 | install: 33 | - pip install -U platformio 34 | 35 | script: 36 | - platformio run -e esp01_1m 37 | -------------------------------------------------------------------------------- /flashing/README.md: -------------------------------------------------------------------------------- 1 | # PCB Flashing Instructions 2 | 3 | This guide explains how to flash your PCB. 4 | 5 | ## Prerequisites 6 | - Ensure you have the required drivers installed for your PCB. 7 | - Download/Build the firmware file (e.g., `firmware.hex` or `firmware.bin`). 8 | - Connect your PCB to your computer via USB. 9 | 10 | ## Files 11 | For Windows: 12 | - `WindowsFlash.bat`: The batch script to automate the flashing process. 13 | - `firmware.hex` or `firmware.bin`: The firmware to be flashed. 14 | - `esptool.py`: Python script for flashing ESP-based devices. Generally included in platformio when the firmware is built. i.e: \.platformio\packages\tool-esptoolpy\esptool.py 15 | 16 | ## Flashing Steps 17 | 18 | 1. **Run Script** 19 | Run the following command with the appropriate parameters: 20 | ``` 21 | ./WindowsFlash.bat 921600 COM1 C:\path\to\esptool.py C:\path\to\flashfile.bin 22 | ``` 23 | - Replace `921600` with the desired baud rate. 24 | - Replace `COM1` with the correct COM port for your PCB. 25 | - Replace `C:\path\to\esptool.py` with the path to your `esptool.py`. 26 | - Replace `C:\path\to\flashfile.bin` with the path to your firmware file. 27 | 28 | 2. **Connect PCB** 29 | Plug your PCB into your computer. 30 | 31 | -------------------------------------------------------------------------------- /platformio_prebuild.py: -------------------------------------------------------------------------------- 1 | import datetime 2 | import subprocess 3 | 4 | Import("env") 5 | 6 | revision = ( 7 | subprocess.check_output(["git", "rev-parse", "--short", "HEAD"]) 8 | .strip() 9 | .decode("utf-8") 10 | ) 11 | #get current date and time 12 | curr_date = datetime.datetime.now() 13 | date_str = f"{curr_date.year}-{curr_date.month:02}-{curr_date.day:02}" 14 | time_str = f"{curr_date.hour:02}:{curr_date.minute:02}:{curr_date.second:02}" 15 | 16 | # add revision and date/time as build informations to the existing build flags (workaround to fix compilation issue with "date +%%..." ) 17 | build_flags = env['BUILD_FLAGS'] 18 | build_flags[0] = "!echo" + ' "-DPIO_SRC_REV="' + str(revision) + ' "-DPIO_BUILD_DATE="' + date_str + ' "-DPIO_BUILD_TIME="' + time_str + " " + build_flags[0] 19 | 20 | 21 | # retrieve build flags 22 | my_flags = env.ParseFlags(env['BUILD_FLAGS']) 23 | defines = {k: v for (k, v) in my_flags.get("CPPDEFINES")} 24 | 25 | version_string = defines.get("VERSION_STRING") # e.g. "1.2.2" 26 | board_name = env["BOARD"] # e.g. "esp01m" 27 | 28 | # replace dots in version if linker can't find the path 29 | #version_string = version_string.replace(".","_") 30 | 31 | # set board and version in firmware name 32 | env.Replace(PROGNAME="mavesp-{}-{}".format(board_name, version_string)) -------------------------------------------------------------------------------- /platformio.ini: -------------------------------------------------------------------------------- 1 | # 2 | # Project Configuration File 3 | # 4 | # A detailed documentation with the EXAMPLES is located here: 5 | # http://docs.platformio.org/en/latest/projectconf.html 6 | # 7 | 8 | # The upload speed below (921600) has worked fine for all modules I tested. If you have upload issues, 9 | # try reducing to 115200. 10 | 11 | # Set mavesp version 12 | 13 | [version] 14 | major = 2 15 | minor = 4 16 | build = 0 17 | 18 | # Generate version string (e.g "1.2.2") and flags 19 | 20 | [version_env] 21 | version_string = ${version.major}.${version.minor}.${version.build} 22 | version_flags = "-DMAVESP8266_VERSION_MINOR="${version.minor} "-DMAVESP8266_VERSION_MAJOR="${version.major} "-DMAVESP8266_VERSION_BUILD="${version.build} "-DVERSION_STRING="${version_env.version_string} 23 | 24 | # General settings 25 | # - Set platform and framework 26 | # - Generate revision, date and time flags 27 | # - Run prebuild script to set firmware name 28 | 29 | [env] 30 | platform = espressif8266 31 | framework = arduino 32 | build_flags = ${version_env.version_flags} 33 | extra_scripts = pre:platformio_prebuild.py 34 | 35 | # Platform specific settings 36 | 37 | ;monitor_speed = 115200 38 | ;monitor_speed = 74880 39 | upload_speed = 921600 40 | monitor_speed = 921600 41 | 42 | ;[env:esp12e] 43 | ;board = esp12e 44 | ;build_flags = ${env.build_flags} -Wl,-Tesp8266.flash.4m.ld -DDEBUG_ESP_PORT=Serial 45 | ; [env:esp01_1m] 46 | ; board = esp01_1m 47 | 48 | ; [env:esp01] 49 | ; board = esp01 50 | 51 | [env:esp07s] 52 | board = esp07s 53 | 54 | build_dir = build/esp07s 55 | 56 | -------------------------------------------------------------------------------- /flashing/WindowsFlash.bat: -------------------------------------------------------------------------------- 1 | @echo off 2 | :: Windows Flash Script for ESP8266 3 | :: Usage: WindowsFlash.bat 4 | :: Example: WindowsFlash.bat 921600 COM15 C:\path\to\esptool.py C:\path\to\flashfile.bin 5 | REM This script continuously flashes the ESP8266 with the specified parameters. 6 | REM Make sure to run this script in a command prompt with administrative privileges. 7 | 8 | 9 | set baudrate=%1 10 | set COM=%2 11 | set esptool=%3 12 | set flashfile=%4 13 | :loop 14 | ::python C:\Users\mbgpcsk4\.platformio\packages\tool-esptoolpy\esptool.py --after soft_reset --chip esp8266 --port "COM15" --baud 921600 write_flash 0x0 C:\Work\Kahuna\.pio\build\esp07s\mavesp-esp07s-2.4.0.bin 15 | ::python %esptool% --after soft_reset --chip esp8266 --port %COM% --baud %baudrate% write_flash 0x0 %flashfile% 16 | python %esptool% --after soft_reset --chip esp8266 --port %COM% --baud %baudrate% write_flash 0x0 %flashfile% > esptool_output.txt 2>&1 17 | set PYTHON_ERRORLEVEL=%ERRORLEVEL% 18 | type esptool_output.txt 19 | 20 | if "%PYTHON_ERRORLEVEL%"=="0" ( 21 | for /f "tokens=2 delims= " %%A in ('findstr /I "MAC" esptool_output.txt') do ( 22 | setlocal enabledelayedexpansion 23 | set "MAC=%%A" 24 | findstr "!MAC!" mac_addresses.txt >nul 2>&1 25 | if errorlevel 1 ( 26 | echo !MAC! >> mac_addresses.txt 27 | echo New MAC address added: !MAC! 28 | ) else ( 29 | echo MAC address already exists: !MAC! 30 | ) 31 | endlocal 32 | ) 33 | echo Flashing succeeded. Waiting before next attempt... 34 | TIMEOUT /T 5 35 | ) else ( 36 | echo Flashing failed. Retrying... 37 | ) 38 | goto loop 39 | 40 | :: cd flashing 41 | :: ./WindowsFlash.bat 921600 COM15 C:\Users\mbgpcsk4\.platformio\packages\tool-esptoolpy\esptool.py C:\Work\Kahuna\.pio\build\esp07s\mavesp-esp07s-2.4.0.bin -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | This software is licensed generally under a permissive 3-Clause BSD license. Contributions are required 2 | to be made under the same license. 3 | ``` 4 | /**************************************************************************** 5 | * 6 | * Copyright (c) 2015, 2016 Gus Grubba. All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * 2. Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in 16 | * the documentation and/or other materials provided with the 17 | * distribution. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | ``` -------------------------------------------------------------------------------- /flashing/esptool_output.txt: -------------------------------------------------------------------------------- 1 | esptool.py v4.5.1 2 | Serial port COM16 3 | Connecting......................... 4 | Traceback (most recent call last): 5 | File "C:\Users\mbgpcsk4\.platformio\packages\tool-esptoolpy\esptool.py", line 34, in 6 | esptool._main() 7 | File "C:\Users\mbgpcsk4\.platformio\packages\tool-esptoolpy\esptool\__init__.py", line 1032, in _main 8 | main() 9 | File "C:\Users\mbgpcsk4\.platformio\packages\tool-esptoolpy\esptool\__init__.py", line 674, in main 10 | esp = esp or get_default_connected_device( 11 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 12 | File "C:\Users\mbgpcsk4\.platformio\packages\tool-esptoolpy\esptool\__init__.py", line 921, in get_default_connected_device 13 | _esp.connect(before, connect_attempts) 14 | File "C:\Users\mbgpcsk4\.platformio\packages\tool-esptoolpy\esptool\loader.py", line 640, in connect 15 | last_error = self._connect_attempt(reset_strategy, mode) 16 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 17 | File "C:\Users\mbgpcsk4\.platformio\packages\tool-esptoolpy\esptool\loader.py", line 542, in _connect_attempt 18 | self.sync() 19 | File "C:\Users\mbgpcsk4\.platformio\packages\tool-esptoolpy\esptool\loader.py", line 464, in sync 20 | val, _ = self.command( 21 | ^^^^^^^^^^^^^ 22 | File "C:\Users\mbgpcsk4\.platformio\packages\tool-esptoolpy\esptool\loader.py", line 404, in command 23 | p = self.read() 24 | ^^^^^^^^^^^ 25 | File "C:\Users\mbgpcsk4\.platformio\packages\tool-esptoolpy\esptool\loader.py", line 337, in read 26 | return next(self._slip_reader) 27 | ^^^^^^^^^^^^^^^^^^^^^^^ 28 | File "C:\Users\mbgpcsk4\.platformio\packages\tool-esptoolpy\esptool\loader.py", line 1506, in slip_reader 29 | read_bytes = port.read(1 if waiting == 0 else waiting) 30 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 31 | File "C:\Users\mbgpcsk4\AppData\Local\Programs\Python\Python312\Lib\site-packages\serial\serialwin32.py", line 288, in read 32 | result_ok = win32.GetOverlappedResult( 33 | ^^^^^^^^^^^^^^^^^^^^^^^^^^ 34 | KeyboardInterrupt 35 | ^C -------------------------------------------------------------------------------- /src/mavesp8266_httpd.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015, 2016 Gus Grubba. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 19 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 20 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 22 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 23 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 24 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 26 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | ****************************************************************************/ 30 | 31 | /** 32 | * @file mavesp8266_httpd.h 33 | * ESP8266 Wifi AP, MavLink UART/UDP Bridge 34 | * 35 | * @author Gus Grubba 36 | */ 37 | 38 | #ifndef MAVESP8266_HTTPD_H 39 | #define MAVESP8266_HTTPD_H 40 | 41 | #include "mavesp8266.h" 42 | 43 | class MavESP8266Httpd { 44 | public: 45 | MavESP8266Httpd(); 46 | void begin (MavESP8266Update* updateCB); 47 | void checkUpdates (); 48 | }; 49 | 50 | #endif 51 | 52 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Beyond Robotix Kahuna Firmware 2 | 3 | This is the repository for the Kahuna Mavlink Wifi Module for Ardupilot, PX4 and iNAV. 4 | - The docs can be found here: [Docs](https://beyond-robotix.gitbook.io/docs/kahuna) 5 | - The store page here: [Store](https://www.beyondrobotix.com/products/kahuna) 6 | 7 | Specifications: 8 | - Mass: 4g 9 | - Dimensions: 30x18x6mm 10 | - Max Output Power: 20dBm (~112 mW) 11 | - Range: up to 2.5km 12 | - Input Voltage: 5V 13 | - Continuous Current: 20mA 14 | - Peak Current: 250mA 15 | - Connector: JST-GH 6 Pin (compatible with Pixhawk and Cube Telem port) 16 | 17 | 18 | ## Current Binary 19 | 20 | The latest release is available in the releases section on the right-hand side. Download the .bin file and upload it to your kahuna via the web UI to update. 21 | 22 | ### Useful commands: 23 | 24 | * ```platformio run``` - process/build all targets 25 | * ```platformio run -e esp12e``` - process/build just the ESP12e target (the NodeMcu v2, Adafruit HUZZAH, etc.) 26 | * ```platformio run -e esp12e -t upload``` - build and upload firmware to embedded board 27 | * ```platformio run -t clean``` - clean project (remove compiled files) 28 | 29 | The resulting image(s) can be found in the directory ```.pioenvs``` created during the build process. 30 | 31 | ### MavLink Submodule 32 | 33 | The ```git clone --recursive``` above not only cloned the MavESP8266 repository but it also installed the dependent [MavLink](https://github.com/mavlink/c_library) sub-module. To upated the module (when needed), use the command: 34 | 35 | ```git submodule update --init``` 36 | 37 | ### Wiring it up 38 | 39 | User level (as well as wiring) instructions can be found [here for px4](https://docs.px4.io/en/telemetry/esp8266_wifi_module.html) and [here for ArduPilot](http://ardupilot.org/copter/docs/common-esp8266-telemetry.html) 40 | 41 | * Resetting to Defaults: Parameters can be reset to default by jumping J2 in the first 5 seconds after powering the Kahuna. 42 | 43 | ### MavLink Protocol 44 | 45 | The MavESP8266 handles its own set of parameters and commands. Look at the [PARAMETERS](PARAMETERS.md) page for more information. 46 | 47 | ### HTTP Protocol 48 | 49 | There are some preliminary URLs that can be used for checking the WiFi Bridge status as well as updating firmware and changing parameters. [You can find it here.](HTTP.md) 50 | -------------------------------------------------------------------------------- /src/mavesp8266_gcs.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015, 2016 Gus Grubba. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 19 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 20 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 22 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 23 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 24 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 26 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | ****************************************************************************/ 30 | 31 | /** 32 | * @file mavesp8266_gcs.h 33 | * ESP8266 Wifi AP, MavLink UART/UDP Bridge 34 | * 35 | * @author Gus Grubba 36 | */ 37 | 38 | #ifndef MAVESP8266_GCS_H 39 | #define MAVESP8266_GCS_H 40 | 41 | #include "mavesp8266.h" 42 | #include "led_manager.h" 43 | 44 | class MavESP8266GCS : public MavESP8266Bridge 45 | { 46 | public: 47 | MavESP8266GCS(LEDManager &ledManager); 48 | 49 | void begin(MavESP8266Bridge *forwardTo, IPAddress gcsIP); 50 | void readMessage(); 51 | void readMessageRaw(); 52 | int sendMessage(mavlink_message_t *message, int count); 53 | int sendMessage(mavlink_message_t *message); 54 | int sendMessageRaw(uint8_t *buffer, int len); 55 | 56 | protected: 57 | void _sendRadioStatus(); 58 | 59 | private: 60 | bool _readMessage(); 61 | void _sendSingleUdpMessage(mavlink_message_t *msg); 62 | void _checkUdpErrors(mavlink_message_t *msg); 63 | 64 | private: 65 | WiFiUDP _udp; 66 | IPAddress _ip; 67 | uint16_t _udp_port; 68 | mavlink_message_t _message; 69 | unsigned long _last_status_time; 70 | LEDManager &_ledManager; 71 | }; 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /src/mavesp8266_vehicle.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015, 2016 Gus Grubba. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 19 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 20 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 22 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 23 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 24 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 26 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | ****************************************************************************/ 30 | 31 | /** 32 | * @file mavesp8266_vehicle.h 33 | * ESP8266 Wifi AP, MavLink UART/UDP Bridge 34 | * 35 | * @author Gus Grubba 36 | */ 37 | 38 | #ifndef MAVESP8266_VEHICLE_H 39 | #define MAVESP8266_VEHICLE_H 40 | 41 | #include "mavesp8266.h" 42 | #include "led_manager.h" 43 | 44 | //-- UDP Outgoing Packet Queue 45 | #define UAS_QUEUE_SIZE 60 46 | #define UAS_QUEUE_THRESHOLD 20 47 | #define UAS_QUEUE_TIMEOUT 5 // 5ms 48 | 49 | class MavESP8266Vehicle : public MavESP8266Bridge 50 | { 51 | public: 52 | MavESP8266Vehicle(LEDManager &ledManager); 53 | void begin(MavESP8266Bridge *forwardTo); 54 | void readMessage(); 55 | void readMessageRaw(); 56 | int sendMessage(mavlink_message_t *message, int count); 57 | int sendMessage(mavlink_message_t *message); 58 | int sendMessageRaw(uint8_t *buffer, int len); 59 | linkStatus *getStatus(); 60 | 61 | protected: 62 | void _sendRadioStatus(); 63 | 64 | private: 65 | bool _readMessage(); 66 | 67 | private: 68 | int _queue_count; 69 | unsigned long _queue_time; 70 | float _buffer_status; 71 | LEDManager &_ledManager; 72 | mavlink_message_t _message[UAS_QUEUE_SIZE]; 73 | }; 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /src/mavesp8266_component.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015, 2016 Gus Grubba. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 19 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 20 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 22 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 23 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 24 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 26 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | ****************************************************************************/ 30 | 31 | /** 32 | * @file mavesp8266_component.h 33 | * ESP8266 Wifi AP, MavLink UART/UDP Bridge 34 | * 35 | * @author Gus Grubba 36 | */ 37 | 38 | #ifndef MAVESP8266_COMPONENT_H 39 | #define MAVESP8266_COMPONENT_H 40 | 41 | #include "mavesp8266.h" 42 | 43 | class MavESP8266Component { 44 | public: 45 | MavESP8266Component(); 46 | 47 | //- Returns true if the component consumed the message 48 | bool handleMessage (MavESP8266Bridge* sender, mavlink_message_t* message); 49 | bool inRawMode (); 50 | void resetRawMode () { _in_raw_mode_time = millis(); } 51 | 52 | private: 53 | void _sendStatusMessage (MavESP8266Bridge* sender, uint8_t type, const char* text); 54 | void _handleParamSet (MavESP8266Bridge* sender, mavlink_param_set_t* param); 55 | void _handleParamRequestList (MavESP8266Bridge* sender); 56 | void _handleParamRequestRead (MavESP8266Bridge* sender, mavlink_param_request_read_t* param); 57 | void _sendParameter (MavESP8266Bridge* sender, uint16_t index); 58 | void _sendParameter (MavESP8266Bridge* sender, const char* id, uint32_t value, uint16_t index); 59 | 60 | void _handleCmdLong (MavESP8266Bridge* sender, mavlink_command_long_t* cmd, uint8_t compID); 61 | 62 | void _wifiReboot (MavESP8266Bridge* sender); 63 | 64 | bool _in_raw_mode; 65 | unsigned long _in_raw_mode_time; 66 | }; 67 | 68 | #endif 69 | -------------------------------------------------------------------------------- /HTTP.md: -------------------------------------------------------------------------------- 1 | ## MavESP8266 2 | ### MavESP8266 Web Interface 3 | 4 | #### API 5 | 6 | The idea is to have some sort of web interface so you can check the status and change parameters. While that is not done, there are however, a few URLs that can be used. Obviously, you need to be connected to the WiFi Bridge's Access Point for these URLs to work. 7 | 8 | ##### Get Parameters 9 | 10 | http://192.168.4.1/getparameters 11 | 12 | This will show the current parameters and their values. 13 | 14 | ##### Get Status 15 | 16 | http://192.168.4.1/getstatus 17 | 18 | This will show the current comm link status. 19 | 20 | ##### Set Parameters 21 | 22 | http://192.168.4.1/setparameters?key=value&key=value 23 | 24 | This will allow you to set any parameter to the specified value. Once set, the values are stored in non volatile memory (EEPROM) but will only take effect once you reboot it. Use this with caution as you may lock yourself out. For instance, if you change the AP password and don't remember later, currently you must either trigger the reset pin (see README) or reflash the module in order to revert the parameters. Also note that there is no validation done to the values entered. 25 | 26 | There are the supported parameters: 27 | 28 | | Key | Default Value | Description | Example | 29 | | ------------- | -------------- | -------------- | -------------- | 30 | | baud | 57600 | UAS UART Link Baud Rate | http://192.168.4.1/setparameters?baud=57600 | 31 | | channel | 11 | AP WiFi Channel | http://192.168.4.1/setparameters?channel=11 | 32 | | cport | 14555 | Local UDP Port | http://192.168.4.1/setparameters?cport=14555 | 33 | | debug | 0 | Enable Debug Messages | http://192.168.4.1/setparameters?debug=0 | 34 | | hport | 14550 | GCS UDP Port | http://192.168.4.1/setparameters?hport=14550 | 35 | | mode | 0 | Set to AP Mode (0) or Station Mode (1) | http://192.168.4.1/setparameters?mode=1 | 36 | | pwd | robotix1 | WiFi AP Password | http://192.168.4.1/setparameters?pwd=robotix1 | 37 | | pwdsta | robotix1 | WiFi STA Password | http://192.168.4.1/setparameters?pwdsta=robotix1 | 38 | | reboot | 0 | Reboot the WiFi Bridge | http://192.168.4.1/setparameters?reboot=1 | 39 | | ssid | Beyond1 | WiFi AP SSID | http://192.168.4.1/setparameters?ssid=Beyond1 | 40 | | ssidsta | Beyond1 | WiFi STA SSID | http://192.168.4.1/setparameters?ssidsta=Beyond1 | 41 | | ipsta | 0.0.0.0 | Wifi STA Static IP | http://192.168.4.1/setparameters?ipsta=192.168.4.2 | 42 | | gatewaysta | 0.0.0.0 | Wifi STA Gateway | http://192.168.4.1/setparameters?gatewaysta=192.168.4.1 | 43 | | subnetsta | 0.0.0.0 | Wifi STA Subnet | http://192.168.4.1/setparameters?subnetsta=255.255.255.0 | 44 | 45 | You can combine any number of parameters into one request. For example: 46 | 47 | http://192.168.4.1/setparameters?baud=57600&channel=9&reboot=1 48 | 49 | To connect to a typical existing network with a static ip: 50 | 51 | http://192.168.4.1/setparameters?mode=1&ssidsta=networkname&pwdsta=thepassword&ipsta=192.168.1.123&gatewaysta=192.168.1.1&subnetsta=255.255.255.0 52 | 53 | ##### Upload New Firmware 54 | 55 | http://192.168.4.1/update 56 | 57 | This will bring up a simple page with a button to allow you to pick the new ```firmware.bin``` file to upload and a button to upload it to the WiFi Bridge. Again, caution when using this as there is no validation yet. 58 | -------------------------------------------------------------------------------- /src/led_manager.cpp: -------------------------------------------------------------------------------- 1 | #include "led_manager.h" 2 | 3 | void LEDManager::setLED(Led selectedLed, LedStatus ledStatus) 4 | { 5 | switch (selectedLed) 6 | { 7 | case gcs: 8 | if (_gcsLedStatus != ledStatus) 9 | { 10 | _gcsLedStatus = ledStatus; 11 | switch (_gcsLedStatus) 12 | { 13 | case off: 14 | digitalWrite(gcs, LOW); 15 | _gcsValue = LOW; 16 | break; 17 | case on: 18 | digitalWrite(gcs, HIGH); 19 | _gcsValue = HIGH; 20 | break; 21 | default: 22 | break; 23 | } 24 | } 25 | break; 26 | case wifi: 27 | if (_wifiLedStatus != ledStatus) 28 | { 29 | _wifiLedStatus = ledStatus; 30 | switch (_wifiLedStatus) 31 | { 32 | case off: 33 | digitalWrite(wifi, LOW); 34 | _wifiValue = LOW; 35 | break; 36 | case on: 37 | digitalWrite(wifi, HIGH); 38 | _wifiValue = HIGH; 39 | break; 40 | case doubleBlink: 41 | digitalWrite(wifi, LOW); 42 | _wifiValue = LOW; 43 | default: 44 | break; 45 | } 46 | } 47 | break; 48 | case air: 49 | if (_airLedStatus != ledStatus) 50 | { 51 | _airLedStatus = ledStatus; 52 | switch (_airLedStatus) 53 | { 54 | case off: 55 | digitalWrite(air, LOW); 56 | _airValue = LOW; 57 | break; 58 | case on: 59 | digitalWrite(air, HIGH); 60 | _airValue = HIGH; 61 | default: 62 | break; 63 | } 64 | } 65 | break; 66 | default: 67 | break; 68 | } 69 | } 70 | void LEDManager::blinkLED() 71 | { 72 | if (millis() >= _timeNextBlink) 73 | { 74 | _timeNextBlink = millis() + _cycleTime; 75 | if (_gcsLedStatus == blink) 76 | { 77 | _gcsValue = !_gcsValue; 78 | digitalWrite(gcs, _gcsValue); 79 | } 80 | if (_wifiLedStatus == blink) 81 | { 82 | _wifiValue = !_wifiValue; 83 | digitalWrite(wifi, _wifiValue); 84 | } 85 | if (_airLedStatus == blink) 86 | { 87 | _airValue = !_airValue; 88 | digitalWrite(air, _airValue); 89 | } 90 | } 91 | } 92 | 93 | // double blink on for 200, off for 200, on for 200, off for 600 94 | 95 | void LEDManager::doubleBlinkLED() 96 | { 97 | if (millis() >= _timeNextDoubleBlink && _doubleBlinkCount < 3) 98 | { 99 | _doubleBlinkCount++; 100 | _timeNextDoubleBlink = millis() + (_cycleTime / 3); 101 | if (_wifiLedStatus == doubleBlink) 102 | { 103 | _wifiValue = !_wifiValue; 104 | digitalWrite(wifi, _wifiValue); 105 | } 106 | } 107 | else if (millis() >= _timeNextDoubleBlink) 108 | { 109 | _doubleBlinkCount = 0; 110 | _timeNextDoubleBlink = millis() + _cycleTime; 111 | if (_wifiLedStatus == doubleBlink) 112 | { 113 | _wifiValue = !_wifiValue; 114 | digitalWrite(wifi, _wifiValue); 115 | } 116 | } 117 | } -------------------------------------------------------------------------------- /PARAMETERS.md: -------------------------------------------------------------------------------- 1 | ## MavESP8266 2 | ### Mavlink Paramaters and Messages supported by the MavESP8266 Firmware 3 | 4 | #### Parameters 5 | 6 | The MavESP8266 uses ```MAV_COMP_ID_UDP_BRIDGE``` (240) as its component id. It will process messages sent to this component id or messages sent to ```MAV_COMP_ID_ALL``` only. 7 | 8 | ##### MAVLINK_MSG_ID_PARAM_REQUEST_LIST 9 | 10 | If this message is sent to *All Components* (```MAV_COMP_ID_ALL```), or specifically to the MavESP8266 component ID, it will return its list of parameters. Messages sent to ```MAV_COMP_ID_ALL``` will be forwarded to the UAS as well. 11 | 12 | ##### MAVLINK_MSG_ID_PARAM_REQUEST_READ 13 | 14 | If this message is sent specifically to the MavESP8266 component ID, it will return the requested parameter (either by ID or by Index) 15 | 16 | ##### MAVLINK_MSG_ID_PARAM_SET 17 | 18 | If this message is sent specifically to the MavESP8266 component ID, it will set the new value for the specified parameter. Note that this only sets the value for the current session. It does not write the values to EEPROM. See MAV_CMD_PREFLIGHT_STORAGE below. 19 | 20 | #### Available Parameters 21 | 22 | | Parameter ID | Parameter Type | Description | 23 | | ------------- | -------------- | ----------- | 24 | | DEBUG_ENABLED | MAV_PARAM_TYPE_INT8 | Enable Debug Messages (1) | 25 | | SW_VER | MAV_PARAM_TYPE_UINT32 | Firmware Version (Read Only) | 26 | | UART_BAUDRATE | MAV_PARAM_TYPE_UINT32 | UAS UART Link Baud Rate (default to 57600) | 27 | | WIFI_CHANNEL | MAV_PARAM_TYPE_UINT32 | AP WiFi Channel (default to 11) | 28 | | WIFI_IPADDRESS | MAV_PARAM_TYPE_UINT32 | Local IP Address (default to 192.168.4.1 when in AP Mode) (Read Only) | 29 | | WIFI_MODE | MAV_PARAM_TYPE_INT8 | WiFi Operating Mode (3) | 30 | | WIFI_PASSWORD1 | MAV_PARAM_TYPE_UINT32 | WiFi AP Password (2) | 31 | | WIFI_PASSWORD2 | MAV_PARAM_TYPE_UINT32 | | 32 | | WIFI_PASSWORD3 | MAV_PARAM_TYPE_UINT32 | | 33 | | WIFI_PASSWORD4 | MAV_PARAM_TYPE_UINT32 | | 34 | | WIFI_PWDSTA1 | MAV_PARAM_TYPE_UINT32 | WiFi STA Password (2) | 35 | | WIFI_PWDSTA2 | MAV_PARAM_TYPE_UINT32 | | 36 | | WIFI_PWDSTA3 | MAV_PARAM_TYPE_UINT32 | | 37 | | WIFI_PWDSTA4 | MAV_PARAM_TYPE_UINT32 | | 38 | | WIFI_SSID1 | MAV_PARAM_TYPE_UINT32 | WiFi AP SSID (2) | 39 | | WIFI_SSID2 | MAV_PARAM_TYPE_UINT32 | | 40 | | WIFI_SSID3 | MAV_PARAM_TYPE_UINT32 | | 41 | | WIFI_SSID4 | MAV_PARAM_TYPE_UINT32 | | 42 | | WIFI_SSIDSTA1 | MAV_PARAM_TYPE_UINT32 | WiFi STA SSID (2) | 43 | | WIFI_SSIDSTA2 | MAV_PARAM_TYPE_UINT32 | | 44 | | WIFI_SSIDSTA3 | MAV_PARAM_TYPE_UINT32 | | 45 | | WIFI_SSIDSTA4 | MAV_PARAM_TYPE_UINT32 | | 46 | | WIFI_IPSTA | MAV_PARAM_TYPE_UINT32 | Wifi STA Static IP Address (4) | 47 | | WIFI_GATEWAYSTA | MAV_PARAM_TYPE_UINT32 | Wifi STA Gateway Address (4) | 48 | | WIFI_SUBNETSTA | MAV_PARAM_TYPE_UINT32 | Wifi STA Subnet Address (4) | 49 | | WIFI_UDP_CPORT | MAV_PARAM_TYPE_UINT16 | Local UDP Port (default to 14555) | 50 | | WIFI_UDP_HPORT | MAV_PARAM_TYPE_UINT16 | GCS UDP Port (default to 14550) | 51 | 52 | ##### Notes 53 | 54 | * (1) If debug is enabled, debug messages are sent using ```MAVLINK_MSG_ID_STATUSTEXT``` with a proper ```MAV_SEVERITY_DEBUG``` type. Other messages types, ```MAV_SEVERITY_NOTICE``` for example, are sent regardless. 55 | * (2) MavLink parameter messages only support a 32-Bit parameter (be it a float, an uint32_t, etc.) In other to fit a 16-character SSID and a 16-character Password, 4 paramaters are used for each. The 32-Bit storage is used to contain 4 bytes for the string. 56 | * (3) The mode defaults to 0. Set to 0 to act as an Access Point. Set to 1 to connect to an existing WiFi network using the STA (Station Mode) SSID and password. When in *Station Mode*, the module will attempt to connect for up to one minute. If after that it cannot connect, it reverts to AP mode. 57 | * (4) Defaults to 0 for an unset address. If either the STA IP, Gateway, or Subnet are set, then all three need to be set for it to work properly. 58 | 59 | #### MAVLINK_MSG_ID_COMMAND_LONG 60 | 61 | In addition to parameters, MavESP8266 also supports a few commands, which will be handled _**if addressed to its component ID**_: 62 | 63 | ##### MAV_CMD_PREFLIGHT_STORAGE 64 | 65 | * If ```param1``` == 0 It will load all parameters from EEPROM overwriting any changes. 66 | * If ```param1``` == 1 It will save all current parameters to EEPROM. 67 | * If ```param1``` == 2 It will reset all parameters to the original default values. Note that it will not store them to EEPROM. You must request that separately if that's what you want to do. 68 | 69 | ##### MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN 70 | 71 | * If ```param2``` == 1 It will cause the ESP8266 module to reboot. This is necessary if you want parameter changes to take effect. Out of the above, the only parameter that takes effect immediatly upon changing is **DEBUG_ENABLED**. All other values will only take effect at boot time. 72 | -------------------------------------------------------------------------------- /src/mavesp8266_parameters.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015, 2016 Gus Grubba. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 19 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 20 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 22 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 23 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 24 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 26 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | ****************************************************************************/ 30 | 31 | /** 32 | * @file mavesp8266_parameters.h 33 | * ESP8266 Wifi AP, MavLink UART/UDP Bridge 34 | * 35 | * @author Gus Grubba 36 | */ 37 | 38 | #ifndef MAVESP8266_PARAMETERS_H 39 | #define MAVESP8266_PARAMETERS_H 40 | 41 | #define WIFI_MODE_AP 0 42 | #define WIFI_MODE_STA 1 43 | 44 | //-- Constants 45 | #define DEFAULT_WIFI_MODE WIFI_MODE_AP 46 | #define DEFAULT_UART_SPEED 57600 47 | #define DEFAULT_WIFI_CHANNEL 11 48 | #define DEFAULT_UDP_HPORT 14550 49 | #define DEFAULT_UDP_CPORT 14555 50 | 51 | struct stMavEspParameters 52 | { 53 | char id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; 54 | void *value; 55 | uint16_t index; 56 | uint8_t length; 57 | uint8_t type; 58 | bool readOnly; 59 | }; 60 | 61 | class MavESP8266Parameters 62 | { 63 | public: 64 | MavESP8266Parameters(); 65 | 66 | enum 67 | { 68 | ID_FWVER = 0, 69 | ID_DEBUG, 70 | ID_MODE, 71 | ID_CHANNEL, 72 | ID_HPORT, 73 | ID_CPORT, 74 | ID_IPADDRESS, 75 | ID_SSID1, 76 | ID_SSID2, 77 | ID_SSID3, 78 | ID_SSID4, 79 | ID_PASS1, 80 | ID_PASS2, 81 | ID_PASS3, 82 | ID_PASS4, 83 | ID_SSIDSTA1, 84 | ID_SSIDSTA2, 85 | ID_SSIDSTA3, 86 | ID_SSIDSTA4, 87 | ID_PASSSTA1, 88 | ID_PASSSTA2, 89 | ID_PASSSTA3, 90 | ID_PASSSTA4, 91 | ID_IPSTA, 92 | ID_GATEWAYSTA, 93 | ID_SUBNETSTA, 94 | ID_UART, 95 | ID_COUNT 96 | }; 97 | 98 | void begin(); 99 | void loadAllFromEeprom(); 100 | uint32_t paramHashCheck(); 101 | void resetToDefaults(); 102 | void saveAllToEeprom(); 103 | 104 | uint32_t getSwVersion(); 105 | int8_t getDebugEnabled(); 106 | int8_t getWifiMode(); 107 | uint32_t getLocalIPAddress(); 108 | uint32_t getWifiChannel(); 109 | uint16_t getWifiUdpHport(); 110 | uint16_t getWifiUdpCport(); 111 | char *getWifiSsid(); 112 | char *getWifiPassword(); 113 | char *getWifiStaSsid(); 114 | char *getWifiStaPassword(); 115 | uint32_t getWifiStaIP(); 116 | uint32_t getWifiStaGateway(); 117 | uint32_t getWifiStaSubnet(); 118 | uint32_t getUartBaudRate(); 119 | 120 | void setDebugEnabled(int8_t enabled); 121 | void setWifiMode(int8_t mode); 122 | void setWifiChannel(uint32_t channel); 123 | void setWifiUdpHport(uint16_t port); 124 | void setWifiUdpCport(uint16_t port); 125 | void setWifiSsid(const char *ssid); 126 | void setWifiPassword(const char *pwd); 127 | void setWifiStaSsid(const char *ssid); 128 | void setWifiStaPassword(const char *pwd); 129 | void setWifiStaIP(uint32_t addr); 130 | void setWifiStaGateway(uint32_t addr); 131 | void setWifiStaSubnet(uint32_t addr); 132 | void setUartBaudRate(uint32_t baud); 133 | void setLocalIPAddress(uint32_t ipAddress); 134 | 135 | stMavEspParameters *getAt(int index); 136 | 137 | private: 138 | uint32_t _crc32part(uint8_t *value, uint32_t len, uint32_t crc); 139 | uint32_t _getEepromCrc(); 140 | void _initEeprom(); 141 | 142 | private: 143 | }; 144 | 145 | #endif 146 | -------------------------------------------------------------------------------- /src/crc.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015, 2016 Gus Grubba. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 19 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 20 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 22 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 23 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 24 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 26 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | ****************************************************************************/ 30 | 31 | /** 32 | * @file crc.h 33 | * ESP8266 Wifi AP, MavLink UART/UDP Bridge 34 | * 35 | * @author Gus Grubba 36 | */ 37 | 38 | //-- 16-Entry CRC Lookup Table 39 | static uint32_t crc_table[] = { 40 | 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, 41 | 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 42 | 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, 43 | 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, 44 | 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, 45 | 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, 46 | 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, 47 | 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, 48 | 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, 49 | 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, 50 | 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 51 | 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, 52 | 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, 53 | 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, 54 | 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, 55 | 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, 56 | 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, 57 | 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, 58 | 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, 59 | 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, 60 | 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, 61 | 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, 62 | 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 63 | 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, 64 | 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, 65 | 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, 66 | 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, 67 | 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, 68 | 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 69 | 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, 70 | 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, 71 | 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d 72 | }; 73 | -------------------------------------------------------------------------------- /src/mavesp8266.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015, 2016 Gus Grubba. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 19 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 20 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 22 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 23 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 24 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 26 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | ****************************************************************************/ 30 | 31 | /** 32 | * @file mavesp8266.cpp 33 | * ESP8266 Wifi AP, MavLink UART/UDP Bridge 34 | * 35 | * @author Gus Grubba 36 | */ 37 | 38 | #include "mavesp8266.h" 39 | #include "mavesp8266_parameters.h" 40 | 41 | //--------------------------------------------------------------------------------- 42 | //-- Base Comm Link 43 | MavESP8266Bridge::MavESP8266Bridge() 44 | : _heard_from(false) 45 | , _system_id(0) 46 | , _component_id(0) 47 | , _seq_expected(0) 48 | , _last_heartbeat(0) 49 | , _last_status_time(0) 50 | , _forwardTo(NULL) 51 | { 52 | memset(&_status, 0, sizeof(_status)); 53 | } 54 | 55 | //--------------------------------------------------------------------------------- 56 | //-- Initialize 57 | void 58 | MavESP8266Bridge::begin(MavESP8266Bridge* forwardTo) 59 | { 60 | _forwardTo = forwardTo; 61 | } 62 | 63 | //--------------------------------------------------------------------------------- 64 | //-- Check for link errors 65 | void 66 | MavESP8266Bridge::_checkLinkErrors(mavlink_message_t* msg) 67 | { 68 | //-- Don't bother if we have not heard from the link (and it's the proper sys/comp ids) 69 | if(!_heard_from || msg->sysid != _system_id || msg->compid != _component_id) { 70 | return; 71 | } 72 | uint16_t seq_received = (uint16_t)msg->seq; 73 | uint16_t packet_lost_count = 0; 74 | //-- Account for overflow during packet loss 75 | if(seq_received < _seq_expected) { 76 | packet_lost_count = (seq_received + 255) - _seq_expected; 77 | } else { 78 | packet_lost_count = seq_received - _seq_expected; 79 | } 80 | _seq_expected = msg->seq + 1; 81 | _status.packets_lost += packet_lost_count; 82 | } 83 | 84 | //--------------------------------------------------------------------------------- 85 | MavESP8266Log:: MavESP8266Log() 86 | : _buffer(NULL) 87 | , _buffer_size(0) 88 | , _log_offset(0) 89 | , _log_position(0) 90 | { 91 | 92 | } 93 | 94 | //--------------------------------------------------------------------------------- 95 | void 96 | MavESP8266Log::begin(size_t bufferSize) 97 | { 98 | _buffer_size = bufferSize & 0xFFFE; 99 | _buffer = (char*)malloc(_buffer_size); 100 | } 101 | 102 | //--------------------------------------------------------------------------------- 103 | size_t 104 | MavESP8266Log::log(const char *format, ...) { 105 | va_list arg; 106 | va_start(arg, format); 107 | char temp[1024]; 108 | size_t len = ets_vsnprintf(temp, 1024, format, arg); 109 | #ifdef ENABLE_DEBUG 110 | Serial1.print(temp); 111 | #endif 112 | 113 | if(_buffer) { 114 | for(int i = 0; i < (int)len; i++) { 115 | _buffer[_log_offset] = temp[i]; 116 | _log_offset = (_log_offset + 1) % _buffer_size; 117 | _log_position++; 118 | } 119 | } 120 | va_end(arg); 121 | return len; 122 | } 123 | 124 | //--------------------------------------------------------------------------------- 125 | String 126 | MavESP8266Log::getLog(uint32_t* pStart, uint32_t* pLen) { 127 | String buffer; 128 | 129 | uint32_t position = *pStart, len = getLogSize(); 130 | if (position < _log_position - len) { //-- Can't read data that was overriden 131 | position = _log_position - len; 132 | } else if (position > _log_position) { //-- Can't read data from the future 133 | position = _log_position; 134 | } 135 | len = _log_position - position; 136 | *pStart = position; 137 | *pLen = len; 138 | 139 | uint32_t r = (_log_offset + (_buffer_size - len)) % _buffer_size; 140 | while(len > 0) { 141 | char c = _buffer[r]; 142 | //-- Copy as JSON encoded characters 143 | if (c == '\\' || c == '"') { 144 | buffer += '\\'; 145 | buffer += c; 146 | } else if (c < ' ') { 147 | char tmp[12]; 148 | snprintf(tmp, 12, "\\u%04x", c); 149 | buffer += tmp; 150 | } else { 151 | buffer += c; 152 | } 153 | r = (r + 1) % _buffer_size; 154 | len--; 155 | } 156 | return buffer; 157 | } 158 | 159 | //--------------------------------------------------------------------------------- 160 | uint32_t 161 | MavESP8266Log::getLogSize() 162 | { 163 | return min(_log_position, _buffer_size); 164 | } 165 | 166 | //--------------------------------------------------------------------------------- 167 | uint32_t 168 | MavESP8266Log::getPosition() 169 | { 170 | return _log_position; 171 | } 172 | -------------------------------------------------------------------------------- /src/mavesp8266.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015, 2016 Gus Grubba. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 19 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 20 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 22 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 23 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 24 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 26 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | ****************************************************************************/ 30 | 31 | /** 32 | * @file mavesp8266.h 33 | * ESP8266 Wifi AP, MavLink UART/UDP Bridge 34 | * 35 | * @author Gus Grubba 36 | */ 37 | 38 | #ifndef MAVESP8266_H 39 | #define MAVESP8266_H 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | #undef F 46 | #include 47 | 48 | extern "C" 49 | { 50 | // Espressif SDK 51 | #include "user_interface.h" 52 | } 53 | 54 | class MavESP8266Parameters; 55 | class MavESP8266Component; 56 | class MavESP8266Vehicle; 57 | class MavESP8266GCS; 58 | class MavESP8266Status; 59 | 60 | #define DEFAULT_UART_SPEED 921600 61 | #define DEFAULT_WIFI_CHANNEL 11 62 | #define DEFAULT_UDP_HPORT 14550 63 | #define DEFAULT_UDP_CPORT 14555 64 | 65 | #define HEARTBEAT_TIMEOUT 10 * 1000 66 | 67 | //-- The version is set from the build system (major, minor and build) 68 | #define MAVESP8266_VERSION ((MAVESP8266_VERSION_MAJOR << 24) & 0xFF00000) | ((MAVESP8266_VERSION_MINOR << 16) & 0x00FF0000) | (MAVESP8266_VERSION_BUILD & 0xFFFF) 69 | 70 | //-- Debug sent out to Serial1 (GPIO02), which is TX only (no RX). 71 | // #define ENABLE_DEBUG 72 | 73 | #ifdef ENABLE_DEBUG 74 | #define DEBUG_LOG(format, ...) \ 75 | do \ 76 | { \ 77 | getWorld()->getLogger()->log(format, ##__VA_ARGS__); \ 78 | } while (0) 79 | #else 80 | #define DEBUG_LOG(format, ...) \ 81 | do \ 82 | { \ 83 | } while (0) 84 | #endif 85 | 86 | //--------------------------------------------------------------------------------- 87 | //-- Link Status 88 | struct linkStatus 89 | { 90 | uint32_t packets_received; 91 | uint32_t packets_lost; 92 | uint32_t packets_sent; 93 | uint32_t radio_status_sent; 94 | uint8_t queue_status; 95 | }; 96 | //--------------------------------------------------------------------------------- 97 | //-- Base Comm Link 98 | class MavESP8266Bridge 99 | { 100 | public: 101 | MavESP8266Bridge(); 102 | virtual ~MavESP8266Bridge() { ; } 103 | virtual void begin(MavESP8266Bridge *forwardTo); 104 | virtual void readMessage() = 0; 105 | virtual void readMessageRaw() = 0; 106 | virtual int sendMessage(mavlink_message_t *message, int count) = 0; 107 | virtual int sendMessage(mavlink_message_t *message) = 0; 108 | virtual int sendMessageRaw(uint8_t *buffer, int len) = 0; 109 | virtual bool heardFrom() { return _heard_from; } 110 | virtual uint8_t systemID() { return _system_id; } 111 | virtual uint8_t componentID() { return _component_id; } 112 | virtual linkStatus *getStatus() { return &_status; } 113 | mavlink_channel_t _send_chan; 114 | mavlink_channel_t _recv_chan; 115 | 116 | protected: 117 | virtual void _checkLinkErrors(mavlink_message_t *msg); 118 | virtual void _sendRadioStatus() = 0; 119 | 120 | protected: 121 | bool _heard_from; 122 | uint8_t _system_id; 123 | uint8_t _component_id; 124 | uint8_t _seq_expected; 125 | uint32_t _last_heartbeat; 126 | linkStatus _status; 127 | unsigned long _last_status_time; 128 | MavESP8266Bridge *_forwardTo; 129 | mavlink_status_t _mav_status; 130 | mavlink_message_t _rxmsg; 131 | mavlink_status_t _rxstatus; 132 | }; 133 | 134 | //--------------------------------------------------------------------------------- 135 | //-- Logger 136 | class MavESP8266Log 137 | { 138 | public: 139 | MavESP8266Log(); 140 | void begin(size_t bufferSize); // Allocate a buffer for the log 141 | size_t log(const char *format, ...); // Add to the log 142 | String getLog(uint32_t *pStart, uint32_t *pLen); // Get the log starting at a position 143 | uint32_t getLogSize(); // Number of bytes available at the current log position 144 | uint32_t getPosition(); 145 | 146 | private: 147 | char *_buffer; // Raw memory 148 | size_t _buffer_size; // Size of the above memory 149 | uint32_t _log_offset; // Position in the buffer 150 | uint32_t _log_position; // Absolute position in the log since boot 151 | }; 152 | 153 | //--------------------------------------------------------------------------------- 154 | //-- Accessors 155 | class MavESP8266World 156 | { 157 | public: 158 | virtual ~MavESP8266World() { ; } 159 | virtual MavESP8266Parameters *getParameters() = 0; 160 | virtual MavESP8266Component *getComponent() = 0; 161 | virtual MavESP8266Vehicle *getVehicle() = 0; 162 | virtual MavESP8266GCS *getGCS() = 0; 163 | virtual MavESP8266Log *getLogger() = 0; 164 | }; 165 | 166 | //--------------------------------------------------------------------------------- 167 | //-- HTTP Update Status 168 | class MavESP8266Update 169 | { 170 | public: 171 | virtual ~MavESP8266Update() { ; } 172 | virtual void updateStarted() = 0; 173 | virtual void updateCompleted() = 0; 174 | virtual void updateError() = 0; 175 | }; 176 | 177 | extern MavESP8266World *getWorld(); 178 | 179 | #endif 180 | -------------------------------------------------------------------------------- /src/mavesp8266_vehicle.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015, 2016 Gus Grubba. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 19 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 20 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 22 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 23 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 24 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 26 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | ****************************************************************************/ 30 | 31 | /** 32 | * @file mavesp8266_vehicle.cpp 33 | * ESP8266 Wifi AP, MavLink UART/UDP Bridge 34 | * 35 | * @author Gus Grubba 36 | */ 37 | 38 | #include "mavesp8266.h" 39 | #include "mavesp8266_vehicle.h" 40 | #include "mavesp8266_parameters.h" 41 | #include "mavesp8266_component.h" 42 | #include "led_manager.h" 43 | //--------------------------------------------------------------------------------- 44 | MavESP8266Vehicle::MavESP8266Vehicle(LEDManager &ledManager) 45 | : _queue_count(0), _queue_time(0), _buffer_status(50.0), _ledManager(ledManager) 46 | { 47 | _recv_chan = MAVLINK_COMM_0; 48 | _send_chan = MAVLINK_COMM_1; 49 | memset(_message, 0, sizeof(_message)); 50 | } 51 | //--------------------------------------------------------------------------------- 52 | //-- Initialize 53 | void MavESP8266Vehicle::begin(MavESP8266Bridge *forwardTo) 54 | { 55 | MavESP8266Bridge::begin(forwardTo); 56 | //-- Start UART connected to UAS 57 | Serial.begin(getWorld()->getParameters()->getUartBaudRate()); 58 | //-- Swap to TXD2/RXD2 (GPIO015/GPIO013) For ESP12 Only 59 | #define ARDUINO_ESP8266_ESP12 60 | #ifdef ENABLE_DEBUG 61 | #ifdef ARDUINO_ESP8266_ESP12 62 | // Serial.swap(); 63 | #endif 64 | #endif 65 | // raise serial buffer size (default is 256) 66 | Serial.setRxBufferSize(1024); 67 | } 68 | 69 | //--------------------------------------------------------------------------------- 70 | //-- Read MavLink message from UAS 71 | void MavESP8266Vehicle::readMessage() 72 | { 73 | if (_queue_count < UAS_QUEUE_SIZE) 74 | { 75 | if (_readMessage()) 76 | { 77 | _queue_count++; 78 | } 79 | } 80 | //-- Do we have a message to send and is it time to forward data? 81 | if (_queue_count && (_queue_count >= UAS_QUEUE_THRESHOLD || (millis() - _queue_time) > UAS_QUEUE_TIMEOUT)) 82 | { 83 | int sent = _forwardTo->sendMessage(_message, _queue_count); 84 | //-- Sent it all? 85 | if (sent == _queue_count) 86 | { 87 | memset(_message, 0, sizeof(_message)); 88 | _queue_count = 0; 89 | _queue_time = millis(); 90 | //-- Sent at least some? 91 | } 92 | else if (sent) 93 | { 94 | //-- Move the pending ones up the queue 95 | int left = _queue_count - sent; 96 | for (int i = 0; i < left; i++) 97 | { 98 | memcpy(&_message[sent + i], &_message[i], sizeof(mavlink_message_t)); 99 | } 100 | _queue_count = left; 101 | } 102 | //-- Maintain buffer status 103 | float cur_status = 0.0; 104 | float buffer_size = (float)UAS_QUEUE_THRESHOLD; 105 | float buffer_left = (float)(UAS_QUEUE_THRESHOLD - _queue_count); 106 | if (buffer_left > 0.0) 107 | cur_status = ((buffer_left / buffer_size) * 100.0f); 108 | _buffer_status = (_buffer_status * 0.05f) + (cur_status * 0.95); 109 | } 110 | //-- Update radio status (1Hz) 111 | if (_heard_from && (millis() - _last_status_time > 1000)) 112 | { 113 | delay(0); 114 | _sendRadioStatus(); 115 | _last_status_time = millis(); 116 | } 117 | } 118 | 119 | void MavESP8266Vehicle::readMessageRaw() 120 | { 121 | char buf[1024]; 122 | int buf_index = 0; 123 | 124 | while (Serial.available() && buf_index < 300) 125 | { 126 | int result = Serial.read(); 127 | if (result >= 0) 128 | { 129 | buf[buf_index] = (char)result; 130 | buf_index++; 131 | } 132 | } 133 | 134 | _forwardTo->sendMessageRaw((uint8_t *)buf, buf_index); 135 | } 136 | 137 | //--------------------------------------------------------------------------------- 138 | //-- Send MavLink message to UAS 139 | int MavESP8266Vehicle::sendMessage(mavlink_message_t *message, int count) 140 | { 141 | for (int i = 0; i < count; i++) 142 | { 143 | sendMessage(&message[i]); 144 | } 145 | return count; 146 | } 147 | 148 | //--------------------------------------------------------------------------------- 149 | //-- Send MavLink message to UAS 150 | int MavESP8266Vehicle::sendMessage(mavlink_message_t *message) 151 | { 152 | // Translate message to buffer 153 | char buf[300]; 154 | unsigned len = mavlink_msg_to_send_buffer((uint8_t *)buf, message); 155 | // Send it 156 | Serial.write((uint8_t *)(void *)buf, len); 157 | _status.packets_sent++; 158 | return 1; 159 | } 160 | 161 | int MavESP8266Vehicle::sendMessageRaw(uint8_t *buffer, int len) 162 | { 163 | Serial.write(buffer, len); 164 | // Serial.flush(); 165 | return len; 166 | } 167 | 168 | //--------------------------------------------------------------------------------- 169 | //-- We have some special status to capture when asked for 170 | linkStatus * 171 | MavESP8266Vehicle::getStatus() 172 | { 173 | _status.queue_status = (uint8_t)_buffer_status; 174 | return &_status; 175 | } 176 | 177 | //--------------------------------------------------------------------------------- 178 | //-- Read MavLink message from UAS 179 | bool MavESP8266Vehicle::_readMessage() 180 | { 181 | bool msgReceived = false; 182 | while (Serial.available()) 183 | { 184 | int result = Serial.read(); 185 | if (result >= 0) 186 | { 187 | // Parsing 188 | msgReceived = mavlink_frame_char_buffer(&_rxmsg, 189 | &_rxstatus, 190 | result, 191 | &_message[_queue_count], 192 | &_mav_status); 193 | if (msgReceived) 194 | { 195 | _status.packets_received++; 196 | //-- Is this the first packet we got? 197 | if (!_heard_from) 198 | { 199 | if (_message[_queue_count].msgid == MAVLINK_MSG_ID_HEARTBEAT) 200 | { 201 | _ledManager.setLED(_ledManager.air, _ledManager.on); 202 | _heard_from = true; 203 | _component_id = _message[_queue_count].compid; 204 | _system_id = _message[_queue_count].sysid; 205 | _seq_expected = _message[_queue_count].seq + 1; 206 | _last_heartbeat = millis(); 207 | } 208 | } 209 | else 210 | { 211 | if (_message[_queue_count].msgid == MAVLINK_MSG_ID_HEARTBEAT) 212 | { 213 | _last_heartbeat = millis(); 214 | } 215 | _checkLinkErrors(&_message[_queue_count]); 216 | } 217 | 218 | if (msgReceived == MAVLINK_FRAMING_BAD_CRC || 219 | msgReceived == MAVLINK_FRAMING_BAD_SIGNATURE) 220 | { 221 | // we don't process messages locally with bad CRC, 222 | // but we do forward them, so when new messages 223 | // are added we can bridge them 224 | break; 225 | } 226 | 227 | //-- Check for message we might be interested 228 | if (getWorld()->getComponent()->handleMessage(this, &_message[_queue_count])) 229 | { 230 | //-- Eat message (don't send it to GCS) 231 | memset(&_message[_queue_count], 0, sizeof(mavlink_message_t)); 232 | msgReceived = false; 233 | continue; 234 | } 235 | 236 | break; 237 | } 238 | } 239 | } 240 | if (!msgReceived) 241 | { 242 | if (_heard_from && (millis() - _last_heartbeat) > HEARTBEAT_TIMEOUT) 243 | { 244 | _ledManager.setLED(_ledManager.air, _ledManager.blink); 245 | _heard_from = false; 246 | getWorld()->getLogger()->log("Heartbeat timeout from Vehicle\n"); 247 | } 248 | } 249 | return msgReceived; 250 | } 251 | 252 | //--------------------------------------------------------------------------------- 253 | //-- Send Radio Status 254 | void MavESP8266Vehicle::_sendRadioStatus() 255 | { 256 | getStatus(); 257 | //-- Build message 258 | mavlink_message_t msg{}; 259 | mavlink_msg_radio_status_pack_chan( 260 | _forwardTo->systemID(), 261 | MAV_COMP_ID_UDP_BRIDGE, 262 | _send_chan, 263 | &msg, 264 | 0, // We don't have access to RSSI 265 | 0, // We don't have access to Remote RSSI 266 | _status.queue_status, // UDP queue status 267 | 0, // We don't have access to noise data 268 | 0, // We don't have access to remote noise data 269 | (uint16_t)(_status.packets_lost / 10), 270 | 0 // We don't fix anything 271 | ); 272 | sendMessage(&msg); 273 | _status.radio_status_sent++; 274 | } -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015, 2016 Gus Grubba. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 19 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 20 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 22 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 23 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 24 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 26 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | ****************************************************************************/ 30 | 31 | /** 32 | * @file main.cpp 33 | * ESP8266 Wifi AP, MavLink UART/UDP Bridge 34 | * 35 | * @author Gus Grubba 36 | */ 37 | 38 | #include "mavesp8266.h" 39 | #include "mavesp8266_parameters.h" 40 | #include "mavesp8266_gcs.h" 41 | #include "mavesp8266_vehicle.h" 42 | #include "mavesp8266_httpd.h" 43 | #include "mavesp8266_component.h" 44 | #include "led_manager.h" 45 | 46 | #include 47 | 48 | #define GPIO02 2 49 | //--------------------------------------------------------------------------------- 50 | //-- HTTP Update Status 51 | class MavESP8266UpdateImp : public MavESP8266Update 52 | { 53 | public: 54 | MavESP8266UpdateImp() 55 | : _isUpdating(false) 56 | { 57 | } 58 | void updateStarted() 59 | { 60 | _isUpdating = true; 61 | } 62 | void updateCompleted() 63 | { 64 | //-- TODO 65 | } 66 | void updateError() 67 | { 68 | //-- TODO 69 | } 70 | bool isUpdating() { return _isUpdating; } 71 | 72 | private: 73 | bool _isUpdating; 74 | }; 75 | 76 | //-- Singletons 77 | LEDManager ledManager; 78 | IPAddress localIP; 79 | MavESP8266Component Component; 80 | MavESP8266Parameters Parameters; 81 | MavESP8266GCS GCS(ledManager); 82 | MavESP8266Vehicle Vehicle(ledManager); 83 | MavESP8266Httpd updateServer; 84 | MavESP8266UpdateImp updateStatus; 85 | MavESP8266Log Logger; 86 | 87 | //--------------------------------------------------------------------------------- 88 | //-- Accessors 89 | class MavESP8266WorldImp : public MavESP8266World 90 | { 91 | public: 92 | MavESP8266Parameters *getParameters() { return &Parameters; } 93 | MavESP8266Component *getComponent() { return &Component; } 94 | MavESP8266Vehicle *getVehicle() { return &Vehicle; } 95 | MavESP8266GCS *getGCS() { return &GCS; } 96 | MavESP8266Log *getLogger() { return &Logger; } 97 | }; 98 | 99 | MavESP8266WorldImp World; 100 | 101 | MavESP8266World *getWorld() 102 | { 103 | return &World; 104 | } 105 | 106 | //--------------------------------------------------------------------------------- 107 | //-- Wait for a DHCPD client 108 | void wait_for_client() 109 | { 110 | DEBUG_LOG("Waiting for a client...\n"); 111 | 112 | uint8 client_count = wifi_softap_get_station_num(); 113 | while (!client_count) 114 | { 115 | ledManager.blinkLED(); 116 | delay(200); 117 | client_count = wifi_softap_get_station_num(); 118 | } 119 | ledManager.setLED(ledManager.wifi, ledManager.on); 120 | ledManager.setLED(ledManager.gcs, ledManager.blink); 121 | ledManager.setLED(ledManager.air, ledManager.blink); 122 | DEBUG_LOG("Got %d client(s)\n", client_count); 123 | } 124 | 125 | void check_wifi_connected() 126 | { 127 | if (Parameters.getWifiMode() == WIFI_MODE_AP && !wifi_softap_get_station_num()) 128 | { 129 | ledManager.setLED(ledManager.wifi, ledManager.blink); 130 | } 131 | else if (Parameters.getWifiMode() == WIFI_MODE_AP) 132 | { 133 | ledManager.setLED(ledManager.wifi, ledManager.on); 134 | } 135 | 136 | if (Parameters.getWifiMode() == WIFI_MODE_STA && WiFi.status() != WL_CONNECTED) 137 | { 138 | ledManager.setLED(ledManager.wifi, ledManager.doubleBlink); 139 | } 140 | else if (Parameters.getWifiMode() == WIFI_MODE_STA && WiFi.status() == WL_CONNECTED) 141 | { 142 | ledManager.setLED(ledManager.wifi, ledManager.on); 143 | } 144 | } 145 | 146 | //--------------------------------------------------------------------------------- 147 | //-- Reset all parameters whenever the reset gpio pin is active within first 5s of boot 148 | IRAM_ATTR void reset_interrupt() 149 | { 150 | if (millis() < 5000) 151 | { 152 | Parameters.resetToDefaults(); 153 | Parameters.saveAllToEeprom(); 154 | ESP.reset(); 155 | } 156 | else 157 | { 158 | detachInterrupt(GPIO02); 159 | } 160 | } 161 | 162 | void setup_station() 163 | { 164 | //-- Connect to an existing network 165 | ledManager.setLED(ledManager.wifi, ledManager.doubleBlink); // Double blink while searching for station 166 | WiFi.setSleepMode(WIFI_NONE_SLEEP); // Aparrently it can go to sleep in station mode 167 | WiFi.mode(WIFI_STA); 168 | WiFi.config(Parameters.getWifiStaIP(), Parameters.getWifiStaGateway(), Parameters.getWifiStaSubnet(), 0U, 0U); 169 | WiFi.begin(Parameters.getWifiStaSsid(), Parameters.getWifiStaPassword()); 170 | } 171 | 172 | void setup_AP() 173 | { 174 | ledManager.setLED(ledManager.wifi, ledManager.blink); 175 | WiFi.mode(WIFI_AP); 176 | WiFi.encryptionType(AUTH_WPA2_PSK); 177 | WiFi.softAP(Parameters.getWifiSsid(), Parameters.getWifiPassword(), Parameters.getWifiChannel()); 178 | localIP = WiFi.softAPIP(); 179 | } 180 | 181 | void setup_wifi() 182 | { 183 | //-- Boost power to Max 184 | WiFi.setOutputPower(20.5); 185 | //-- MDNS 186 | char mdsnName[256]; 187 | sprintf(mdsnName, "MavEsp8266-%d", localIP[3]); 188 | MDNS.begin(mdsnName); 189 | MDNS.addService("http", "tcp", 80); 190 | //-- Initialize Comm Links 191 | DEBUG_LOG("Start WiFi Bridge\n"); 192 | DEBUG_LOG("Local IP: %s\n", localIP.toString().c_str()); 193 | 194 | Parameters.setLocalIPAddress(localIP); 195 | 196 | IPAddress gcs_ip(localIP); 197 | //-- I'm getting bogus IP from the DHCP server. Broadcasting for now. 198 | gcs_ip[3] = 255; 199 | 200 | GCS.begin(&Vehicle, gcs_ip); 201 | //-- Initialize Update Server 202 | updateServer.begin(&updateStatus); 203 | } 204 | 205 | bool connect_wifi() 206 | { 207 | if (Parameters.getWifiMode() == WIFI_MODE_STA) 208 | { 209 | if (WiFi.status() == WL_CONNECTED) 210 | { 211 | ledManager.setLED(ledManager.wifi, ledManager.on); 212 | ledManager.setLED(ledManager.gcs, ledManager.blink); 213 | localIP = WiFi.localIP(); 214 | WiFi.setAutoReconnect(true); 215 | setup_wifi(); 216 | return false; 217 | } 218 | else if (millis() > 60000) 219 | { 220 | //-- Fall back to AP mode if no connection could be established 221 | WiFi.disconnect(true); 222 | Parameters.setWifiMode(WIFI_MODE_AP); 223 | setup_AP(); 224 | } 225 | } 226 | 227 | if (Parameters.getWifiMode() == WIFI_MODE_AP) 228 | { 229 | DEBUG_LOG("Waiting for a client...\n"); 230 | 231 | uint8 client_count = wifi_softap_get_station_num(); 232 | 233 | if (client_count) 234 | { 235 | ledManager.setLED(ledManager.wifi, ledManager.on); 236 | ledManager.setLED(ledManager.gcs, ledManager.blink); 237 | DEBUG_LOG("Got %d client(s)\n", client_count); 238 | setup_wifi(); 239 | return false; 240 | } 241 | } 242 | return true; 243 | } 244 | 245 | //--------------------------------------------------------------------------------- 246 | //-- Set things up 247 | void setup() 248 | { 249 | delay(1000); 250 | Parameters.begin(); 251 | // set up pins for LEDs 252 | pinMode(12, OUTPUT); 253 | pinMode(4, OUTPUT); 254 | pinMode(5, OUTPUT); 255 | 256 | //-- Initialize GPIO02 (Used for "Reset To Factory") 257 | pinMode(GPIO02, INPUT_PULLUP); 258 | attachInterrupt(GPIO02, reset_interrupt, FALLING); 259 | 260 | Logger.begin(2048); 261 | 262 | DEBUG_LOG("\nConfiguring access point...\n"); 263 | DEBUG_LOG("Free Sketch Space: %u\n", ESP.getFreeSketchSpace()); 264 | 265 | WiFi.disconnect(true); 266 | 267 | // Set LED state when reboot 268 | ledManager.setLED(ledManager.wifi, ledManager.off); 269 | ledManager.setLED(ledManager.air, ledManager.blink); 270 | ledManager.setLED(ledManager.gcs, ledManager.blink); 271 | 272 | if (Parameters.getWifiMode() == WIFI_MODE_STA) 273 | { 274 | setup_station(); 275 | } 276 | 277 | if (Parameters.getWifiMode() == WIFI_MODE_AP) 278 | { 279 | setup_AP(); 280 | } 281 | 282 | Vehicle.begin(&GCS); 283 | } 284 | 285 | //--------------------------------------------------------------------------------- 286 | //-- Main Loop 287 | bool firstConnection = true; // Flag to see if this is the first time connecting to wifi 288 | 289 | void loop() 290 | { 291 | if (firstConnection) 292 | { 293 | firstConnection = connect_wifi(); 294 | if (Component.inRawMode()) 295 | { 296 | Vehicle.readMessageRaw(); 297 | } 298 | else 299 | { 300 | Vehicle.readMessage(); 301 | } 302 | } 303 | else 304 | { 305 | if (!updateStatus.isUpdating()) 306 | { 307 | check_wifi_connected(); 308 | if (Component.inRawMode()) 309 | { 310 | GCS.readMessageRaw(); 311 | delay(0); 312 | Vehicle.readMessageRaw(); 313 | } 314 | else 315 | { 316 | GCS.readMessage(); 317 | delay(0); 318 | Vehicle.readMessage(); 319 | } 320 | } 321 | updateServer.checkUpdates(); 322 | } 323 | ledManager.blinkLED(); 324 | ledManager.doubleBlinkLED(); 325 | } 326 | -------------------------------------------------------------------------------- /src/mavesp8266_gcs.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015, 2016 Gus Grubba. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 19 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 20 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 22 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 23 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 24 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 26 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | ****************************************************************************/ 30 | 31 | /** 32 | * @file mavesp8266_gcs.cpp 33 | * ESP8266 Wifi AP, MavLink UART/UDP Bridge 34 | * 35 | * @author Gus Grubba 36 | */ 37 | 38 | #include "mavesp8266.h" 39 | #include "mavesp8266_gcs.h" 40 | #include "mavesp8266_parameters.h" 41 | #include "mavesp8266_component.h" 42 | #include "led_manager.h" 43 | 44 | //--------------------------------------------------------------------------------- 45 | MavESP8266GCS::MavESP8266GCS(LEDManager &ledManager) 46 | : _udp_port(DEFAULT_UDP_HPORT), _ledManager(ledManager) 47 | { 48 | _recv_chan = MAVLINK_COMM_1; 49 | _send_chan = MAVLINK_COMM_0; 50 | memset(&_message, 0, sizeof(_message)); 51 | } 52 | 53 | //--------------------------------------------------------------------------------- 54 | //-- Initialize 55 | void MavESP8266GCS::begin(MavESP8266Bridge *forwardTo, IPAddress gcsIP) 56 | { 57 | MavESP8266Bridge::begin(forwardTo); 58 | _ip = gcsIP; 59 | //-- Init variables that shouldn't change unless we reboot 60 | _udp_port = getWorld()->getParameters()->getWifiUdpHport(); 61 | //-- Start UDP 62 | _udp.begin(getWorld()->getParameters()->getWifiUdpCport()); 63 | } 64 | 65 | //--------------------------------------------------------------------------------- 66 | //-- Read MavLink message from GCS 67 | void MavESP8266GCS::readMessage() 68 | { 69 | //-- Read UDP 70 | if (_readMessage()) 71 | { 72 | //-- If we have a message, forward it 73 | _forwardTo->sendMessage(&_message); 74 | memset(&_message, 0, sizeof(_message)); 75 | } 76 | //-- Update radio status (1Hz) 77 | if (_heard_from && (millis() - _last_status_time > 1000)) 78 | { 79 | delay(0); 80 | _sendRadioStatus(); 81 | _last_status_time = millis(); 82 | } 83 | } 84 | 85 | //--------------------------------------------------------------------------------- 86 | //-- Read MavLink message from GCS 87 | bool MavESP8266GCS::_readMessage() 88 | { 89 | bool msgReceived = false; 90 | int udp_count = _udp.parsePacket(); 91 | if (udp_count > 0) 92 | { 93 | while (udp_count--) 94 | { 95 | int result = _udp.read(); 96 | if (result >= 0) 97 | { 98 | // Parsing 99 | msgReceived = mavlink_frame_char_buffer(&_rxmsg, 100 | &_rxstatus, 101 | result, 102 | &_message, 103 | &_mav_status); 104 | if (msgReceived) 105 | { 106 | //-- We no longer need to broadcast 107 | _status.packets_received++; 108 | if (_ip[3] == 255) 109 | { 110 | _ip = _udp.remoteIP(); 111 | getWorld()->getLogger()->log("Response from GCS. Setting GCS IP to: %s\n", _ip.toString().c_str()); 112 | _ledManager.setLED(_ledManager.wifi, _ledManager.on); 113 | } 114 | //-- First packets 115 | if (!_heard_from) 116 | { 117 | if (_message.msgid == MAVLINK_MSG_ID_HEARTBEAT) 118 | { 119 | _ledManager.setLED(_ledManager.gcs, _ledManager.on); 120 | //-- We no longer need DHCP 121 | if (getWorld()->getParameters()->getWifiMode() == WIFI_MODE_AP) 122 | { 123 | wifi_softap_dhcps_stop(); 124 | } 125 | _heard_from = true; 126 | _system_id = _message.sysid; 127 | _component_id = _message.compid; 128 | _seq_expected = _message.seq + 1; 129 | _last_heartbeat = millis(); 130 | } 131 | } 132 | else 133 | { 134 | if (_message.msgid == MAVLINK_MSG_ID_HEARTBEAT) 135 | { 136 | _last_heartbeat = millis(); 137 | } 138 | _checkLinkErrors(&_message); 139 | } 140 | 141 | if (msgReceived == MAVLINK_FRAMING_BAD_CRC || 142 | msgReceived == MAVLINK_FRAMING_BAD_SIGNATURE) 143 | { 144 | // we don't process messages locally with bad CRC, 145 | // but we do forward them, so when new messages 146 | // are added we can bridge them 147 | break; 148 | } 149 | 150 | //-- Check for message we might be interested 151 | if (getWorld()->getComponent()->handleMessage(this, &_message)) 152 | { 153 | //-- Eat message (don't send it to FC) 154 | memset(&_message, 0, sizeof(_message)); 155 | msgReceived = false; 156 | continue; 157 | } 158 | 159 | //-- Got message, leave 160 | break; 161 | } 162 | } 163 | } 164 | } 165 | if (!msgReceived) 166 | { 167 | if (_heard_from && (millis() - _last_heartbeat) > HEARTBEAT_TIMEOUT) 168 | { 169 | _ledManager.setLED(_ledManager.gcs, _ledManager.blink); 170 | //-- Restart DHCP and start broadcasting again 171 | if (getWorld()->getParameters()->getWifiMode() == WIFI_MODE_AP) 172 | { 173 | wifi_softap_dhcps_start(); 174 | } 175 | _heard_from = false; 176 | _ip[3] = 255; 177 | getWorld()->getLogger()->log("Heartbeat timeout from GCS\n"); 178 | } 179 | } 180 | return msgReceived; 181 | } 182 | 183 | void MavESP8266GCS::readMessageRaw() 184 | { 185 | int udp_count = _udp.parsePacket(); 186 | char buf[1024]; 187 | int buf_index = 0; 188 | 189 | if (udp_count > 0) 190 | { 191 | while (buf_index < udp_count) 192 | { 193 | int result = _udp.read(); 194 | if (result >= 0) 195 | { 196 | buf[buf_index] = (char)result; 197 | buf_index++; 198 | } 199 | } 200 | 201 | if (buf[0] == 0x30 && buf[1] == 0x20) 202 | { 203 | // reboot command, switch out of raw mode soon 204 | getWorld()->getComponent()->resetRawMode(); 205 | } 206 | 207 | _forwardTo->sendMessageRaw((uint8_t *)buf, buf_index); 208 | } 209 | } 210 | 211 | //--------------------------------------------------------------------------------- 212 | //-- Forward message(s) to the GCS 213 | int MavESP8266GCS::sendMessage(mavlink_message_t *message, int count) 214 | { 215 | int sentCount = 0; 216 | _udp.beginPacket(_ip, _udp_port); 217 | for (int i = 0; i < count; i++) 218 | { 219 | // Translate message to buffer 220 | char buf[300]; 221 | unsigned len = mavlink_msg_to_send_buffer((uint8_t *)buf, &message[i]); 222 | // Send it 223 | _status.packets_sent++; 224 | size_t sent = _udp.write((uint8_t *)(void *)buf, len); 225 | if (sent != len) 226 | { 227 | break; 228 | //-- Fibble attempt at not losing data until we get access to the socket TX buffer 229 | // status before we try to send. 230 | _udp.endPacket(); 231 | delay(2); 232 | _udp.beginPacket(_ip, _udp_port); 233 | _udp.write((uint8_t *)(void *)&buf[sent], len - sent); 234 | _udp.endPacket(); 235 | return sentCount; 236 | } 237 | sentCount++; 238 | } 239 | _udp.endPacket(); 240 | return sentCount; 241 | } 242 | 243 | //--------------------------------------------------------------------------------- 244 | //-- Forward message to the GCS 245 | int MavESP8266GCS::sendMessage(mavlink_message_t *message) 246 | { 247 | _sendSingleUdpMessage(message); 248 | return 1; 249 | } 250 | 251 | int MavESP8266GCS::sendMessageRaw(uint8_t *buffer, int len) 252 | { 253 | _udp.beginPacket(_ip, _udp_port); 254 | size_t sent = _udp.write(buffer, len); 255 | _udp.endPacket(); 256 | //_udp.flush(); 257 | return sent; 258 | } 259 | 260 | //--------------------------------------------------------------------------------- 261 | //-- Send Radio Status 262 | void MavESP8266GCS::_sendRadioStatus() 263 | { 264 | linkStatus *st = _forwardTo->getStatus(); 265 | uint8_t rssi = 0; 266 | uint8_t lostVehicleMessages = 100; 267 | uint8_t lostGcsMessages = 100; 268 | 269 | if (wifi_get_opmode() == STATION_MODE) 270 | { 271 | rssi = (uint8_t)wifi_station_get_rssi(); 272 | } 273 | 274 | if (st->packets_received > 0) 275 | { 276 | lostVehicleMessages = (st->packets_lost * 100) / st->packets_received; 277 | } 278 | 279 | if (_status.packets_received > 0) 280 | { 281 | lostGcsMessages = (_status.packets_lost * 100) / _status.packets_received; 282 | } 283 | 284 | //-- Build message 285 | mavlink_message_t msg; 286 | mavlink_msg_radio_status_pack_chan( 287 | _forwardTo->systemID(), 288 | MAV_COMP_ID_UDP_BRIDGE, 289 | _forwardTo->_recv_chan, 290 | &msg, 291 | rssi, // RSSI Only valid in STA mode 292 | 0, // We don't have access to Remote RSSI 293 | st->queue_status, // UDP queue status 294 | 0, // We don't have access to noise data 295 | lostVehicleMessages, // Percent of lost messages from Vehicle (UART) 296 | lostGcsMessages, // Percent of lost messages from GCS (UDP) 297 | 0 // We don't fix anything 298 | ); 299 | 300 | _sendSingleUdpMessage(&msg); 301 | _status.radio_status_sent++; 302 | } 303 | 304 | //--------------------------------------------------------------------------------- 305 | //-- Send UDP Single Message 306 | void MavESP8266GCS::_sendSingleUdpMessage(mavlink_message_t *msg) 307 | { 308 | // Translate message to buffer 309 | char buf[300]; 310 | unsigned len = mavlink_msg_to_send_buffer((uint8_t *)buf, msg); 311 | // Send it 312 | _udp.beginPacket(_ip, _udp_port); 313 | size_t sent = _udp.write((uint8_t *)(void *)buf, len); 314 | _udp.endPacket(); 315 | //-- Fibble attempt at not losing data until we get access to the socket TX buffer 316 | // status before we try to send. 317 | if (sent != len) 318 | { 319 | delay(1); 320 | _udp.beginPacket(_ip, _udp_port); 321 | _udp.write((uint8_t *)(void *)&buf[sent], len - sent); 322 | _udp.endPacket(); 323 | } 324 | _status.packets_sent++; 325 | } 326 | -------------------------------------------------------------------------------- /src/mavesp8266_component.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015, 2016 Gus Grubba. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 19 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 20 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 22 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 23 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 24 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 26 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | ****************************************************************************/ 30 | 31 | /** 32 | * @file mavesp8266_component.cpp 33 | * ESP8266 Wifi AP, MavLink UART/UDP Bridge 34 | * 35 | * @author Gus Grubba 36 | */ 37 | 38 | #include "mavesp8266.h" 39 | #include "mavesp8266_component.h" 40 | #include "mavesp8266_parameters.h" 41 | #include "mavesp8266_vehicle.h" 42 | 43 | const char* kHASH_PARAM = "_HASH_CHECK"; 44 | 45 | 46 | MavESP8266Component::MavESP8266Component() { 47 | 48 | 49 | } 50 | 51 | bool 52 | MavESP8266Component::inRawMode() { 53 | // switch out of raw mode when not needed anymore 54 | if (_in_raw_mode_time > 0 && millis() > _in_raw_mode_time + 5000) { 55 | _in_raw_mode = false; 56 | _in_raw_mode_time = 0; 57 | getWorld()->getLogger()->log("Raw mode disabled\n"); 58 | } 59 | 60 | return _in_raw_mode; 61 | } 62 | 63 | bool 64 | MavESP8266Component::handleMessage(MavESP8266Bridge* sender, mavlink_message_t* message) { 65 | 66 | // 67 | // TODO: These response messages need to be queued up and sent as part of the main loop and not all 68 | // at once from here. 69 | // 70 | //----------------------------------------------- 71 | 72 | //-- MAVLINK_MSG_ID_PARAM_SET 73 | if(message->msgid == MAVLINK_MSG_ID_PARAM_SET) { 74 | mavlink_param_set_t param; 75 | mavlink_msg_param_set_decode(message, ¶m); 76 | DEBUG_LOG("MAVLINK_MSG_ID_PARAM_SET: %u %s\n", param.target_component, param.param_id); 77 | if(param.target_component == MAV_COMP_ID_UDP_BRIDGE) { 78 | _handleParamSet(sender, ¶m); 79 | return true; 80 | } 81 | //----------------------------------------------- 82 | //-- MAVLINK_MSG_ID_COMMAND_LONG 83 | } else if(message->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { 84 | mavlink_command_long_t cmd; 85 | mavlink_msg_command_long_decode(message, &cmd); 86 | if(cmd.target_component == MAV_COMP_ID_ALL || cmd.target_component == MAV_COMP_ID_UDP_BRIDGE) { 87 | _handleCmdLong(sender, &cmd, cmd.target_component); 88 | //-- If it was directed to us, eat it and loop 89 | if(cmd.target_component == MAV_COMP_ID_UDP_BRIDGE) { 90 | return true; 91 | } 92 | } 93 | //----------------------------------------------- 94 | //-- MAVLINK_MSG_ID_PARAM_REQUEST_LIST 95 | } else if(message->msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { 96 | mavlink_param_request_list_t param; 97 | mavlink_msg_param_request_list_decode(message, ¶m); 98 | DEBUG_LOG("MAVLINK_MSG_ID_PARAM_REQUEST_LIST: %u\n", param.target_component); 99 | if(param.target_component == MAV_COMP_ID_ALL || param.target_component == MAV_COMP_ID_UDP_BRIDGE) { 100 | _handleParamRequestList(sender); 101 | } 102 | //----------------------------------------------- 103 | //-- MAVLINK_MSG_ID_PARAM_REQUEST_READ 104 | } else if(message->msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) { 105 | mavlink_param_request_read_t param; 106 | mavlink_msg_param_request_read_decode(message, ¶m); 107 | //-- This component or all components? 108 | if(param.target_component == MAV_COMP_ID_ALL || param.target_component == MAV_COMP_ID_UDP_BRIDGE) { 109 | //-- If asking for hash, respond and pass through 110 | if(strncmp(param.param_id, kHASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN) == 0) { 111 | _sendParameter(sender, kHASH_PARAM, getWorld()->getParameters()->paramHashCheck(), 0xFFFF); 112 | } else { 113 | _handleParamRequestRead(sender, ¶m); 114 | //-- If this was addressed to me only eat message 115 | if(param.target_component == MAV_COMP_ID_UDP_BRIDGE) { 116 | //-- Eat message (don't send it to FC) 117 | return true; 118 | } 119 | } 120 | } 121 | } 122 | 123 | //-- Couldn't handle the message, pass on 124 | return false; 125 | } 126 | 127 | 128 | 129 | 130 | 131 | //--------------------------------------------------------------------------------- 132 | //-- Send Debug Message 133 | void 134 | MavESP8266Component::_sendStatusMessage(MavESP8266Bridge* sender, uint8_t type, const char* text) 135 | { 136 | if(!getWorld()->getParameters()->getDebugEnabled() && type == MAV_SEVERITY_DEBUG) { 137 | return; 138 | } 139 | //-- Build message 140 | mavlink_message_t msg; 141 | mavlink_msg_statustext_pack_chan( 142 | getWorld()->getVehicle()->systemID(), 143 | MAV_COMP_ID_UDP_BRIDGE, 144 | sender->_send_chan, 145 | &msg, 146 | type, 147 | text 148 | ); 149 | sender->sendMessage(&msg); 150 | } 151 | 152 | 153 | 154 | 155 | //--------------------------------------------------------------------------------- 156 | //-- Set parameter 157 | void 158 | MavESP8266Component::_handleParamSet(MavESP8266Bridge* sender, mavlink_param_set_t* param) 159 | { 160 | for(int i = 0; i < MavESP8266Parameters::ID_COUNT; i++) { 161 | //-- Find parameter 162 | if(strncmp(param->param_id, getWorld()->getParameters()->getAt(i)->id, strlen(getWorld()->getParameters()->getAt(i)->id)) == 0) { 163 | //-- Skip Read Only 164 | if(!getWorld()->getParameters()->getAt(i)->readOnly) { 165 | //-- Set new value 166 | memcpy(getWorld()->getParameters()->getAt(i)->value, ¶m->param_value, getWorld()->getParameters()->getAt(i)->length); 167 | } 168 | //-- "Ack" it 169 | _sendParameter(sender, getWorld()->getParameters()->getAt(i)->index); 170 | return; 171 | } 172 | } 173 | } 174 | 175 | //--------------------------------------------------------------------------------- 176 | //-- Handle Parameter Request List 177 | void 178 | MavESP8266Component::_handleParamRequestList(MavESP8266Bridge* sender) 179 | { 180 | for(int i = 0; i < MavESP8266Parameters::ID_COUNT; i++) { 181 | _sendParameter(sender, getWorld()->getParameters()->getAt(i)->index); 182 | delay(0); 183 | } 184 | } 185 | 186 | //--------------------------------------------------------------------------------- 187 | //-- Handle Parameter Request Read 188 | void 189 | MavESP8266Component::_handleParamRequestRead(MavESP8266Bridge* sender, mavlink_param_request_read_t* param) 190 | { 191 | for(int i = 0; i < MavESP8266Parameters::ID_COUNT; i++) { 192 | //-- Find parameter 193 | if(param->param_index == getWorld()->getParameters()->getAt(i)->index || strncmp(param->param_id, getWorld()->getParameters()->getAt(i)->id, strlen(getWorld()->getParameters()->getAt(i)->id)) == 0) { 194 | _sendParameter(sender, getWorld()->getParameters()->getAt(i)->index); 195 | return; 196 | } 197 | } 198 | } 199 | 200 | //--------------------------------------------------------------------------------- 201 | //-- Send Parameter (Index Based) 202 | void 203 | MavESP8266Component::_sendParameter(MavESP8266Bridge* sender, uint16_t index) 204 | { 205 | //-- Build message 206 | mavlink_param_value_t msg; 207 | msg.param_count = MavESP8266Parameters::ID_COUNT; 208 | msg.param_index = index; 209 | strncpy(msg.param_id, getWorld()->getParameters()->getAt(index)->id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); 210 | uint32_t val = 0; 211 | memcpy(&val, getWorld()->getParameters()->getAt(index)->value, getWorld()->getParameters()->getAt(index)->length); 212 | memcpy(&msg.param_value, &val, sizeof(uint32_t)); 213 | msg.param_type = getWorld()->getParameters()->getAt(index)->type; 214 | mavlink_message_t mmsg; 215 | mavlink_msg_param_value_encode_chan( 216 | getWorld()->getVehicle()->systemID(), 217 | MAV_COMP_ID_UDP_BRIDGE, 218 | sender->_send_chan, 219 | &mmsg, 220 | &msg 221 | ); 222 | 223 | sender->sendMessage(&mmsg); 224 | } 225 | 226 | //--------------------------------------------------------------------------------- 227 | //-- Send Parameter (Raw) 228 | void 229 | MavESP8266Component::_sendParameter(MavESP8266Bridge* sender, const char* id, uint32_t value, uint16_t index) 230 | { 231 | //-- Build message 232 | mavlink_param_value_t msg; 233 | msg.param_count = MavESP8266Parameters::ID_COUNT; 234 | msg.param_index = index; 235 | strncpy(msg.param_id, id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); 236 | memcpy(&msg.param_value, &value, sizeof(uint32_t)); 237 | msg.param_type = MAV_PARAM_TYPE_UINT32; 238 | mavlink_message_t mmsg; 239 | mavlink_msg_param_value_encode_chan( 240 | getWorld()->getVehicle()->systemID(), 241 | MAV_COMP_ID_UDP_BRIDGE, 242 | sender->_send_chan, 243 | &mmsg, 244 | &msg 245 | ); 246 | sender->sendMessage(&mmsg); 247 | } 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | //--------------------------------------------------------------------------------- 257 | //-- Handle Commands 258 | void 259 | MavESP8266Component::_handleCmdLong(MavESP8266Bridge* sender, mavlink_command_long_t* cmd, uint8_t compID) 260 | { 261 | bool reboot = false; 262 | uint8_t result = MAV_RESULT_UNSUPPORTED; 263 | if(cmd->command == MAV_CMD_PREFLIGHT_STORAGE) { 264 | //-- Read from EEPROM 265 | if((uint8_t)cmd->param1 == 0) { 266 | result = MAV_RESULT_ACCEPTED; 267 | getWorld()->getParameters()->loadAllFromEeprom(); 268 | //-- Write to EEPROM 269 | } else if((uint8_t)cmd->param1 == 1) { 270 | result = MAV_RESULT_ACCEPTED; 271 | getWorld()->getParameters()->saveAllToEeprom(); 272 | delay(0); 273 | //-- Restore defaults 274 | } else if((uint8_t)cmd->param1 == 2) { 275 | result = MAV_RESULT_ACCEPTED; 276 | getWorld()->getParameters()->resetToDefaults(); 277 | } 278 | } else if(cmd->command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN) { 279 | //-- Reset "Companion Computer" 280 | if((uint8_t)cmd->param2 == 1) { 281 | result = MAV_RESULT_ACCEPTED; 282 | reboot = true; 283 | } 284 | 285 | // recognize FC reboot command and switch to raw mode for bootloader protocol to work 286 | if(compID == MAV_COMP_ID_ALL && (uint8_t)cmd->param1 > 0) { 287 | getWorld()->getLogger()->log("Raw mode enabled (cmd %d %d)\n", cmd->command, compID); 288 | _in_raw_mode = true; 289 | _in_raw_mode_time = 0; 290 | } 291 | } 292 | //-- Response 293 | if(compID == MAV_COMP_ID_UDP_BRIDGE) { 294 | mavlink_message_t msg; 295 | mavlink_msg_command_ack_pack_chan( 296 | getWorld()->getVehicle()->systemID(), 297 | //_forwardTo->systemID(), 298 | MAV_COMP_ID_UDP_BRIDGE, 299 | sender->_send_chan, 300 | &msg, 301 | cmd->command, 302 | result, 303 | 0,0,0,0 304 | ); 305 | sender->sendMessage(&msg); 306 | } 307 | if(reboot) { 308 | _wifiReboot(sender); 309 | } 310 | } 311 | 312 | 313 | //--------------------------------------------------------------------------------- 314 | //-- Reboot 315 | void 316 | MavESP8266Component::_wifiReboot(MavESP8266Bridge* sender) 317 | { 318 | _sendStatusMessage(sender, MAV_SEVERITY_NOTICE, "Rebooting WiFi Bridge."); 319 | delay(50); 320 | ESP.reset(); 321 | } 322 | -------------------------------------------------------------------------------- /src/mavesp8266_parameters.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015, 2016 Gus Grubba. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 19 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 20 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 22 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 23 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 24 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 26 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | ****************************************************************************/ 30 | 31 | /** 32 | * @file mavesp8266_parameters.cpp 33 | * ESP8266 Wifi AP, MavLink UART/UDP Bridge 34 | * 35 | * @author Gus Grubba 36 | */ 37 | 38 | #include 39 | #include 40 | #include "mavesp8266.h" 41 | #include "mavesp8266_parameters.h" 42 | #include "crc.h" 43 | 44 | const char *kDEFAULT_SSID = "BeyondRobotix"; 45 | const char *kDEFAULT_PASSWORD = "beyondrobotix"; // -- This MUST be 8 characters or longer 46 | 47 | //-- Reserved space for EEPROM persistence. A change in this will cause all values to reset to defaults. 48 | #define EEPROM_SPACE 32 * sizeof(uint32_t) 49 | #define EEPROM_CRC_ADD EEPROM_SPACE - (sizeof(uint32_t) << 1) 50 | 51 | uint32_t _sw_version; 52 | int8_t _debug_enabled; 53 | int8_t _wifi_mode; 54 | uint32_t _wifi_channel; 55 | uint16_t _wifi_udp_hport; 56 | uint16_t _wifi_udp_cport; 57 | uint32_t _wifi_ip_address; 58 | char _wifi_ssid[16]; 59 | char _wifi_password[16]; 60 | char _wifi_ssidsta[16]; 61 | char _wifi_passwordsta[16]; 62 | uint32_t _wifi_ipsta; 63 | uint32_t _wifi_gatewaysta; 64 | uint32_t _wifi_subnetsta; 65 | uint32_t _uart_baud_rate; 66 | uint32_t _flash_left; 67 | uint32_t _wifi_targetsta; 68 | 69 | //-- Parameters 70 | // No string support in parameters so we stash a char[16] into 4 uint32_t 71 | struct stMavEspParameters mavParameters[] = { 72 | {"SW_VER", &_sw_version, MavESP8266Parameters::ID_FWVER, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, true}, 73 | {"DEBUG_ENABLED", &_debug_enabled, MavESP8266Parameters::ID_DEBUG, sizeof(int8_t), MAV_PARAM_TYPE_INT8, false}, 74 | {"WIFI_MODE", &_wifi_mode, MavESP8266Parameters::ID_MODE, sizeof(int8_t), MAV_PARAM_TYPE_INT8, false}, 75 | {"WIFI_CHANNEL", &_wifi_channel, MavESP8266Parameters::ID_CHANNEL, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 76 | {"WIFI_UDP_HPORT", &_wifi_udp_hport, MavESP8266Parameters::ID_HPORT, sizeof(uint16_t), MAV_PARAM_TYPE_UINT16, false}, 77 | {"WIFI_UDP_CPORT", &_wifi_udp_cport, MavESP8266Parameters::ID_CPORT, sizeof(uint16_t), MAV_PARAM_TYPE_UINT16, false}, 78 | {"WIFI_IPADDRESS", &_wifi_ip_address, MavESP8266Parameters::ID_IPADDRESS, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, true}, 79 | {"WIFI_SSID1", &_wifi_ssid[0], MavESP8266Parameters::ID_SSID1, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 80 | {"WIFI_SSID2", &_wifi_ssid[4], MavESP8266Parameters::ID_SSID2, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 81 | {"WIFI_SSID3", &_wifi_ssid[8], MavESP8266Parameters::ID_SSID3, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 82 | {"WIFI_SSID4", &_wifi_ssid[12], MavESP8266Parameters::ID_SSID4, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 83 | {"WIFI_PASSWORD1", &_wifi_password[0], MavESP8266Parameters::ID_PASS1, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 84 | {"WIFI_PASSWORD2", &_wifi_password[4], MavESP8266Parameters::ID_PASS2, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 85 | {"WIFI_PASSWORD3", &_wifi_password[8], MavESP8266Parameters::ID_PASS3, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 86 | {"WIFI_PASSWORD4", &_wifi_password[12], MavESP8266Parameters::ID_PASS4, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 87 | {"WIFI_SSIDSTA1", &_wifi_ssidsta[0], MavESP8266Parameters::ID_SSIDSTA1, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 88 | {"WIFI_SSIDSTA2", &_wifi_ssidsta[4], MavESP8266Parameters::ID_SSIDSTA2, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 89 | {"WIFI_SSIDSTA3", &_wifi_ssidsta[8], MavESP8266Parameters::ID_SSIDSTA3, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 90 | {"WIFI_SSIDSTA4", &_wifi_ssidsta[12], MavESP8266Parameters::ID_SSIDSTA4, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 91 | {"WIFI_PWDSTA1", &_wifi_passwordsta[0], MavESP8266Parameters::ID_PASSSTA1, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 92 | {"WIFI_PWDSTA2", &_wifi_passwordsta[4], MavESP8266Parameters::ID_PASSSTA2, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 93 | {"WIFI_PWDSTA3", &_wifi_passwordsta[8], MavESP8266Parameters::ID_PASSSTA3, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 94 | {"WIFI_PWDSTA4", &_wifi_passwordsta[12], MavESP8266Parameters::ID_PASSSTA4, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 95 | {"WIFI_IPSTA", &_wifi_ipsta, MavESP8266Parameters::ID_IPSTA, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 96 | {"WIFI_GATEWAYSTA", &_wifi_gatewaysta, MavESP8266Parameters::ID_GATEWAYSTA, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 97 | {"WIFI_SUBNET_STA", &_wifi_subnetsta, MavESP8266Parameters::ID_SUBNETSTA, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 98 | {"UART_BAUDRATE", &_uart_baud_rate, MavESP8266Parameters::ID_UART, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, false}, 99 | }; 100 | 101 | //--------------------------------------------------------------------------------- 102 | MavESP8266Parameters::MavESP8266Parameters() 103 | { 104 | } 105 | 106 | //--------------------------------------------------------------------------------- 107 | //-- Fail safe 108 | uint32_t bogusVar = 0; 109 | struct stMavEspParameters bogus = {"ERROR", &bogusVar, MavESP8266Parameters::ID_COUNT, sizeof(uint32_t), MAV_PARAM_TYPE_UINT32, true}; 110 | 111 | //--------------------------------------------------------------------------------- 112 | //-- Initialize 113 | void MavESP8266Parameters::begin() 114 | { 115 | EEPROM.begin(EEPROM_SPACE); 116 | _initEeprom(); 117 | } 118 | 119 | //--------------------------------------------------------------------------------- 120 | //-- Initialize 121 | void MavESP8266Parameters::setLocalIPAddress(uint32_t ipAddress) 122 | { 123 | _wifi_ip_address = ipAddress; 124 | } 125 | 126 | //--------------------------------------------------------------------------------- 127 | //-- Array accessor 128 | stMavEspParameters * 129 | MavESP8266Parameters::getAt(int index) 130 | { 131 | if (index < ID_COUNT) 132 | return &mavParameters[index]; 133 | else 134 | return &bogus; 135 | } 136 | 137 | //--------------------------------------------------------------------------------- 138 | //-- Parameters 139 | uint32_t MavESP8266Parameters::getSwVersion() { return _sw_version; } 140 | int8_t MavESP8266Parameters::getDebugEnabled() { return _debug_enabled; } 141 | int8_t MavESP8266Parameters::getWifiMode() { return _wifi_mode; } 142 | uint32_t MavESP8266Parameters::getLocalIPAddress() { return _wifi_ip_address; } 143 | uint32_t MavESP8266Parameters::getWifiChannel() { return _wifi_channel; } 144 | uint16_t MavESP8266Parameters::getWifiUdpHport() { return _wifi_udp_hport; } 145 | uint16_t MavESP8266Parameters::getWifiUdpCport() { return _wifi_udp_cport; } 146 | char *MavESP8266Parameters::getWifiSsid() { return _wifi_ssid; } 147 | char *MavESP8266Parameters::getWifiPassword() { return _wifi_password; } 148 | char *MavESP8266Parameters::getWifiStaSsid() { return _wifi_ssidsta; } 149 | char *MavESP8266Parameters::getWifiStaPassword() { return _wifi_passwordsta; } 150 | uint32_t MavESP8266Parameters::getWifiStaIP() { return _wifi_ipsta; } 151 | uint32_t MavESP8266Parameters::getWifiStaGateway() { return _wifi_gatewaysta; } 152 | uint32_t MavESP8266Parameters::getWifiStaSubnet() { return _wifi_subnetsta; } 153 | uint32_t MavESP8266Parameters::getUartBaudRate() { return _uart_baud_rate; } 154 | 155 | //--------------------------------------------------------------------------------- 156 | //-- Reset all to defaults 157 | void MavESP8266Parameters::resetToDefaults() 158 | { 159 | _sw_version = MAVESP8266_VERSION; 160 | _debug_enabled = 0; 161 | _wifi_mode = DEFAULT_WIFI_MODE; 162 | _wifi_channel = DEFAULT_WIFI_CHANNEL; 163 | _wifi_udp_hport = DEFAULT_UDP_HPORT; 164 | _wifi_udp_cport = DEFAULT_UDP_CPORT; 165 | _uart_baud_rate = DEFAULT_UART_SPEED; 166 | _wifi_ipsta = 0; 167 | _wifi_gatewaysta = 0; 168 | _wifi_subnetsta = 0; 169 | strncpy(_wifi_ssid, kDEFAULT_SSID, sizeof(_wifi_ssid)); 170 | strncpy(_wifi_password, kDEFAULT_PASSWORD, sizeof(_wifi_password)); 171 | strncpy(_wifi_ssidsta, kDEFAULT_SSID, sizeof(_wifi_ssidsta)); 172 | strncpy(_wifi_passwordsta, kDEFAULT_PASSWORD, sizeof(_wifi_passwordsta)); 173 | _flash_left = ESP.getFreeSketchSpace(); 174 | } 175 | 176 | //--------------------------------------------------------------------------------- 177 | //-- Saves all parameters to EEPROM 178 | void MavESP8266Parameters::loadAllFromEeprom() 179 | { 180 | uint32_t address = 0; 181 | for (int i = 0; i < ID_COUNT; i++) 182 | { 183 | uint8_t *ptr = (uint8_t *)mavParameters[i].value; 184 | for (int j = 0; j < mavParameters[i].length; j++, address++, ptr++) 185 | { 186 | *ptr = EEPROM.read(address); 187 | } 188 | #ifdef DEBUG 189 | Serial1.print("Loading from EEPROM: "); 190 | Serial1.print(mavParameters[i].id); 191 | Serial1.print(" Value: "); 192 | if (mavParameters[i].type == MAV_PARAM_TYPE_UINT32) 193 | Serial1.println(*((uint32_t *)mavParameters[i].value)); 194 | else if (mavParameters[i].type == MAV_PARAM_TYPE_UINT16) 195 | Serial1.println(*((uint16_t *)mavParameters[i].value)); 196 | else 197 | Serial1.println(*((int8_t *)mavParameters[i].value)); 198 | #endif 199 | } 200 | #ifdef DEBUG 201 | Serial1.println(""); 202 | #endif 203 | //-- Version if hardwired 204 | _sw_version = MAVESP8266_VERSION; 205 | _flash_left = ESP.getFreeSketchSpace(); 206 | } 207 | 208 | //--------------------------------------------------------------------------------- 209 | //-- Compute parameters hash 210 | uint32_t MavESP8266Parameters::paramHashCheck() 211 | { 212 | uint32_t crc = 0; 213 | for (int i = 0; i < ID_COUNT; i++) 214 | { 215 | crc = _crc32part((uint8_t *)(void *)mavParameters[i].id, strlen(mavParameters[i].id), crc); 216 | //-- QGC Expects a CRC of sizeof(uint32_t) 217 | uint32_t val = 0; 218 | if (mavParameters[i].type == MAV_PARAM_TYPE_UINT32) 219 | val = *((uint32_t *)mavParameters[i].value); 220 | else if (mavParameters[i].type == MAV_PARAM_TYPE_UINT16) 221 | val = (uint32_t) * ((uint16_t *)mavParameters[i].value); 222 | else 223 | val = (uint32_t) * ((int8_t *)mavParameters[i].value); 224 | crc = _crc32part((uint8_t *)(void *)&val, sizeof(uint32_t), crc); 225 | } 226 | delay(0); 227 | return crc; 228 | } 229 | 230 | //--------------------------------------------------------------------------------- 231 | //-- Saves all parameters to EEPROM 232 | void MavESP8266Parameters::saveAllToEeprom() 233 | { 234 | //-- Init flash space 235 | uint8_t *ptr = EEPROM.getDataPtr(); 236 | memset(ptr, 0, EEPROM_SPACE); 237 | //-- Write all paramaters to flash 238 | uint32_t address = 0; 239 | for (int i = 0; i < ID_COUNT; i++) 240 | { 241 | ptr = (uint8_t *)mavParameters[i].value; 242 | #ifdef DEBUG 243 | Serial1.print("Saving to EEPROM: "); 244 | Serial1.print(mavParameters[i].id); 245 | Serial1.print(" Value: "); 246 | if (mavParameters[i].type == MAV_PARAM_TYPE_UINT32) 247 | Serial1.println(*((uint32_t *)mavParameters[i].value)); 248 | else if (mavParameters[i].type == MAV_PARAM_TYPE_UINT16) 249 | Serial1.println(*((uint16_t *)mavParameters[i].value)); 250 | else 251 | Serial1.println(*((int8_t *)mavParameters[i].value)); 252 | #endif 253 | for (int j = 0; j < mavParameters[i].length; j++, address++, ptr++) 254 | { 255 | EEPROM.write(address, *ptr); 256 | } 257 | } 258 | uint32_t saved_crc = _getEepromCrc(); 259 | EEPROM.put(EEPROM_CRC_ADD, saved_crc); 260 | EEPROM.commit(); 261 | #ifdef DEBUG 262 | Serial1.print("Saved CRC: "); 263 | Serial1.print(saved_crc); 264 | Serial1.println(""); 265 | #endif 266 | } 267 | 268 | //--------------------------------------------------------------------------------- 269 | //-- Compute CRC32 for a buffer 270 | uint32_t 271 | MavESP8266Parameters::_crc32part(uint8_t *src, uint32_t len, uint32_t crc) 272 | { 273 | for (int i = 0; i < (int)len; i++) 274 | { 275 | crc = crc_table[(crc ^ src[i]) & 0xff] ^ (crc >> 8); 276 | } 277 | return crc; 278 | } 279 | 280 | //--------------------------------------------------------------------------------- 281 | //-- Computes EEPROM CRC 282 | uint32_t 283 | MavESP8266Parameters::_getEepromCrc() 284 | { 285 | uint32_t crc = 0; 286 | uint32_t size = 0; 287 | //-- Get size of all parameter data 288 | for (int i = 0; i < ID_COUNT; i++) 289 | { 290 | size += mavParameters[i].length; 291 | } 292 | //-- Computer CRC 293 | for (int i = 0; i < (int)size; i++) 294 | { 295 | crc = crc_table[(crc ^ EEPROM.read(i)) & 0xff] ^ (crc >> 8); 296 | } 297 | return crc; 298 | } 299 | 300 | //--------------------------------------------------------------------------------- 301 | //-- Initializes EEPROM. If not initialized, set to defaults and save it. 302 | void MavESP8266Parameters::_initEeprom() 303 | { 304 | loadAllFromEeprom(); 305 | //-- Is it uninitialized? 306 | uint32_t saved_crc = 0; 307 | EEPROM.get(EEPROM_CRC_ADD, saved_crc); 308 | uint32_t current_crc = _getEepromCrc(); 309 | if (saved_crc != current_crc) 310 | { 311 | #ifdef DEBUG 312 | Serial1.print("Initializing EEPROM. Saved: "); 313 | Serial1.print(saved_crc); 314 | Serial1.print(" Current: "); 315 | Serial1.println(current_crc); 316 | #endif 317 | //-- Set all defaults 318 | resetToDefaults(); 319 | //-- Save it all and store CRC 320 | saveAllToEeprom(); 321 | } 322 | else 323 | { 324 | //-- Load all parameters from EEPROM 325 | loadAllFromEeprom(); 326 | } 327 | } 328 | 329 | //--------------------------------------------------------------------------------- 330 | void MavESP8266Parameters::setDebugEnabled(int8_t enabled) 331 | { 332 | _debug_enabled = enabled; 333 | } 334 | 335 | //--------------------------------------------------------------------------------- 336 | void MavESP8266Parameters::setWifiMode(int8_t mode) 337 | { 338 | _wifi_mode = mode; 339 | } 340 | 341 | //--------------------------------------------------------------------------------- 342 | void MavESP8266Parameters::setWifiChannel(uint32_t channel) 343 | { 344 | _wifi_channel = channel; 345 | } 346 | 347 | //--------------------------------------------------------------------------------- 348 | void MavESP8266Parameters::setWifiUdpHport(uint16_t port) 349 | { 350 | _wifi_udp_hport = port; 351 | } 352 | 353 | //--------------------------------------------------------------------------------- 354 | void MavESP8266Parameters::setWifiUdpCport(uint16_t port) 355 | { 356 | _wifi_udp_cport = port; 357 | } 358 | 359 | //--------------------------------------------------------------------------------- 360 | void MavESP8266Parameters::setWifiSsid(const char *ssid) 361 | { 362 | strncpy(_wifi_ssid, ssid, sizeof(_wifi_ssid)); 363 | } 364 | 365 | //--------------------------------------------------------------------------------- 366 | void MavESP8266Parameters::setWifiPassword(const char *pwd) 367 | { 368 | strncpy(_wifi_password, pwd, sizeof(_wifi_password)); 369 | } 370 | 371 | //--------------------------------------------------------------------------------- 372 | void MavESP8266Parameters::setWifiStaSsid(const char *ssid) 373 | { 374 | strncpy(_wifi_ssidsta, ssid, sizeof(_wifi_ssidsta)); 375 | } 376 | 377 | //--------------------------------------------------------------------------------- 378 | void MavESP8266Parameters::setWifiStaPassword(const char *pwd) 379 | { 380 | strncpy(_wifi_passwordsta, pwd, sizeof(_wifi_passwordsta)); 381 | } 382 | 383 | //--------------------------------------------------------------------------------- 384 | void MavESP8266Parameters::setWifiStaIP(uint32_t addr) 385 | { 386 | _wifi_ipsta = addr; 387 | } 388 | 389 | //--------------------------------------------------------------------------------- 390 | void MavESP8266Parameters::setWifiStaGateway(uint32_t addr) 391 | { 392 | _wifi_gatewaysta = addr; 393 | } 394 | 395 | //--------------------------------------------------------------------------------- 396 | void MavESP8266Parameters::setWifiStaSubnet(uint32_t addr) 397 | { 398 | _wifi_subnetsta = addr; 399 | } 400 | 401 | //--------------------------------------------------------------------------------- 402 | void MavESP8266Parameters::setUartBaudRate(uint32_t baud) 403 | { 404 | _uart_baud_rate = baud; 405 | } 406 | -------------------------------------------------------------------------------- /src/mavesp8266_httpd.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015, 2016 Gus Grubba. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 17 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 18 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 19 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 20 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 22 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 23 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 24 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 26 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | ****************************************************************************/ 30 | 31 | /** 32 | * @file mavesp8266_httpd.cpp 33 | * ESP8266 Wifi AP, MavLink UART/UDP Bridge 34 | * 35 | * @author Gus Grubba 36 | */ 37 | 38 | #include 39 | #include "mavesp8266.h" 40 | #include "mavesp8266_httpd.h" 41 | #include "mavesp8266_parameters.h" 42 | #include "mavesp8266_gcs.h" 43 | #include "mavesp8266_vehicle.h" 44 | 45 | const char PROGMEM kTEXTPLAIN[] = "text/plain"; 46 | const char PROGMEM kTEXTHTML[] = "text/html"; 47 | const char PROGMEM kBADARG[] = "BAD ARGS"; 48 | const char PROGMEM kAPPJSON[] = "application/json"; 49 | const char PROGMEM kACCESSCTL[] = "Access-Control-Allow-Origin"; 50 | const char PROGMEM kDEFAULTBTN[] = ""; 51 | const char PROGMEM kUPLOADFORM[] = "

Upload new firmware


"; 52 | const char PROGMEM kUPLOADSUCCESS[] = "

Firmware Update in progress, waiting for the Kahuna to come back online...

"; 53 | 54 | const char PROGMEM kHEADER2[] = "
"; 55 | 56 | const char PROGMEM kHEADER1_C1[] = "BR Kahuna

Kahuna

Version: "; 62 | 63 | const uint32_t PROGMEM baudRates[] = {9600, 19200, 38400, 57600, 111100, 115200, 230400, 256000, 460800, 500000, 921600}; 64 | const uint8_t PROGMEM numBaudRates = 11; 65 | char buffer[1024]; 66 | 67 | const char *kBAUD = "baud"; 68 | const char *kPWD = "pwd"; 69 | const char *kSSID = "ssid"; 70 | const char *kPWDSTA = "pwdsta"; 71 | const char *kSSIDSTA = "ssidsta"; 72 | const char *kIPSTA = "ipsta"; 73 | const char *kGATESTA = "gatewaysta"; 74 | const char *kSUBSTA = "subnetsta"; 75 | const char *kCPORT = "cport"; 76 | const char *kHPORT = "hport"; 77 | const char *kCHANNEL = "channel"; 78 | const char *kDEBUG = "debug"; 79 | const char *kREBOOT = "reboot"; 80 | const char *kPOSITION = "position"; 81 | const char *kMODE = "mode"; 82 | 83 | const char *kFlashMaps[7] = { 84 | "512KB (256/256)", 85 | "256KB", 86 | "1MB (512/512)", 87 | "2MB (512/512)", 88 | "4MB (512/512)", 89 | "2MB (1024/1024)", 90 | "4MB (1024/1024)"}; 91 | 92 | static uint32_t flash = 0; 93 | static char paramCRC[12] = {""}; 94 | 95 | ESP8266WebServer webServer(80); 96 | MavESP8266Update *updateCB = NULL; 97 | bool started = false; 98 | 99 | // String message; 100 | //--------------------------------------------------------------------------------- 101 | void setNoCacheHeaders() 102 | { 103 | webServer.sendHeader("Cache-Control", "no-cache, no-store, must-revalidate"); 104 | webServer.sendHeader("Pragma", "no-cache"); 105 | webServer.sendHeader("Expires", "-1"); 106 | } 107 | 108 | //--------------------------------------------------------------------------------- 109 | void returnFail(String msg) 110 | { 111 | webServer.send(500, FPSTR(kTEXTPLAIN), msg + "\r\n"); 112 | } 113 | 114 | //--------------------------------------------------------------------------------- 115 | void respondOK() 116 | { 117 | webServer.send(200, FPSTR(kTEXTPLAIN), "OK"); 118 | } 119 | 120 | //--------------------------------------------------------------------------------- 121 | void handle_update() 122 | { 123 | webServer.sendHeader("Connection", "close"); 124 | webServer.sendHeader(FPSTR(kACCESSCTL), "*"); 125 | webServer.setContentLength(CONTENT_LENGTH_UNKNOWN); 126 | webServer.send_P(200, kTEXTHTML, kHEADER1_C1); 127 | webServer.sendContent_P(kHEADER1_C2); 128 | webServer.sendContent_P(kHEADER1_C3); 129 | webServer.sendContent_P(kHEADER1_C4); 130 | webServer.sendContent_P(kHEADER1_C5); 131 | webServer.sendContent_P(kHEADER1_C6); 132 | 133 | snprintf(buffer, sizeof(buffer), "%u.%u.%u

SetupStatusParametersFirmware UpdateReboot", MAVESP8266_VERSION_MAJOR, MAVESP8266_VERSION_MINOR, MAVESP8266_VERSION_BUILD); 134 | webServer.sendContent(buffer); 135 | 136 | webServer.sendContent_P(kHEADER2); 137 | webServer.sendContent_P(kUPLOADFORM); 138 | webServer.sendContent(""); 139 | } 140 | 141 | //--------------------------------------------------------------------------------- 142 | void handle_upload() 143 | { 144 | webServer.sendHeader("Connection", "close"); 145 | webServer.sendHeader(FPSTR(kACCESSCTL), "*"); 146 | webServer.setContentLength(CONTENT_LENGTH_UNKNOWN); 147 | webServer.send_P(200, kTEXTHTML, kHEADER1_C1); 148 | webServer.sendContent_P(kHEADER1_C2); 149 | webServer.sendContent_P(kHEADER1_C3); 150 | webServer.sendContent_P(kHEADER1_C4); 151 | webServer.sendContent_P(kHEADER1_C5); 152 | webServer.sendContent_P(kHEADER1_C6); 153 | 154 | snprintf(buffer, sizeof(buffer), "%u.%u.%u

SetupStatusParametersFirmware UpdateReboot", MAVESP8266_VERSION_MAJOR, MAVESP8266_VERSION_MINOR, MAVESP8266_VERSION_BUILD); 155 | webServer.sendContent(buffer); 156 | 157 | webServer.sendContent_P(kHEADER2); 158 | webServer.sendContent_P(kUPLOADSUCCESS); 159 | 160 | webServer.sendContent(""); 161 | 162 | if (updateCB) 163 | { 164 | updateCB->updateCompleted(); 165 | } 166 | delay(5000); // ensure the response is sent before rebooting 167 | ESP.restart(); // Restart the Kahuna 168 | } 169 | 170 | //--------------------------------------------------------------------------------- 171 | void handle_upload_status() 172 | { 173 | bool success = true; 174 | if (!started) 175 | { 176 | started = true; 177 | if (updateCB) 178 | { 179 | updateCB->updateStarted(); 180 | } 181 | } 182 | HTTPUpload &upload = webServer.upload(); 183 | if (upload.status == UPLOAD_FILE_START) 184 | { 185 | #ifdef DEBUG_SERIAL 186 | DEBUG_SERIAL.setDebugOutput(true); 187 | #endif 188 | WiFiUDP::stopAll(); 189 | #ifdef DEBUG_SERIAL 190 | DEBUG_SERIAL.printf("Update: %s\n", upload.filename.c_str()); 191 | #endif 192 | uint32_t maxSketchSpace = (ESP.getFreeSketchSpace() - 0x1000) & 0xFFFFF000; 193 | if (!Update.begin(maxSketchSpace)) 194 | { 195 | #ifdef DEBUG_SERIAL 196 | Update.printError(DEBUG_SERIAL); 197 | #endif 198 | success = false; 199 | } 200 | } 201 | else if (upload.status == UPLOAD_FILE_WRITE) 202 | { 203 | if (Update.write(upload.buf, upload.currentSize) != upload.currentSize) 204 | { 205 | #ifdef DEBUG_SERIAL 206 | Update.printError(DEBUG_SERIAL); 207 | #endif 208 | success = false; 209 | } 210 | } 211 | else if (upload.status == UPLOAD_FILE_END) 212 | { 213 | if (Update.end(true)) 214 | { 215 | #ifdef DEBUG_SERIAL 216 | DEBUG_SERIAL.printf("Update Success: %u\nRebooting...\n", upload.totalSize); 217 | #endif 218 | } 219 | else 220 | { 221 | #ifdef DEBUG_SERIAL 222 | Update.printError(DEBUG_SERIAL); 223 | #endif 224 | success = false; 225 | } 226 | #ifdef DEBUG_SERIAL 227 | DEBUG_SERIAL.setDebugOutput(false); 228 | #endif 229 | } 230 | yield(); 231 | if (!success) 232 | { 233 | if (updateCB) 234 | { 235 | updateCB->updateError(); 236 | } 237 | } 238 | } 239 | 240 | //--------------------------------------------------------------------------------- 241 | void handle_getParameters() // This can be improved a lot 242 | { 243 | setNoCacheHeaders(); 244 | 245 | webServer.setContentLength(CONTENT_LENGTH_UNKNOWN); 246 | webServer.send_P(200, kTEXTHTML, kHEADER1_C1); 247 | webServer.sendContent_P(kHEADER1_C2); 248 | webServer.sendContent_P(kHEADER1_C3); 249 | webServer.sendContent_P(kHEADER1_C4); 250 | webServer.sendContent_P(kHEADER1_C5); 251 | webServer.sendContent_P(kHEADER1_C6); 252 | 253 | snprintf(buffer, sizeof(buffer), "%u.%u.%u

SetupStatusParametersFirmware UpdateReboot", MAVESP8266_VERSION_MAJOR, MAVESP8266_VERSION_MINOR, MAVESP8266_VERSION_BUILD); 254 | webServer.sendContent(buffer); 255 | 256 | webServer.sendContent_P(kHEADER2); 257 | 258 | webServer.sendContent("
"); 259 | 260 | for (int i = 0; i < MavESP8266Parameters::ID_COUNT; i++) 261 | { 262 | if (i == getWorld()->getParameters()->ID_FWVER) 263 | { 264 | snprintf(buffer, sizeof(buffer), "", getWorld()->getParameters()->getAt(i)->id, MAVESP8266_VERSION_MAJOR, MAVESP8266_VERSION_MINOR, MAVESP8266_VERSION_BUILD); 265 | webServer.sendContent(buffer); 266 | } 267 | else if (i == getWorld()->getParameters()->ID_MODE) 268 | { 269 | snprintf(buffer, sizeof(buffer), "", getWorld()->getParameters()->getAt(i)->id, getWorld()->getParameters()->getWifiMode() == WIFI_MODE_AP ? "AP" : "STA"); 270 | webServer.sendContent(buffer); 271 | } 272 | else if (i == getWorld()->getParameters()->ID_IPADDRESS) 273 | { 274 | 275 | uint32_t local_ip = getWorld()->getParameters()->getLocalIPAddress(); 276 | snprintf(buffer, sizeof(buffer), "", getWorld()->getParameters()->getAt(i)->id, (int)(local_ip & 0xFF), (int)(local_ip >> 8 & 0xFF), (int)(local_ip >> 16 & 0xFF), (int)(local_ip >> 24 & 0xFF)); 277 | webServer.sendContent(buffer); 278 | } 279 | else if (i == getWorld()->getParameters()->ID_SSID1) 280 | { 281 | snprintf(buffer, sizeof(buffer), "", getWorld()->getParameters()->getAt(i)->id, getWorld()->getParameters()->getWifiSsid()); 282 | webServer.sendContent(buffer); 283 | } 284 | else if (i > getWorld()->getParameters()->ID_SSID1 && i <= getWorld()->getParameters()->ID_SSID4) 285 | { 286 | } 287 | else if (i == getWorld()->getParameters()->ID_PASS1) 288 | { 289 | snprintf(buffer, sizeof(buffer), "", getWorld()->getParameters()->getAt(i)->id, getWorld()->getParameters()->getWifiPassword()); 290 | webServer.sendContent(buffer); 291 | } 292 | else if (i > getWorld()->getParameters()->ID_PASS1 && i <= getWorld()->getParameters()->ID_PASS4) 293 | { 294 | } 295 | else if (i == getWorld()->getParameters()->ID_SSIDSTA1) 296 | { 297 | snprintf(buffer, sizeof(buffer), "", getWorld()->getParameters()->getAt(i)->id, getWorld()->getParameters()->getWifiStaSsid()); 298 | webServer.sendContent(buffer); 299 | } 300 | else if (i > getWorld()->getParameters()->ID_SSIDSTA1 && i <= getWorld()->getParameters()->ID_SSIDSTA4) 301 | { 302 | } 303 | else if (i == getWorld()->getParameters()->ID_PASSSTA1) 304 | { 305 | snprintf(buffer, sizeof(buffer), "", getWorld()->getParameters()->getAt(i)->id, getWorld()->getParameters()->getWifiStaPassword()); 306 | webServer.sendContent(buffer); 307 | } 308 | else if (i > getWorld()->getParameters()->ID_PASSSTA1 && i <= getWorld()->getParameters()->ID_PASSSTA4) 309 | { 310 | } 311 | else if (i == getWorld()->getParameters()->ID_IPSTA) 312 | { 313 | uint32_t wifi_sta_ip = getWorld()->getParameters()->getWifiStaIP(); 314 | snprintf(buffer, sizeof(buffer), "", getWorld()->getParameters()->getAt(i)->id, (int)(wifi_sta_ip & 0xFF), (int)(wifi_sta_ip >> 8 & 0xFF), (int)(wifi_sta_ip >> 16 & 0xFF), (int)(wifi_sta_ip >> 24 & 0xFF)); 315 | webServer.sendContent(buffer); 316 | } 317 | else if (i == getWorld()->getParameters()->ID_GATEWAYSTA) 318 | { 319 | uint32_t wifi_sta_gateway = getWorld()->getParameters()->getWifiStaGateway(); 320 | snprintf(buffer, sizeof(buffer), "", getWorld()->getParameters()->getAt(i)->id, (int)(wifi_sta_gateway & 0xFF), (int)(wifi_sta_gateway >> 8 & 0xFF), (int)(wifi_sta_gateway >> 16 & 0xFF), (int)(wifi_sta_gateway >> 24 & 0xFF)); 321 | webServer.sendContent(buffer); 322 | } 323 | else if (i == getWorld()->getParameters()->ID_SUBNETSTA) 324 | { 325 | uint32_t wifi_sta_subnet = getWorld()->getParameters()->getWifiStaSubnet(); 326 | snprintf(buffer, sizeof(buffer), "", getWorld()->getParameters()->getAt(i)->id, (int)(wifi_sta_subnet & 0xFF), (int)(wifi_sta_subnet >> 8 & 0xFF), (int)(wifi_sta_subnet >> 16 & 0xFF), (int)(wifi_sta_subnet >> 24 & 0xFF)); 327 | webServer.sendContent(buffer); 328 | } 329 | else // integer values 330 | { 331 | unsigned long val = 0; 332 | if (getWorld()->getParameters()->getAt(i)->type == MAV_PARAM_TYPE_UINT32) 333 | val = (unsigned long)*((uint32_t *)getWorld()->getParameters()->getAt(i)->value); 334 | else if (getWorld()->getParameters()->getAt(i)->type == MAV_PARAM_TYPE_UINT16) 335 | val = (unsigned long)*((uint16_t *)getWorld()->getParameters()->getAt(i)->value); 336 | else 337 | val = (unsigned long)*((int8_t *)getWorld()->getParameters()->getAt(i)->value); 338 | 339 | snprintf(buffer, sizeof(buffer), "", getWorld()->getParameters()->getAt(i)->id, val); 340 | webServer.sendContent(buffer); 341 | } 342 | } 343 | webServer.sendContent("
NameValue
%s%u.%u.%u
%s%s
%s%d.%d.%d.%d
%s%s
%s%s
%s%s
%s%s
%s%d.%d.%d.%d
%s%d.%d.%d.%d
%s%d.%d.%d.%d
%s%lu
"); 344 | webServer.sendContent(""); 345 | } 346 | 347 | //--------------------------------------------------------------------------------- 348 | static void handle_setup() 349 | { 350 | setNoCacheHeaders(); 351 | webServer.setContentLength(CONTENT_LENGTH_UNKNOWN); 352 | 353 | webServer.send_P(200, kTEXTHTML, kHEADER1_C1); 354 | webServer.sendContent_P(kHEADER1_C2); 355 | webServer.sendContent_P(kHEADER1_C3); 356 | webServer.sendContent_P(kHEADER1_C4); 357 | webServer.sendContent_P(kHEADER1_C5); 358 | webServer.sendContent_P(kHEADER1_C6); 359 | 360 | snprintf(buffer, sizeof(buffer), 361 | "%u.%u.%u

" 362 | "SetupStatus" 363 | "ParametersFirmware Update" 364 | "Reboot", 365 | MAVESP8266_VERSION_MAJOR, 366 | MAVESP8266_VERSION_MINOR, 367 | MAVESP8266_VERSION_BUILD); 368 | webServer.sendContent(buffer); 369 | 370 | webServer.sendContent_P(kHEADER2); 371 | 372 | snprintf(buffer, sizeof(buffer), 373 | "
" 374 | "
" 375 | "
" 376 | "
" 377 | "
" 378 | " " 379 | "" 380 | "
" 381 | "
" 382 | "
" 383 | "
" 384 | "
" 405 | "
" 406 | "
" 407 | "
" 408 | "
" 409 | "
" 410 | "
" 411 | "
" 412 | "
" 413 | "
" 414 | "
" 415 | "
" 416 | "
" 417 | "
" 418 | "
" 419 | "
" 420 | "
" 421 | "
" 422 | "
" 423 | "
" 424 | "
" 425 | "
" 447 | "
" 448 | "
" 449 | "
" 450 | "
" 451 | "
" 452 | "
" 453 | "
" 454 | "
" 455 | "
", 456 | getWorld()->getParameters()->getWifiStaSsid(), 457 | getWorld()->getParameters()->getWifiStaPassword()); 458 | webServer.sendContent(buffer); 459 | 460 | IPAddress StaIP = getWorld()->getParameters()->getWifiStaIP(); 461 | IPAddress StaGateway = getWorld()->getParameters()->getWifiStaGateway(); 462 | IPAddress StaSubnet = getWorld()->getParameters()->getWifiStaSubnet(); 463 | snprintf(buffer, sizeof(buffer), 464 | "
" 465 | "
" 466 | "
" 467 | "
" 468 | "
" 469 | "
" 470 | "
" 471 | "
" 472 | "
" 473 | "
" 474 | "
" 475 | "
" 476 | "
", 477 | StaIP.toString().c_str(), 478 | StaGateway.toString().c_str(), 479 | StaSubnet.toString().c_str()); 480 | webServer.sendContent(buffer); 481 | 482 | webServer.sendContent_P(kDEFAULTBTN); 483 | webServer.sendContent(""); 484 | } 485 | 486 | //--------------------------------------------------------------------------------- 487 | static void handle_getStatus() 488 | { 489 | if (!flash) 490 | flash = ESP.getFreeSketchSpace(); 491 | if (!paramCRC[0]) 492 | { 493 | snprintf(paramCRC, sizeof(paramCRC), "%08X", getWorld()->getParameters()->paramHashCheck()); 494 | } 495 | linkStatus *gcsStatus = getWorld()->getGCS()->getStatus(); 496 | linkStatus *vehicleStatus = getWorld()->getVehicle()->getStatus(); 497 | 498 | setNoCacheHeaders(); 499 | 500 | webServer.setContentLength(CONTENT_LENGTH_UNKNOWN); 501 | webServer.send_P(200, kTEXTHTML, kHEADER1_C1); 502 | webServer.sendContent_P(kHEADER1_C2); 503 | webServer.sendContent_P(kHEADER1_C3); 504 | webServer.sendContent_P(kHEADER1_C4); 505 | webServer.sendContent_P(kHEADER1_C5); 506 | webServer.sendContent_P(kHEADER1_C6); 507 | 508 | snprintf(buffer, sizeof(buffer), "%u.%u.%u

SetupStatusParametersFirmware UpdateReboot", MAVESP8266_VERSION_MAJOR, MAVESP8266_VERSION_MINOR, MAVESP8266_VERSION_BUILD); 509 | webServer.sendContent(buffer); 510 | 511 | webServer.sendContent_P(kHEADER2); 512 | 513 | snprintf(buffer, sizeof(buffer), "

Comm Status
Packets Received from GCS%u
Packets Sent to GCS%u
GCS Packets Lost%u
Packets Received from Vehicle%u
Packets Sent to Vehicle%u
Vehicle Packets Lost%u
Radio Messages%u

System Status
Flash Size%u
Flash Available%u
RAM Left%u
Parameters CRC'%s'

", gcsStatus->packets_received, gcsStatus->packets_sent, gcsStatus->packets_lost, vehicleStatus->packets_received, vehicleStatus->packets_sent, vehicleStatus->packets_lost, gcsStatus->radio_status_sent, ESP.getFlashChipRealSize(), flash, ESP.getFreeHeap(), paramCRC); 514 | 515 | webServer.sendContent(buffer); 516 | 517 | webServer.sendContent(""); 518 | } 519 | 520 | //--------------------------------------------------------------------------------- 521 | void handle_getJLog() 522 | { 523 | uint32_t position = 0, len; 524 | if (webServer.hasArg(kPOSITION)) 525 | { 526 | position = webServer.arg(kPOSITION).toInt(); 527 | } 528 | String logText = getWorld()->getLogger()->getLog(&position, &len); 529 | char jStart[128]; 530 | snprintf(jStart, 128, "{\"len\":%d, \"start\":%d, \"text\": \"", len, position); 531 | String payLoad = jStart; 532 | payLoad += logText; 533 | payLoad += "\"}"; 534 | webServer.send(200, FPSTR(kAPPJSON), payLoad); 535 | } 536 | 537 | //--------------------------------------------------------------------------------- 538 | void handle_getJSysInfo() 539 | { 540 | if (!flash) 541 | flash = ESP.getFreeSketchSpace(); 542 | if (!paramCRC[0]) 543 | { 544 | snprintf(paramCRC, sizeof(paramCRC), "%08X", getWorld()->getParameters()->paramHashCheck()); 545 | } 546 | uint32_t fid = spi_flash_get_id(); 547 | char message[512]; 548 | snprintf(message, 512, 549 | "{ " 550 | "\"size\": \"%s\", " 551 | "\"id\": \"0x%02lX 0x%04lX\", " 552 | "\"flashfree\": \"%u\", " 553 | "\"heapfree\": \"%u\", " 554 | "\"logsize\": \"%u\", " 555 | "\"paramcrc\": \"%s\"" 556 | " }", 557 | kFlashMaps[system_get_flash_size_map()], 558 | (long unsigned int)(fid & 0xff), (long unsigned int)((fid & 0xff00) | ((fid >> 16) & 0xff)), 559 | flash, 560 | ESP.getFreeHeap(), 561 | getWorld()->getLogger()->getPosition(), 562 | paramCRC); 563 | webServer.send(200, "application/json", message); 564 | } 565 | 566 | //--------------------------------------------------------------------------------- 567 | void handle_getJSysStatus() 568 | { 569 | bool reset = false; 570 | if (webServer.hasArg("r")) 571 | { 572 | reset = webServer.arg("r").toInt() != 0; 573 | } 574 | linkStatus *gcsStatus = getWorld()->getGCS()->getStatus(); 575 | linkStatus *vehicleStatus = getWorld()->getVehicle()->getStatus(); 576 | if (reset) 577 | { 578 | memset(gcsStatus, 0, sizeof(linkStatus)); 579 | memset(vehicleStatus, 0, sizeof(linkStatus)); 580 | } 581 | char message[512]; 582 | snprintf(message, 512, 583 | "{ " 584 | "\"gpackets\": \"%u\", " 585 | "\"gsent\": \"%u\", " 586 | "\"glost\": \"%u\", " 587 | "\"vpackets\": \"%u\", " 588 | "\"vsent\": \"%u\", " 589 | "\"vlost\": \"%u\", " 590 | "\"radio\": \"%u\", " 591 | "\"buffer\": \"%u\"" 592 | " }", 593 | gcsStatus->packets_received, 594 | gcsStatus->packets_sent, 595 | gcsStatus->packets_lost, 596 | vehicleStatus->packets_received, 597 | vehicleStatus->packets_sent, 598 | vehicleStatus->packets_lost, 599 | gcsStatus->radio_status_sent, 600 | vehicleStatus->queue_status); 601 | webServer.send(200, "application/json", message); 602 | } 603 | 604 | //--------------------------------------------------------------------------------- 605 | void handle_setParameters() 606 | { 607 | if (webServer.args() == 0) 608 | { 609 | returnFail(kBADARG); 610 | return; 611 | } 612 | bool ok = false; 613 | bool reboot = false; 614 | if (webServer.hasArg(kBAUD)) 615 | { 616 | ok = true; 617 | getWorld()->getParameters()->setUartBaudRate(webServer.arg(kBAUD).toInt()); 618 | } 619 | if (webServer.hasArg(kPWD)) 620 | { 621 | // Ensure password is between 8 characters and 16 characters 622 | if (webServer.arg(kPWD).length() >= 8 && webServer.arg(kPWD).length() <= 16) 623 | { 624 | ok = true; 625 | getWorld()->getParameters()->setWifiPassword(webServer.arg(kPWD).c_str()); 626 | } 627 | } 628 | if (webServer.hasArg(kSSID)) 629 | { 630 | // Ensure SSID is less than or equal to 16 characters 631 | if (webServer.arg(kSSID).length() <= 16) 632 | { 633 | ok = true; 634 | getWorld()->getParameters()->setWifiSsid(webServer.arg(kSSID).c_str()); 635 | } 636 | } 637 | if (webServer.hasArg(kPWDSTA)) 638 | { 639 | ok = true; 640 | getWorld()->getParameters()->setWifiStaPassword(webServer.arg(kPWDSTA).c_str()); 641 | } 642 | if (webServer.hasArg(kSSIDSTA)) 643 | { 644 | ok = true; 645 | getWorld()->getParameters()->setWifiStaSsid(webServer.arg(kSSIDSTA).c_str()); 646 | } 647 | if (webServer.hasArg(kIPSTA)) 648 | { 649 | IPAddress ip; 650 | ip.fromString(webServer.arg(kIPSTA).c_str()); 651 | getWorld()->getParameters()->setWifiStaIP(ip); 652 | } 653 | if (webServer.hasArg(kGATESTA)) 654 | { 655 | IPAddress ip; 656 | ip.fromString(webServer.arg(kGATESTA).c_str()); 657 | getWorld()->getParameters()->setWifiStaGateway(ip); 658 | } 659 | if (webServer.hasArg(kSUBSTA)) 660 | { 661 | IPAddress ip; 662 | ip.fromString(webServer.arg(kSUBSTA).c_str()); 663 | getWorld()->getParameters()->setWifiStaSubnet(ip); 664 | } 665 | if (webServer.hasArg(kCPORT)) 666 | { 667 | ok = true; 668 | getWorld()->getParameters()->setWifiUdpCport(webServer.arg(kCPORT).toInt()); 669 | } 670 | if (webServer.hasArg(kHPORT)) 671 | { 672 | ok = true; 673 | getWorld()->getParameters()->setWifiUdpHport(webServer.arg(kHPORT).toInt()); 674 | } 675 | if (webServer.hasArg(kCHANNEL)) 676 | { 677 | ok = true; 678 | getWorld()->getParameters()->setWifiChannel(webServer.arg(kCHANNEL).toInt()); 679 | } 680 | if (webServer.hasArg(kDEBUG)) 681 | { 682 | ok = true; 683 | getWorld()->getParameters()->setDebugEnabled(webServer.arg(kDEBUG).toInt()); 684 | } 685 | if (webServer.hasArg(kMODE)) 686 | { 687 | ok = true; 688 | getWorld()->getParameters()->setWifiMode(webServer.arg(kMODE).toInt()); 689 | } 690 | if (webServer.hasArg(kREBOOT)) 691 | { 692 | ok = true; 693 | reboot = webServer.arg(kREBOOT) == "1"; 694 | } 695 | if (ok) 696 | { 697 | getWorld()->getParameters()->saveAllToEeprom(); 698 | //-- Send new parameters back 699 | handle_getParameters(); 700 | if (reboot) 701 | { 702 | delay(100); 703 | ESP.restart(); 704 | } 705 | } 706 | else 707 | returnFail(kBADARG); 708 | } 709 | 710 | void handle_setDefaults() 711 | { 712 | getWorld()->getParameters()->resetToDefaults(); 713 | getWorld()->getParameters()->saveAllToEeprom(); 714 | handle_getParameters(); 715 | } 716 | 717 | //--------------------------------------------------------------------------------- 718 | static void handle_reboot() 719 | { 720 | setNoCacheHeaders(); 721 | webServer.setContentLength(CONTENT_LENGTH_UNKNOWN); 722 | webServer.send_P(200, kTEXTHTML, kHEADER1_C1); 723 | webServer.sendContent_P(kHEADER1_C2); 724 | webServer.sendContent_P(kHEADER1_C3); 725 | webServer.sendContent_P(kHEADER1_C4); 726 | webServer.sendContent_P(kHEADER1_C5); 727 | webServer.sendContent_P(kHEADER1_C6); 728 | 729 | snprintf(buffer, sizeof(buffer), "%u.%u.%u

SetupStatusParametersFirmware UpdateReboot", MAVESP8266_VERSION_MAJOR, MAVESP8266_VERSION_MINOR, MAVESP8266_VERSION_BUILD); 730 | webServer.sendContent(buffer); 731 | 732 | webServer.sendContent_P(kHEADER2); 733 | 734 | webServer.sendContent("
rebooting ...
"); 735 | webServer.sendContent(""); 736 | delay(500); 737 | ESP.restart(); 738 | } 739 | 740 | //--------------------------------------------------------------------------------- 741 | //-- 404 742 | void handle_notFound() 743 | { 744 | String message = "File Not Found\n\n"; 745 | message += "URI: "; 746 | message += webServer.uri(); 747 | message += "\nMethod: "; 748 | message += (webServer.method() == HTTP_GET) ? "GET" : "POST"; 749 | message += "\nArguments: "; 750 | message += webServer.args(); 751 | message += "\n"; 752 | for (uint8_t i = 0; i < webServer.args(); i++) 753 | { 754 | message += " " + webServer.argName(i) + ": " + webServer.arg(i) + "\n"; 755 | } 756 | webServer.send(404, FPSTR(kTEXTPLAIN), message); 757 | } 758 | 759 | //--------------------------------------------------------------------------------- 760 | MavESP8266Httpd::MavESP8266Httpd() 761 | { 762 | } 763 | 764 | //--------------------------------------------------------------------------------- 765 | //-- Initialize 766 | void MavESP8266Httpd::begin(MavESP8266Update *updateCB_) 767 | { 768 | updateCB = updateCB_; 769 | webServer.on("/", handle_setup); 770 | webServer.on("/setparameters", handle_setParameters); 771 | webServer.on("/getparameters", handle_getParameters); 772 | webServer.on("/setdefaults", handle_setDefaults); 773 | webServer.on("/getstatus", handle_getStatus); 774 | webServer.on("/reboot", handle_reboot); 775 | webServer.on("/info.json", handle_getJSysInfo); 776 | webServer.on("/status.json", handle_getJSysStatus); 777 | webServer.on("/log.json", handle_getJLog); 778 | webServer.on("/update", handle_update); 779 | webServer.on("/upload", HTTP_POST, handle_upload, handle_upload_status); 780 | webServer.onNotFound(handle_notFound); 781 | webServer.begin(); 782 | } 783 | 784 | //--------------------------------------------------------------------------------- 785 | //-- Initialize 786 | void MavESP8266Httpd::checkUpdates() 787 | { 788 | webServer.handleClient(); 789 | } 790 | --------------------------------------------------------------------------------