├── .gitignore ├── .clang-format ├── library.json ├── examples └── BasicExample │ └── BasicExample.ino ├── include ├── msp_protocol.h └── hdzero.h ├── README.md └── src └── hdzero.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .vscode/.browse.c_cpp.db* 3 | .vscode/c_cpp_properties.json 4 | .vscode/launch.json 5 | .vscode/ipch 6 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | TabWidth: 4 2 | IndentWidth: 4 3 | UseTab: Never 4 | ColumnLimit: 0 5 | NamespaceIndentation: All 6 | ConstructorInitializerAllOnOneLineOrOnePerLine: false 7 | BreakConstructorInitializers: BeforeComma 8 | IndentPPDirectives: None 9 | AlignConsecutiveMacros: AcrossComments 10 | -------------------------------------------------------------------------------- /library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "hdzero-displayport", 3 | "version": "0.0.2", 4 | "description": "A library to drive the hdzero vtx via msp displayport", 5 | "keywords": "planet, happiness, people", 6 | "repository": { 7 | "type": "git", 8 | "url": "https://github.com/bkleiner/hdzero-displayport.git" 9 | }, 10 | "authors": [ 11 | { 12 | "name": "Benedikt Kleiner", 13 | "email": "benedikt.kleiner@gmail.com", 14 | "maintainer": true 15 | } 16 | ], 17 | "license": "MIT", 18 | "frameworks": [ 19 | "arduino" 20 | ], 21 | "platforms": "*" 22 | } -------------------------------------------------------------------------------- /examples/BasicExample/BasicExample.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | HDZero hdzero; 6 | 7 | void setup() { 8 | // wait for serial to be ready 9 | Serial2.begin(115200); 10 | while (!Serial2) 11 | ; 12 | 13 | // setup and configure hdzero 14 | hdzero.begin(Serial2); 15 | hdzero.setFcVariant("INAV"); 16 | hdzero.setResolution(HD_5018); 17 | } 18 | 19 | uint32_t lastRefreshTime = 0; 20 | 21 | int8_t pointXDirection = 1; 22 | uint8_t pointX = 0; 23 | 24 | int8_t pointYDirection = 1; 25 | uint8_t pointY = 0; 26 | 27 | void loop() { 28 | if (millis() - lastRefreshTime >= 500) { 29 | hdzero.clear(); 30 | hdzero.writeString(0, 10, 10, "HELLO ARDUINO!"); 31 | hdzero.writeString(0, pointX, pointY, "AHHH"); 32 | 33 | pointX += pointXDirection; 34 | if (pointX + 4 >= 50 || pointX == 0) { 35 | pointXDirection = -pointXDirection; 36 | } 37 | pointY += pointYDirection; 38 | if (pointY >= 18 || pointY == 0) { 39 | pointYDirection = -pointYDirection; 40 | } 41 | hdzero.draw(); 42 | 43 | lastRefreshTime = millis(); 44 | } 45 | 46 | hdzero.run(); 47 | } -------------------------------------------------------------------------------- /include/msp_protocol.h: -------------------------------------------------------------------------------- 1 | #ifndef msp_protocol_h 2 | #define msp_protocol_h 3 | 4 | #define MSP_FC_VARIANT_SIZE 4 5 | #define MSP_RC_CHANNEL_COUNT 16 6 | 7 | #define MSP_FC_VARIANT 2 // out message 8 | #define MSP_VTX_CONFIG 88 // out message Get vtx settings - betaflight 9 | #define MSP_STATUS 101 // out message cycletime & errors_count & sensor present & box activation & current setting number 10 | #define MSP_RC 105 // out message rc channels and more 11 | 12 | #define MSP_DISPLAYPORT 182 13 | 14 | #define MSP_DP_SUBCMD_HEARTBEAT 0 15 | #define MSP_DP_SUBCMD_RELEASE 1 16 | #define MSP_DP_SUBCMD_CLEAR 2 17 | #define MSP_DP_SUBCMD_WRITE 3 18 | #define MSP_DP_SUBCMD_DRAW 4 19 | #define MSP_DP_SUBCMD_CONFIG 5 20 | 21 | struct MSPVTXConfig { 22 | uint8_t vtx_type; 23 | uint8_t band; 24 | uint8_t channel; 25 | uint8_t power; 26 | uint8_t pitmode; 27 | uint8_t freq_lsb; 28 | uint8_t freq_msb; 29 | uint8_t device_is_ready; 30 | uint8_t low_power_disarm; 31 | uint8_t pit_mode_freq_lsb; 32 | uint8_t pit_mode_freq_msb; 33 | uint8_t vtx_table_available; 34 | uint8_t bands; 35 | uint8_t channels; 36 | uint8_t power_levels; 37 | }; 38 | 39 | #endif -------------------------------------------------------------------------------- /include/hdzero.h: -------------------------------------------------------------------------------- 1 | #ifndef hdzero_h 2 | #define hdzero_h 3 | 4 | #include "Arduino.h" 5 | 6 | #include "msp_protocol.h" 7 | 8 | enum HDZeroResolution { 9 | SD_3016, 10 | HD_5018, 11 | HD_3016, 12 | }; 13 | 14 | class HDZero { 15 | public: 16 | HDZero(); 17 | 18 | void begin(Stream &stream); 19 | void end(); 20 | 21 | // needs to be run every loop iteration 22 | void run(); 23 | 24 | void setFcVariant(const char *variant); 25 | void setRcChannels(uint16_t values[MSP_RC_CHANNEL_COUNT]); 26 | void setRcChannel(uint8_t index, uint16_t value); 27 | void setAllRcChannels(uint16_t value); 28 | 29 | void setResolution(HDZeroResolution value); 30 | void setArmed(bool armed); 31 | 32 | void write(uint8_t fontpage, uint8_t x, uint8_t y, uint8_t *data, uint8_t size); 33 | void writeString(uint8_t fontpage, uint8_t x, uint8_t y, const char *string); 34 | void clear(); 35 | void draw(); 36 | 37 | private: 38 | void send(uint8_t cmd, uint8_t direction, uint8_t *payload, uint8_t size); 39 | void sendResponse(uint8_t cmd, uint8_t *payload, uint8_t size); 40 | void sendSubCmd(uint8_t cmd, uint8_t *payload, uint8_t size); 41 | void sendError(uint8_t cmd); 42 | void handleMsp(uint8_t cmd, uint8_t *payload, uint8_t size); 43 | 44 | Stream *stream; 45 | HDZeroResolution resolution; 46 | uint8_t fcVariant[MSP_FC_VARIANT_SIZE]; 47 | uint16_t rcChannels[MSP_RC_CHANNEL_COUNT]; 48 | bool isArmed; 49 | MSPVTXConfig vtxConfig; 50 | }; 51 | 52 | #endif -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # HDZero Arduino Library 2 | 3 | Arduino Library to interact with HDZero VTXes via serial port. 4 | 5 | ![arduino_demo](https://user-images.githubusercontent.com/15615439/202908662-ba035698-c40f-4865-b21f-4e623e36fe30.gif) 6 | 7 | ## Features 8 | 9 | - easy to use api for transmission of fc variant, rc channels and armed state 10 | - simple drawing api 11 | 12 | support for setting up VTX parameters (MSP VTX) is planned. 13 | 14 | ## Example 15 | 16 | ```cpp 17 | #include 18 | 19 | #include 20 | 21 | HDZero hdzero; 22 | 23 | void setup() { 24 | // wait for serial to be ready 25 | Serial2.begin(115200); 26 | while (!Serial2) 27 | ; 28 | 29 | // setup and configure hdzero 30 | hdzero.begin(Serial2); 31 | hdzero.setFcVariant("INAV"); 32 | hdzero.setResolution(HD_5018); 33 | } 34 | 35 | uint32_t lastRefreshTime = 0; 36 | 37 | void loop() { 38 | if (millis() - lastRefreshTime >= 500) { 39 | hdzero.clear(); // clear the screen 40 | hdzero.writeString(0, 10, 10, "HELLO ARDUINO!"); // write some text 41 | hdzero.draw(); // draw everything out 42 | 43 | lastRefreshTime = millis(); 44 | } 45 | 46 | // updates the internal state, needs to be run every loop 47 | hdzero.run(); 48 | } 49 | ``` 50 | 51 | ## Debug Ouput 52 | 53 | You can enable debug output on `Serial` by configuring it in in `setup()` 54 | 55 | ```cpp 56 | void setup() { 57 | Serial.begin(115200); 58 | 59 | Serial2.begin(115200); 60 | hdzero.begin(Serial2); 61 | } 62 | ``` 63 | 64 | and defining `DEBUG_HDZERO` in your platfomio configuration 65 | 66 | ```ini 67 | [env:myenv] 68 | build_flags = 69 | -DDEBUG_HDZERO 70 | ``` 71 | -------------------------------------------------------------------------------- /src/hdzero.cpp: -------------------------------------------------------------------------------- 1 | #include "hdzero.h" 2 | 3 | #ifdef DEBUG_HDZERO 4 | #define debug_printf(...) Serial.printf(__VA_ARGS__) 5 | #else 6 | #define debug_printf(...) 7 | #endif 8 | 9 | HDZero::HDZero() 10 | : resolution(SD_3016) 11 | , isArmed(false) { 12 | setFcVariant("BTFL"); 13 | setAllRcChannels(0); 14 | } 15 | 16 | void HDZero::begin(Stream &_stream) { 17 | stream = &_stream; 18 | } 19 | 20 | void HDZero::end() { 21 | stream = nullptr; 22 | } 23 | 24 | void HDZero::setFcVariant(const char *variant) { 25 | int strLen = strlen(variant); 26 | for (uint8_t i = 0; i < min(MSP_FC_VARIANT_SIZE, strLen); i++) { 27 | fcVariant[i] = variant[i]; 28 | } 29 | } 30 | 31 | void HDZero::setRcChannels(uint16_t values[MSP_RC_CHANNEL_COUNT]) { 32 | memcpy(rcChannels, values, MSP_RC_CHANNEL_COUNT * sizeof(uint16_t)); 33 | } 34 | 35 | void HDZero::setRcChannel(uint8_t index, uint16_t value) { 36 | rcChannels[index] = value; 37 | } 38 | 39 | void HDZero::setAllRcChannels(uint16_t value) { 40 | for (uint8_t i = 0; i < 16; i++) { 41 | setRcChannel(i, value); 42 | } 43 | } 44 | 45 | void HDZero::setArmed(bool armed) { 46 | isArmed = armed; 47 | } 48 | 49 | void HDZero::setResolution(HDZeroResolution value) { 50 | resolution = value; 51 | 52 | uint8_t options[2] = { 53 | 0, 54 | value, 55 | }; 56 | sendSubCmd(MSP_DP_SUBCMD_CONFIG, options, 2); 57 | } 58 | 59 | void HDZero::write(uint8_t fontpage, uint8_t x, uint8_t y, uint8_t *data, uint8_t size) { 60 | uint8_t buf[size + 3]; 61 | buf[0] = y; 62 | buf[1] = x; 63 | buf[2] = fontpage; 64 | 65 | memcpy(buf + 3, data, size); 66 | 67 | sendSubCmd(MSP_DP_SUBCMD_WRITE, buf, size + 3); 68 | } 69 | 70 | void HDZero::writeString(uint8_t fontpage, uint8_t x, uint8_t y, const char *string) { 71 | write(fontpage, x, y, (uint8_t *)string, strlen(string)); 72 | } 73 | 74 | void HDZero::clear() { 75 | sendSubCmd(MSP_DP_SUBCMD_CLEAR, nullptr, 0); 76 | } 77 | 78 | void HDZero::draw() { 79 | sendSubCmd(MSP_DP_SUBCMD_DRAW, nullptr, 0); 80 | } 81 | 82 | void HDZero::send(uint8_t cmd, uint8_t direction, uint8_t *payload, uint8_t size) { 83 | if (stream == nullptr) { 84 | return; 85 | } 86 | 87 | stream->write('$'); 88 | stream->write('M'); 89 | stream->write(direction); 90 | stream->write(size); 91 | stream->write(cmd); 92 | 93 | uint8_t crc = (uint8_t)(size) ^ (uint8_t)(cmd); 94 | for (uint8_t i = 0; i < size; i++) { 95 | const uint8_t byte = payload[i]; 96 | stream->write(byte); 97 | crc ^= byte; 98 | } 99 | 100 | stream->write(crc); 101 | } 102 | 103 | void HDZero::sendResponse(uint8_t cmd, uint8_t *payload, uint8_t size) { 104 | send(cmd, '>', payload, size); 105 | } 106 | 107 | void HDZero::sendError(uint8_t cmd) { 108 | send(cmd, '!', nullptr, 0); 109 | } 110 | 111 | void HDZero::sendSubCmd(uint8_t cmd, uint8_t *data, uint8_t size) { 112 | uint8_t payload[size + 1]; 113 | payload[0] = cmd; 114 | memcpy(payload + 1, data, size); 115 | sendResponse(MSP_DISPLAYPORT, payload, size + 1); 116 | } 117 | 118 | void HDZero::handleMsp(uint8_t cmd, uint8_t *payload, uint8_t size) { 119 | switch (cmd) { 120 | case MSP_FC_VARIANT: 121 | sendResponse(cmd, fcVariant, MSP_FC_VARIANT_SIZE); 122 | break; 123 | 124 | case MSP_STATUS: { 125 | uint8_t data[22]; 126 | memset(data, 0, 22); 127 | 128 | uint32_t flight_mode = 0; 129 | if (isArmed) { 130 | flight_mode |= 0x1; 131 | } 132 | 133 | data[6] = flight_mode >> 0; 134 | data[7] = flight_mode >> 8; 135 | data[8] = flight_mode >> 16; 136 | data[9] = flight_mode >> 24; 137 | 138 | sendResponse(cmd, data, 22); 139 | break; 140 | } 141 | 142 | case MSP_RC: { 143 | sendResponse(cmd, (uint8_t *)rcChannels, MSP_RC_CHANNEL_COUNT * sizeof(uint16_t)); 144 | break; 145 | } 146 | 147 | case MSP_VTX_CONFIG: { 148 | // sendResponse(cmd, (uint8_t *)vtxConfig, sizeof(MSPVTXConfig)); 149 | break; 150 | } 151 | 152 | default: 153 | debug_printf("unhandled msp command %d (%d)\r\n", cmd, size); 154 | sendError(cmd); 155 | break; 156 | } 157 | } 158 | 159 | void HDZero::run() { 160 | if (stream == nullptr) { 161 | return; 162 | } 163 | 164 | while (true) { 165 | int magic = stream->read(); 166 | if (magic == -1) { 167 | return; 168 | } 169 | if (magic == '$') { 170 | break; 171 | } 172 | debug_printf("invalid magic 0x%x\r\n", magic); 173 | } 174 | 175 | int version = stream->read(); 176 | int direction = stream->read(); 177 | if (version != 'M' || direction != '<') { 178 | return; 179 | } 180 | 181 | int size = stream->read(); 182 | int cmd = stream->read(); 183 | if (size == -1 || cmd == -1) { 184 | return; 185 | } 186 | 187 | uint8_t payload[size]; 188 | uint8_t incoming_crc = (uint8_t)(size) ^ (uint8_t)(cmd); 189 | for (uint8_t i = 0; i < size; i++) { 190 | int byte = stream->read(); 191 | if (byte == -1) { 192 | return; 193 | } 194 | incoming_crc ^= (uint8_t)(byte); 195 | payload[i] = byte; 196 | } 197 | 198 | uint8_t crc = stream->read(); 199 | if (crc != incoming_crc) { 200 | debug_printf("invalid crc 0x%x vs 0x%x\r\n", crc, incoming_crc); 201 | return; 202 | } 203 | 204 | handleMsp(cmd, payload, size); 205 | } --------------------------------------------------------------------------------