diff --git a/libcopter-arduino/.gitignore b/libcopter-arduino/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/libcopter-arduino/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/libcopter-arduino/.travis.yml b/libcopter-arduino/.travis.yml new file mode 100644 index 0000000..7c486f1 --- /dev/null +++ b/libcopter-arduino/.travis.yml @@ -0,0 +1,67 @@ +# Continuous Integration (CI) is the practice, in software +# engineering, of merging all developer working copies with a shared mainline +# several times a day < https://docs.platformio.org/page/ci/index.html > +# +# Documentation: +# +# * Travis CI Embedded Builds with PlatformIO +# < https://docs.travis-ci.com/user/integration/platformio/ > +# +# * PlatformIO integration with Travis CI +# < https://docs.platformio.org/page/ci/travis.html > +# +# * User Guide for `platformio ci` command +# < https://docs.platformio.org/page/userguide/cmd_ci.html > +# +# +# Please choose one of the following templates (proposed below) and uncomment +# it (remove "# " before each line) or use own configuration according to the +# Travis CI documentation (see above). +# + + +# +# Template #1: General project. Test it using existing `platformio.ini`. +# + +# language: python +# python: +# - "2.7" +# +# sudo: false +# cache: +# directories: +# - "~/.platformio" +# +# install: +# - pip install -U platformio +# - platformio update +# +# script: +# - platformio run + + +# +# Template #2: The project is intended to be used as a library with examples. +# + +# language: python +# python: +# - "2.7" +# +# sudo: false +# cache: +# directories: +# - "~/.platformio" +# +# env: +# - PLATFORMIO_CI_SRC=path/to/test/file.c +# - PLATFORMIO_CI_SRC=examples/file.ino +# - PLATFORMIO_CI_SRC=path/to/test/directory +# +# install: +# - pip install -U platformio +# - platformio update +# +# script: +# - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N diff --git a/libcopter-arduino/README.md b/libcopter-arduino/README.md new file mode 100644 index 0000000..2d9dbe0 --- /dev/null +++ b/libcopter-arduino/README.md @@ -0,0 +1,22 @@ +# libcopter for ESP32 + +# Connections + +| ESP32 Pin | Function | Input | +|---|---|---| +| GPIO36 | ADC1_CH0 | Throttle | +| GPIO39 | ADC1_CH3 | Pitch | +| GPIO34 | ADC1_CH6 | Yaw | +| GPIO35 | ADC1_CH7 | Roll | +| GPIO32 | ADC1_CH4 | NC | +| GPIO33 | ADC1_CH5 | NC | +| GPIO25 | Digital Input, Int. Pull-Down, Active Low | Takeoff | +| GPIO26 | Digital Input, Int. Pull-Down, Active Low | Land | +| GPIO27 | Digital Input, Int. Pull-Down, Active Low | Panic | +| GPIO14 | Digital Input, Int. Pull-Down, Active Low | Calibrate | +| GPIO23 | Digital Output, Active Low | Connection Indicator | +| GPIO19 | Digital Output, Active Low | Connection Indicator | +| GPIO18 | Digital Output, Active Low | Connection Indicator | +| GPIO5 | Digital Output, Active Low | Connection Indicator | + +![](images/ESP32-DOIT-DEVKIT-V1-Board-Pinout-30-GPIOs.webp) diff --git a/libcopter-arduino/images/ESP32-DOIT-DEVKIT-V1-Board-Pinout-30-GPIOs.webp b/libcopter-arduino/images/ESP32-DOIT-DEVKIT-V1-Board-Pinout-30-GPIOs.webp new file mode 100644 index 0000000..a2b46b2 Binary files /dev/null and b/libcopter-arduino/images/ESP32-DOIT-DEVKIT-V1-Board-Pinout-30-GPIOs.webp differ diff --git a/libcopter-arduino/include/README b/libcopter-arduino/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/libcopter-arduino/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/libcopter-arduino/include/sg500.hpp b/libcopter-arduino/include/sg500.hpp new file mode 100644 index 0000000..3b4f8f9 --- /dev/null +++ b/libcopter-arduino/include/sg500.hpp @@ -0,0 +1,27 @@ +#include +#include +#include + +class SG500 +{ + WiFiUDP udp; + const char *udpAddress = "172.16.10.1"; + const int udpPort = 8080; + const char *ssid = "JJRC-0df227"; + const char *pwd = NULL; + + public: + SG500(); + + void beginInit(); + bool initReady(); + + bool command(float roll, float pitch, float yaw, float height, bool launch = false, bool panic = false, bool land = false, bool recalibrate = false); + + protected: + void init(); + void makeCommand(byte *command, byte height, byte yaw, byte pitch, byte roll, bool launch, bool panic, bool land, bool recalibrate, bool auto_altitude = true, + byte yaw_trim = 0x10, byte pitch_trim = 0x10, byte roll_trim = 0x10, bool compass = false, byte percent_raw = 0); + + boolean request(const byte *data, int length); +}; diff --git a/libcopter-arduino/lib/README b/libcopter-arduino/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/libcopter-arduino/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/libcopter-arduino/platformio.ini b/libcopter-arduino/platformio.ini new file mode 100644 index 0000000..fdf7796 --- /dev/null +++ b/libcopter-arduino/platformio.ini @@ -0,0 +1,15 @@ +;PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:esp32dev] +platform = espressif32 +board = esp32dev +framework = arduino +monitor_speed = 115200 diff --git a/libcopter-arduino/src/main.cpp b/libcopter-arduino/src/main.cpp new file mode 100644 index 0000000..99e1522 --- /dev/null +++ b/libcopter-arduino/src/main.cpp @@ -0,0 +1,105 @@ +#include +#include +#include +#include + +uint16_t throttle_raw; +uint16_t pitch_raw; +uint16_t yaw_raw; +uint16_t roll_raw; + +float throttle; +float pitch; +float yaw; +float roll; + + +int takeoff; +int land; +int panic; +int calibrate; + +SG500 copter; + +//#define DONT_CONNECT + +float convertAnalog(uint16_t value) +{ + return ((((float)value) / 4096) - 0.5f) * 2.0f * 0.8f; +} + +void input() { + throttle_raw = analogRead(36); + pitch_raw = analogRead(39); + yaw_raw = analogRead(34); + roll_raw = analogRead(35); + + throttle = convertAnalog(throttle_raw) + 0.02f; + pitch = -convertAnalog(pitch_raw) - 0.02f; + yaw = convertAnalog(yaw_raw) + 0.02f; + roll = convertAnalog(roll_raw) + 0.03f; + + takeoff = digitalRead(25) == 0; + land = digitalRead(26) == 0; + panic = digitalRead(27) == 0; + calibrate = digitalRead(14) == 0; + + Serial.print("T: ");Serial.print(throttle);Serial.print(", "); + Serial.print("Y: ");Serial.print(yaw);Serial.print(", "); + Serial.print("P: ");Serial.print(pitch);Serial.print(", "); + Serial.print("R: ");Serial.print(roll);Serial.print(", "); + Serial.print("Takeoff: ");Serial.print(takeoff);Serial.print(", "); + Serial.print("Land: ");Serial.print(land);Serial.print(", "); + Serial.print("Panic: ");Serial.print(panic);Serial.print(", "); + Serial.print("Calibrate: ");Serial.print(calibrate); + Serial.println(); +} + +void led(byte leds) { + digitalWrite(23, (leds & 1) == 0); + digitalWrite(19, (leds & 2) == 0); + digitalWrite(18, (leds & 4) == 0); + digitalWrite(5, (leds & 8) == 0); +} + +byte led_blink = 1; +void setup() { + pinMode(25, INPUT_PULLDOWN); + pinMode(26, INPUT_PULLDOWN); + pinMode(27, INPUT_PULLDOWN); + pinMode(14, INPUT_PULLDOWN); + pinMode(23, OUTPUT); + pinMode(19, OUTPUT); + pinMode(18, OUTPUT); + pinMode(5, OUTPUT); + led(0); + Serial.begin(115200); + +#ifndef DONT_CONNECT + copter.beginInit(); + while(!copter.initReady()) + { + led(led_blink); + delay(500); + led_blink <<= 1; + if(led_blink > 8) + led_blink = 1; + Serial.print("."); + } +#endif + led(0xff); +} + +void loop() { + input(); + +#ifndef DONT_CONNECT + if(!copter.command(roll, pitch, yaw, throttle, takeoff, panic, land, calibrate)) + { + Serial.println("Send error, restart."); + ESP.restart(); + } +#endif + + delay(20); +} diff --git a/libcopter-arduino/src/sg500.cpp b/libcopter-arduino/src/sg500.cpp new file mode 100644 index 0000000..af417f6 --- /dev/null +++ b/libcopter-arduino/src/sg500.cpp @@ -0,0 +1,144 @@ +#include + +SG500::SG500() +{ + +} + + +void SG500::beginInit() +{ + WiFi.begin(ssid, pwd); +} + +bool SG500::initReady() +{ + if (WiFi.status() != WL_CONNECTED) { + return false; + } + else + { + init(); + return true; + } +} + +void SG500::init() +{ + byte buffer[1]; + + Serial.println(""); + Serial.print("Connected to "); + Serial.println(ssid); + Serial.print("IP address: "); + Serial.println(WiFi.localIP()); + + udp.begin(udpPort); + + // reset + Serial.println("Reset"); + buffer[0] = 0x0F; + udp.beginPacket(udpAddress, udpPort); + udp.write(buffer, 1); + udp.endPacket(); + + delay(1000); + + // type + Serial.println("Type"); + buffer[0] = 0x28; + while(!request(buffer, 1)); + // version + Serial.println("Version"); + buffer[0] = 0x42; + while(!request(buffer, 1)); +} + +bool SG500::command(float roll, float pitch, float yaw, float height, bool launch, bool panic, bool land, bool recalibrate) +{ + byte buffer[11]; + + height = constrain(height, -1, 1); + yaw = constrain(yaw, -1, 1); + pitch = constrain(pitch, -1, 1); + roll = constrain(roll, -1, 1); + + makeCommand( + buffer, + (byte)(height * 0x7E + 0x7E), + (byte)(yaw * 0x3F + 0x3F), + (byte)(pitch * 0x3F + 0x40), + (byte)(roll * 0x3F + 0x3F), + launch, panic, land, recalibrate + ); + + if(!udp.beginPacket(udpAddress, udpPort)) + { + return false; + } + udp.write(buffer, sizeof(buffer)); + + if(!udp.endPacket()) + { + return false; + } + + return true; +} + +void SG500::makeCommand(byte *command, byte height, byte yaw, byte pitch, byte roll, bool launch, bool panic, bool land, bool recalibrate, bool auto_altitude, + byte yaw_trim, byte pitch_trim, byte roll_trim, bool compass, byte percent_raw) +{ + command[0] = 0xFF; + command[1] = 0x08; + command[2] = height & 0xFF; + command[3] = yaw & 0x7F; + command[4] = pitch & 0x7F; + command[5] = roll & 0x7F; + + command[6] = 0; + if (auto_altitude) command[6] |= 0x80; + if (recalibrate) command[6] |= 0x40; + + command[6] |= yaw_trim & 0x3F; + command[7] = pitch_trim & 0x3F; + command[8] = roll_trim & 0x3F; + + command[9] = 0; + if (launch) command[9] |= 0x40; + if (panic) command[9] |= 0x20; + if (land) command[9] |= 0x80; + if (compass) command[9] |= 0x10; + + command[9] |= percent_raw & 0x03; + + // checksum + uint8_t sum = 0; + for (int i=1; i<10; i++) sum += uint8_t(command[i]); + command[10] = 0xFF - sum; +} + + +boolean SG500::request(const byte *data, int length) +{ + byte buffer[64]; + boolean ok = false; + + while(!ok) + { + udp.beginPacket(udpAddress, udpPort); + udp.write(data, length); + udp.endPacket(); + + delay(100); + + udp.parsePacket(); + if(udp.read(buffer, sizeof(buffer)) > 0){ + Serial.print("Received: "); + Serial.println((char *)buffer); + ok = true; + } + } + + return ok; +} diff --git a/libcopter-arduino/test/README b/libcopter-arduino/test/README new file mode 100644 index 0000000..df5066e --- /dev/null +++ b/libcopter-arduino/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PIO Unit Testing and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PIO Unit Testing: +- https://docs.platformio.org/page/plus/unit-testing.html