├── MSP.cpp ├── MSP.h ├── MSP_OSD.h ├── OSD_positions_config.h ├── README.md └── arduino_DJI_03_RC_ARM.ino /MSP.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | MSP.cpp 3 | 4 | Copyright (c) 2017, Fabrizio Di Vittorio (fdivitto2013@gmail.com) 5 | 6 | This library is free software; you can redistribute it and/or 7 | modify it under the terms of the GNU Lesser General Public 8 | License as published by the Free Software Foundation; either 9 | version 2.1 of the License, or (at your option) any later version. 10 | 11 | This library is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public 17 | License along with this library; if not, write to the Free Software 18 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 19 | */ 20 | 21 | #include 22 | 23 | #include "MSP.h" 24 | 25 | 26 | void MSP::begin(Stream & stream, uint32_t timeout) 27 | { 28 | _stream = &stream; 29 | _timeout = timeout; 30 | } 31 | 32 | 33 | void MSP::reset() 34 | { 35 | _stream->flush(); 36 | while (_stream->available() > 0) 37 | _stream->read(); 38 | } 39 | 40 | void MSP::send(uint8_t messageID, void * payload, uint8_t size) 41 | { 42 | _stream->write('$'); 43 | _stream->write('M'); 44 | _stream->write('<'); 45 | _stream->write(size); 46 | _stream->write(messageID); 47 | uint8_t checksum = size ^ messageID; 48 | uint8_t * payloadPtr = (uint8_t*)payload; 49 | for (uint8_t i = 0; i < size; ++i) { 50 | uint8_t b = *(payloadPtr++); 51 | checksum ^= b; 52 | _stream->write(b); 53 | } 54 | _stream->write(checksum); 55 | } 56 | 57 | 58 | // timeout in milliseconds 59 | bool MSP::recv(uint8_t * messageID, void * payload, uint8_t maxSize, uint8_t * recvSize) 60 | { 61 | uint32_t t0 = millis(); 62 | 63 | while (1) { 64 | 65 | // read header 66 | while (_stream->available() < 6) 67 | if (millis() - t0 >= _timeout) 68 | return false; 69 | char header[3]; 70 | _stream->readBytes((char*)header, 3); 71 | 72 | // check header 73 | if (header[0] == '$' && header[1] == 'M' && (header[2] == '>' || header[2] == '<')) { 74 | 75 | // header ok, read payload size 76 | *recvSize = _stream->read(); 77 | 78 | // read message ID (type) 79 | *messageID = _stream->read(); 80 | 81 | uint8_t checksumCalc = *recvSize ^ *messageID; 82 | 83 | // read payload 84 | uint8_t * payloadPtr = (uint8_t*)payload; 85 | uint8_t idx = 0; 86 | while (idx < *recvSize) { 87 | if (millis() - t0 >= _timeout) 88 | return false; 89 | if (_stream->available() > 0) { 90 | uint8_t b = _stream->read(); 91 | checksumCalc ^= b; 92 | if (idx < maxSize) 93 | *(payloadPtr++) = b; 94 | ++idx; 95 | } 96 | } 97 | // zero remaining bytes if *size < maxSize 98 | for (; idx < maxSize; ++idx) 99 | *(payloadPtr++) = 0; 100 | 101 | // read and check checksum 102 | while (_stream->available() == 0) 103 | if (millis() - t0 >= _timeout) 104 | return false; 105 | uint8_t checksum = _stream->read(); 106 | if (checksumCalc == checksum) { 107 | return true; 108 | } 109 | 110 | } 111 | } 112 | 113 | } 114 | 115 | bool MSP::activityDetected() 116 | { 117 | uint32_t t0 = millis(); 118 | 119 | while (1) { 120 | 121 | // read header 122 | while (_stream->available() < 6) 123 | if (millis() - t0 >= _timeout) 124 | return false; 125 | 126 | return true; 127 | } 128 | 129 | } 130 | 131 | // wait for messageID 132 | // recvSize can be NULL 133 | bool MSP::waitFor(uint8_t messageID, void * payload, uint8_t maxSize, uint8_t * recvSize) 134 | { 135 | uint8_t recvMessageID; 136 | uint8_t recvSizeValue; 137 | uint32_t t0 = millis(); 138 | while (millis() - t0 < _timeout) 139 | if (recv(&recvMessageID, payload, maxSize, (recvSize ? recvSize : &recvSizeValue)) && messageID == recvMessageID) 140 | return true; 141 | 142 | // timeout 143 | return false; 144 | } 145 | 146 | 147 | // send a message and wait for the reply 148 | // recvSize can be NULL 149 | bool MSP::request(uint8_t messageID, void * payload, uint8_t maxSize, uint8_t * recvSize) 150 | { 151 | send(messageID, NULL, 0); 152 | return waitFor(messageID, payload, maxSize, recvSize); 153 | } 154 | 155 | 156 | // send message and wait for ack 157 | bool MSP::command(uint8_t messageID, void * payload, uint8_t size, bool waitACK) 158 | { 159 | send(messageID, payload, size); 160 | 161 | // ack required 162 | if (waitACK) 163 | return waitFor(messageID, NULL, 0); 164 | 165 | return true; 166 | } 167 | 168 | 169 | // map MSP_MODE_xxx to box ids 170 | // mixed values from cleanflight and inav 171 | static const uint8_t BOXIDS[30] PROGMEM = { 172 | 0, // 0: MSP_MODE_ARM 173 | 1, // 1: MSP_MODE_ANGLE 174 | 2, // 2: MSP_MODE_HORIZON 175 | 3, // 3: MSP_MODE_NAVALTHOLD (cleanflight BARO) 176 | 5, // 4: MSP_MODE_MAG 177 | 6, // 5: MSP_MODE_HEADFREE 178 | 7, // 6: MSP_MODE_HEADADJ 179 | 8, // 7: MSP_MODE_CAMSTAB 180 | 10, // 8: MSP_MODE_NAVRTH (cleanflight GPSHOME) 181 | 11, // 9: MSP_MODE_NAVPOSHOLD (cleanflight GPSHOLD) 182 | 12, // 10: MSP_MODE_PASSTHRU 183 | 13, // 11: MSP_MODE_BEEPERON 184 | 15, // 12: MSP_MODE_LEDLOW 185 | 16, // 13: MSP_MODE_LLIGHTS 186 | 19, // 14: MSP_MODE_OSD 187 | 20, // 15: MSP_MODE_TELEMETRY 188 | 21, // 16: MSP_MODE_GTUNE 189 | 22, // 17: MSP_MODE_SONAR 190 | 26, // 18: MSP_MODE_BLACKBOX 191 | 27, // 19: MSP_MODE_FAILSAFE 192 | 28, // 20: MSP_MODE_NAVWP (cleanflight AIRMODE) 193 | 29, // 21: MSP_MODE_AIRMODE (cleanflight DISABLE3DSWITCH) 194 | 30, // 22: MSP_MODE_HOMERESET (cleanflight FPVANGLEMIX) 195 | 31, // 23: MSP_MODE_GCSNAV (cleanflight BLACKBOXERASE) 196 | 32, // 24: MSP_MODE_HEADINGLOCK 197 | 33, // 25: MSP_MODE_SURFACE 198 | 34, // 26: MSP_MODE_FLAPERON 199 | 35, // 27: MSP_MODE_TURNASSIST 200 | 36, // 28: MSP_MODE_NAVLAUNCH 201 | 37, // 29: MSP_MODE_AUTOTRIM 202 | }; 203 | 204 | 205 | // returns active mode (using MSP_STATUS and MSP_BOXIDS messages) 206 | // see MSP_MODE_... for bits inside activeModes 207 | bool MSP::getActiveModes(uint32_t * activeModes) 208 | { 209 | // request status ex 210 | msp_status_t status; 211 | if (request(MSP_STATUS, &status, sizeof(status))) { 212 | // request permanent ids associated to boxes 213 | uint8_t ids[sizeof(BOXIDS)]; 214 | uint8_t recvSize; 215 | if (request(MSP_BOXIDS, ids, sizeof(ids), &recvSize)) { 216 | // compose activeModes, converting BOXIDS to bit map (setting 1 if related flag in flightModeFlags is set) 217 | *activeModes = 0; 218 | for (uint8_t i = 0; i < recvSize; ++i) { 219 | if (status.flightModeFlags & (1 << i)) { 220 | for (uint8_t j = 0; j < sizeof(BOXIDS); ++j) { 221 | if (pgm_read_byte(BOXIDS + j) == ids[i]) { 222 | *activeModes |= 1 << j; 223 | break; 224 | } 225 | } 226 | } 227 | } 228 | return true; 229 | } 230 | } 231 | 232 | return false; 233 | } 234 | 235 | 236 | -------------------------------------------------------------------------------- /MSP.h: -------------------------------------------------------------------------------- 1 | /* 2 | MSP.h 3 | 4 | Copyright (c) 2017, Fabrizio Di Vittorio (fdivitto2013@gmail.com) 5 | 6 | This library is free software; you can redistribute it and/or 7 | modify it under the terms of the GNU Lesser General Public 8 | License as published by the Free Software Foundation; either 9 | version 2.1 of the License, or (at your option) any later version. 10 | 11 | This library is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public 17 | License along with this library; if not, write to the Free Software 18 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 19 | */ 20 | 21 | 22 | #pragma once 23 | 24 | #include 25 | #include 26 | 27 | // requests & replies 28 | #define MSP_API_VERSION 1 29 | #define MSP_FC_VARIANT 2 30 | #define MSP_FC_VERSION 3 31 | #define MSP_BOARD_INFO 4 32 | #define MSP_BUILD_INFO 5 33 | #define MSP_CALIBRATION_DATA 14 34 | #define MSP_FEATURE 36 35 | #define MSP_BOARD_ALIGNMENT 38 36 | #define MSP_CURRENT_METER_CONFIG 40 37 | #define MSP_RX_CONFIG 44 38 | #define MSP_SONAR_ALTITUDE 58 39 | #define MSP_ARMING_CONFIG 61 40 | #define MSP_RX_MAP 64 // get channel map (also returns number of channels total) 41 | #define MSP_LOOP_TIME 73 // FC cycle time i.e looptime parameter 42 | #define MSP_STATUS 101 43 | #define MSP_RAW_IMU 102 44 | #define MSP_SERVO 103 45 | #define MSP_MOTOR 104 46 | #define MSP_RC 105 47 | #define MSP_RAW_GPS 106 48 | #define MSP_COMP_GPS 107 // distance home, direction home 49 | #define MSP_ATTITUDE 108 50 | #define MSP_ALTITUDE 109 51 | #define MSP_ANALOG 110 52 | #define MSP_RC_TUNING 111 // rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID 53 | #define MSP_PID 112 // P I D coeff 54 | #define MSP_MISC 114 55 | #define MSP_SERVO_CONFIGURATIONS 120 56 | #define MSP_NAV_STATUS 121 // navigation status 57 | #define MSP_SENSOR_ALIGNMENT 126 // orientation of acc,gyro,mag 58 | #define MSP_STATUS_EX 150 59 | #define MSP_SENSOR_STATUS 151 60 | #define MSP_BOXIDS 119 61 | #define MSP_UID 160 // Unique device ID 62 | #define MSP_GPSSVINFO 164 // get Signal Strength (only U-Blox) 63 | #define MSP_GPSSTATISTICS 166 // get GPS debugging data 64 | #define MSP_SET_PID 202 // set P I D coeff 65 | 66 | 67 | // commands 68 | #define MSP_SET_HEAD 211 // define a new heading hold direction 69 | #define MSP_SET_RAW_RC 200 // 8 rc chan 70 | #define MSP_SET_RAW_GPS 201 // fix, numsat, lat, lon, alt, speed 71 | #define MSP_SET_WP 209 // sets a given WP (WP#, lat, lon, alt, flags) 72 | 73 | 74 | // bits of getActiveModes() return value 75 | #define MSP_MODE_ARM 0 76 | #define MSP_MODE_ANGLE 1 77 | #define MSP_MODE_HORIZON 2 78 | #define MSP_MODE_NAVALTHOLD 3 /* cleanflight BARO */ 79 | #define MSP_MODE_MAG 4 80 | #define MSP_MODE_HEADFREE 5 81 | #define MSP_MODE_HEADADJ 6 82 | #define MSP_MODE_CAMSTAB 7 83 | #define MSP_MODE_NAVRTH 8 /* cleanflight GPSHOME */ 84 | #define MSP_MODE_NAVPOSHOLD 9 /* cleanflight GPSHOLD */ 85 | #define MSP_MODE_PASSTHRU 10 86 | #define MSP_MODE_BEEPERON 11 87 | #define MSP_MODE_LEDLOW 12 88 | #define MSP_MODE_LLIGHTS 13 89 | #define MSP_MODE_OSD 14 90 | #define MSP_MODE_TELEMETRY 15 91 | #define MSP_MODE_GTUNE 16 92 | #define MSP_MODE_SONAR 17 93 | #define MSP_MODE_BLACKBOX 18 94 | #define MSP_MODE_FAILSAFE 19 95 | #define MSP_MODE_NAVWP 20 /* cleanflight AIRMODE */ 96 | #define MSP_MODE_AIRMODE 21 /* cleanflight DISABLE3DSWITCH */ 97 | #define MSP_MODE_HOMERESET 22 /* cleanflight FPVANGLEMIX */ 98 | #define MSP_MODE_GCSNAV 23 /* cleanflight BLACKBOXERASE */ 99 | #define MSP_MODE_HEADINGLOCK 24 100 | #define MSP_MODE_SURFACE 25 101 | #define MSP_MODE_FLAPERON 26 102 | #define MSP_MODE_TURNASSIST 27 103 | #define MSP_MODE_NAVLAUNCH 28 104 | #define MSP_MODE_AUTOTRIM 29 105 | 106 | 107 | // MSP_API_VERSION reply 108 | struct msp_api_version_t { 109 | uint8_t protocolVersion; 110 | uint8_t APIMajor; 111 | uint8_t APIMinor; 112 | } __attribute__ ((packed)); 113 | 114 | 115 | // MSP_FC_VARIANT reply 116 | struct msp_fc_variant_t { 117 | char flightControlIdentifier[4]; 118 | } __attribute__ ((packed)); 119 | 120 | 121 | // MSP_FC_VERSION reply 122 | struct msp_fc_version_t { 123 | uint8_t versionMajor; 124 | uint8_t versionMinor; 125 | uint8_t versionPatchLevel; 126 | } __attribute__ ((packed)); 127 | 128 | 129 | // MSP_BOARD_INFO reply 130 | struct msp_board_info_t { 131 | char boardIdentifier[4]; 132 | uint16_t hardwareRevision; 133 | } __attribute__ ((packed)); 134 | 135 | 136 | // MSP_BUILD_INFO reply 137 | struct msp_build_info_t { 138 | char buildDate[11]; 139 | char buildTime[8]; 140 | char shortGitRevision[7]; 141 | } __attribute__ ((packed)); 142 | 143 | 144 | // MSP_RAW_IMU reply 145 | struct msp_raw_imu_t { 146 | int16_t acc[3]; // x, y, z 147 | int16_t gyro[3]; // x, y, z 148 | int16_t mag[3]; // x, y, z 149 | } __attribute__ ((packed)); 150 | 151 | 152 | // flags for msp_status_ex_t.sensor and msp_status_t.sensor 153 | #define MSP_STATUS_SENSOR_ACC 1 154 | #define MSP_STATUS_SENSOR_BARO 2 155 | #define MSP_STATUS_SENSOR_MAG 4 156 | #define MSP_STATUS_SENSOR_GPS 8 157 | #define MSP_STATUS_SENSOR_SONAR 16 158 | 159 | 160 | // MSP_STATUS_EX reply 161 | struct msp_status_ex_t { 162 | uint16_t cycleTime; 163 | uint16_t i2cErrorCounter; 164 | uint16_t sensor; // MSP_STATUS_SENSOR_... 165 | uint32_t flightModeFlags; // see getActiveModes() 166 | uint8_t configProfileIndex; 167 | uint16_t averageSystemLoadPercent; // 0...100 168 | uint16_t armingFlags; 169 | uint8_t accCalibrationAxisFlags; 170 | } __attribute__ ((packed)); 171 | 172 | 173 | // MSP_STATUS 174 | struct msp_status_t { 175 | uint16_t cycleTime; 176 | uint16_t i2cErrorCounter; 177 | uint16_t sensor; // MSP_STATUS_SENSOR_... 178 | uint32_t flightModeFlags; // see getActiveModes() 179 | uint8_t configProfileIndex; 180 | } __attribute__ ((packed)); 181 | 182 | 183 | // MSP_SENSOR_STATUS reply 184 | struct msp_sensor_status_t { 185 | uint8_t isHardwareHealthy; // 0...1 186 | uint8_t hwGyroStatus; 187 | uint8_t hwAccelerometerStatus; 188 | uint8_t hwCompassStatus; 189 | uint8_t hwBarometerStatus; 190 | uint8_t hwGPSStatus; 191 | uint8_t hwRangefinderStatus; 192 | uint8_t hwPitotmeterStatus; 193 | uint8_t hwOpticalFlowStatus; 194 | } __attribute__ ((packed)); 195 | 196 | 197 | #define MSP_MAX_SUPPORTED_SERVOS 8 198 | 199 | // MSP_SERVO reply 200 | struct msp_servo_t { 201 | uint16_t servo[MSP_MAX_SUPPORTED_SERVOS]; 202 | } __attribute__ ((packed)); 203 | 204 | 205 | // MSP_SERVO_CONFIGURATIONS reply 206 | struct msp_servo_configurations_t { 207 | __attribute__ ((packed)) struct { 208 | uint16_t min; 209 | uint16_t max; 210 | uint16_t middle; 211 | uint8_t rate; 212 | uint8_t angleAtMin; 213 | uint8_t angleAtMax; 214 | uint8_t forwardFromChannel; 215 | uint32_t reversedSources; 216 | } conf[MSP_MAX_SUPPORTED_SERVOS]; 217 | } __attribute__ ((packed)); 218 | 219 | 220 | #define MSP_MAX_SERVO_RULES (2 * MSP_MAX_SUPPORTED_SERVOS) 221 | 222 | // MSP_SERVO_MIX_RULES reply 223 | struct msp_servo_mix_rules_t { 224 | __attribute__ ((packed)) struct { 225 | uint8_t targetChannel; 226 | uint8_t inputSource; 227 | uint8_t rate; 228 | uint8_t speed; 229 | uint8_t min; 230 | uint8_t max; 231 | } mixRule[MSP_MAX_SERVO_RULES]; 232 | } __attribute__ ((packed)); 233 | 234 | 235 | #define MSP_MAX_SUPPORTED_MOTORS 8 236 | 237 | // MSP_MOTOR reply 238 | struct msp_motor_t { 239 | uint16_t motor[MSP_MAX_SUPPORTED_MOTORS]; 240 | } __attribute__ ((packed)); 241 | 242 | 243 | #define MSP_MAX_SUPPORTED_CHANNELS 16 244 | 245 | // MSP_RC reply 246 | struct msp_rc_t { 247 | uint16_t channelValue[MSP_MAX_SUPPORTED_CHANNELS]; 248 | } __attribute__ ((packed)); 249 | 250 | 251 | // MSP_ATTITUDE reply 252 | struct msp_attitude_t { 253 | int16_t roll; 254 | int16_t pitch; 255 | int16_t yaw; 256 | } __attribute__ ((packed)); 257 | 258 | 259 | // MSP_ALTITUDE reply 260 | struct msp_altitude_t { 261 | int32_t estimatedActualPosition; // cm 262 | int16_t estimatedActualVelocity; // cm/s 263 | int32_t baroLatestAltitude; 264 | } __attribute__ ((packed)); 265 | 266 | 267 | // MSP_SONAR_ALTITUDE reply 268 | struct msp_sonar_altitude_t { 269 | int32_t altitude; 270 | } __attribute__ ((packed)); 271 | 272 | 273 | // MSP_ANALOG reply 274 | struct msp_analog_t { 275 | uint8_t vbat; // 0...255 276 | uint16_t mAhDrawn; // milliamp hours drawn from battery 277 | uint16_t rssi; // 0..1023 278 | int16_t amperage; // send amperage in 0.01 A steps, range is -320A to 320A 279 | } __attribute__ ((packed)); 280 | 281 | 282 | // MSP_ARMING_CONFIG reply 283 | struct msp_arming_config_t { 284 | uint8_t auto_disarm_delay; 285 | uint8_t disarm_kill_switch; 286 | } __attribute__ ((packed)); 287 | 288 | 289 | // MSP_LOOP_TIME reply 290 | struct msp_loop_time_t { 291 | uint16_t looptime; 292 | } __attribute__ ((packed)); 293 | 294 | 295 | // MSP_RC_TUNING reply 296 | struct msp_rc_tuning_t { 297 | uint8_t rcRate8; // no longer used 298 | uint8_t rcExpo8; 299 | uint8_t rates[3]; // R,P,Y 300 | uint8_t dynThrPID; 301 | uint8_t thrMid8; 302 | uint8_t thrExpo8; 303 | uint16_t tpa_breakpoint; 304 | uint8_t rcYawExpo8; 305 | } __attribute__ ((packed)); 306 | 307 | 308 | // MSP_PID reply 309 | struct msp_pid_t { 310 | uint8_t roll[3]; // 0=P, 1=I, 2=D 311 | uint8_t pitch[3]; // 0=P, 1=I, 2=D 312 | uint8_t yaw[3]; // 0=P, 1=I, 2=D 313 | uint8_t pos_z[3]; // 0=P, 1=I, 2=D 314 | uint8_t pos_xy[3]; // 0=P, 1=I, 2=D 315 | uint8_t vel_xy[3]; // 0=P, 1=I, 2=D 316 | uint8_t surface[3]; // 0=P, 1=I, 2=D 317 | uint8_t level[3]; // 0=P, 1=I, 2=D 318 | uint8_t heading[3]; // 0=P, 1=I, 2=D 319 | uint8_t vel_z[3]; // 0=P, 1=I, 2=D 320 | } __attribute__ ((packed)); 321 | 322 | 323 | // MSP_MISC reply 324 | struct msp_misc_t { 325 | uint16_t midrc; 326 | uint16_t minthrottle; 327 | uint16_t maxthrottle; 328 | uint16_t mincommand; 329 | uint16_t failsafe_throttle; 330 | uint8_t gps_provider; 331 | uint8_t gps_baudrate; 332 | uint8_t gps_ubx_sbas; 333 | uint8_t multiwiiCurrentMeterOutput; 334 | uint8_t rssi_channel; 335 | uint8_t dummy; 336 | uint16_t mag_declination; 337 | uint8_t vbatscale; 338 | uint8_t vbatmincellvoltage; 339 | uint8_t vbatmaxcellvoltage; 340 | uint8_t vbatwarningcellvoltage; 341 | } __attribute__ ((packed)); 342 | 343 | 344 | // values for msp_raw_gps_t.fixType 345 | #define MSP_GPS_NO_FIX 0 346 | #define MSP_GPS_FIX_2D 1 347 | #define MSP_GPS_FIX_3D 2 348 | 349 | 350 | // MSP_RAW_GPS reply 351 | struct msp_raw_gps_t { 352 | uint8_t fixType; // MSP_GPS_NO_FIX, MSP_GPS_FIX_2D, MSP_GPS_FIX_3D 353 | uint8_t numSat; 354 | int32_t lat; // 1 / 10000000 deg 355 | int32_t lon; // 1 / 10000000 deg 356 | int16_t alt; // meters 357 | int16_t groundSpeed; // cm/s 358 | int16_t groundCourse; // unit: degree x 10 359 | uint16_t hdop; 360 | } __attribute__ ((packed)); 361 | 362 | 363 | // MSP_COMP_GPS reply 364 | struct msp_comp_gps_t { 365 | int16_t distanceToHome; // distance to home in meters 366 | int16_t directionToHome; // direction to home in degrees 367 | uint8_t heartbeat; // toggles 0 and 1 for each change 368 | } __attribute__ ((packed)); 369 | 370 | 371 | // values for msp_nav_status_t.mode 372 | #define MSP_NAV_STATUS_MODE_NONE 0 373 | #define MSP_NAV_STATUS_MODE_HOLD 1 374 | #define MSP_NAV_STATUS_MODE_RTH 2 375 | #define MSP_NAV_STATUS_MODE_NAV 3 376 | #define MSP_NAV_STATUS_MODE_EMERG 15 377 | 378 | // values for msp_nav_status_t.state 379 | #define MSP_NAV_STATUS_STATE_NONE 0 // None 380 | #define MSP_NAV_STATUS_STATE_RTH_START 1 // RTH Start 381 | #define MSP_NAV_STATUS_STATE_RTH_ENROUTE 2 // RTH Enroute 382 | #define MSP_NAV_STATUS_STATE_HOLD_INFINIT 3 // PosHold infinit 383 | #define MSP_NAV_STATUS_STATE_HOLD_TIMED 4 // PosHold timed 384 | #define MSP_NAV_STATUS_STATE_WP_ENROUTE 5 // WP Enroute 385 | #define MSP_NAV_STATUS_STATE_PROCESS_NEXT 6 // Process next 386 | #define MSP_NAV_STATUS_STATE_DO_JUMP 7 // Jump 387 | #define MSP_NAV_STATUS_STATE_LAND_START 8 // Start Land 388 | #define MSP_NAV_STATUS_STATE_LAND_IN_PROGRESS 9 // Land in Progress 389 | #define MSP_NAV_STATUS_STATE_LANDED 10 // Landed 390 | #define MSP_NAV_STATUS_STATE_LAND_SETTLE 11 // Settling before land 391 | #define MSP_NAV_STATUS_STATE_LAND_START_DESCENT 12 // Start descent 392 | 393 | // values for msp_nav_status_t.activeWpAction, msp_set_wp_t.action 394 | #define MSP_NAV_STATUS_WAYPOINT_ACTION_WAYPOINT 0x01 395 | #define MSP_NAV_STATUS_WAYPOINT_ACTION_RTH 0x04 396 | 397 | // values for msp_nav_status_t.error 398 | #define MSP_NAV_STATUS_ERROR_NONE 0 // All systems clear 399 | #define MSP_NAV_STATUS_ERROR_TOOFAR 1 // Next waypoint distance is more than safety distance 400 | #define MSP_NAV_STATUS_ERROR_SPOILED_GPS 2 // GPS reception is compromised - Nav paused - copter is adrift ! 401 | #define MSP_NAV_STATUS_ERROR_WP_CRC 3 // CRC error reading WP data from EEPROM - Nav stopped 402 | #define MSP_NAV_STATUS_ERROR_FINISH 4 // End flag detected, navigation finished 403 | #define MSP_NAV_STATUS_ERROR_TIMEWAIT 5 // Waiting for poshold timer 404 | #define MSP_NAV_STATUS_ERROR_INVALID_JUMP 6 // Invalid jump target detected, aborting 405 | #define MSP_NAV_STATUS_ERROR_INVALID_DATA 7 // Invalid mission step action code, aborting, copter is adrift 406 | #define MSP_NAV_STATUS_ERROR_WAIT_FOR_RTH_ALT 8 // Waiting to reach RTH Altitude 407 | #define MSP_NAV_STATUS_ERROR_GPS_FIX_LOST 9 // Gps fix lost, aborting mission 408 | #define MSP_NAV_STATUS_ERROR_DISARMED 10 // NAV engine disabled due disarm 409 | #define MSP_NAV_STATUS_ERROR_LANDING 11 // Landing 410 | 411 | 412 | // MSP_NAV_STATUS reply 413 | struct msp_nav_status_t { 414 | uint8_t mode; // one of MSP_NAV_STATUS_MODE_XXX 415 | uint8_t state; // one of MSP_NAV_STATUS_STATE_XXX 416 | uint8_t activeWpAction; // combination of MSP_NAV_STATUS_WAYPOINT_ACTION_XXX 417 | uint8_t activeWpNumber; 418 | uint8_t error; // one of MSP_NAV_STATUS_ERROR_XXX 419 | int16_t magHoldHeading; 420 | } __attribute__ ((packed)); 421 | 422 | 423 | // MSP_GPSSVINFO reply 424 | struct msp_gpssvinfo_t { 425 | uint8_t dummy1; 426 | uint8_t dummy2; 427 | uint8_t dummy3; 428 | uint8_t dummy4; 429 | uint8_t HDOP; 430 | } __attribute__ ((packed)); 431 | 432 | 433 | // MSP_GPSSTATISTICS reply 434 | struct msp_gpsstatistics_t { 435 | uint16_t lastMessageDt; 436 | uint32_t errors; 437 | uint32_t timeouts; 438 | uint32_t packetCount; 439 | uint16_t hdop; 440 | uint16_t eph; 441 | uint16_t epv; 442 | } __attribute__ ((packed)); 443 | 444 | 445 | // MSP_UID reply 446 | struct msp_uid_t { 447 | uint32_t uid0; 448 | uint32_t uid1; 449 | uint32_t uid2; 450 | } __attribute__ ((packed)); 451 | 452 | 453 | // MSP_FEATURE mask 454 | #define MSP_FEATURE_RX_PPM (1 << 0) 455 | #define MSP_FEATURE_VBAT (1 << 1) 456 | #define MSP_FEATURE_UNUSED_1 (1 << 2) 457 | #define MSP_FEATURE_RX_SERIAL (1 << 3) 458 | #define MSP_FEATURE_MOTOR_STOP (1 << 4) 459 | #define MSP_FEATURE_SERVO_TILT (1 << 5) 460 | #define MSP_FEATURE_SOFTSERIAL (1 << 6) 461 | #define MSP_FEATURE_GPS (1 << 7) 462 | #define MSP_FEATURE_UNUSED_3 (1 << 8) // was FEATURE_FAILSAFE 463 | #define MSP_FEATURE_UNUSED_4 (1 << 9) // was FEATURE_SONAR 464 | #define MSP_FEATURE_TELEMETRY (1 << 10) 465 | #define MSP_FEATURE_CURRENT_METER (1 << 11) 466 | #define MSP_FEATURE_3D (1 << 12) 467 | #define MSP_FEATURE_RX_PARALLEL_PWM (1 << 13) 468 | #define MSP_FEATURE_RX_MSP (1 << 14) 469 | #define MSP_FEATURE_RSSI_ADC (1 << 15) 470 | #define MSP_FEATURE_LED_STRIP (1 << 16) 471 | #define MSP_FEATURE_DASHBOARD (1 << 17) 472 | #define MSP_FEATURE_UNUSED_2 (1 << 18) 473 | #define MSP_FEATURE_BLACKBOX (1 << 19) 474 | #define MSP_FEATURE_CHANNEL_FORWARDING (1 << 20) 475 | #define MSP_FEATURE_TRANSPONDER (1 << 21) 476 | #define MSP_FEATURE_AIRMODE (1 << 22) 477 | #define MSP_FEATURE_SUPEREXPO_RATES (1 << 23) 478 | #define MSP_FEATURE_VTX (1 << 24) 479 | #define MSP_FEATURE_RX_SPI (1 << 25) 480 | #define MSP_FEATURE_SOFTSPI (1 << 26) 481 | #define MSP_FEATURE_PWM_SERVO_DRIVER (1 << 27) 482 | #define MSP_FEATURE_PWM_OUTPUT_ENABLE (1 << 28) 483 | #define MSP_FEATURE_OSD (1 << 29) 484 | 485 | 486 | // MSP_FEATURE reply 487 | struct msp_feature_t { 488 | uint32_t featureMask; // combination of MSP_FEATURE_XXX 489 | } __attribute__ ((packed)); 490 | 491 | 492 | // MSP_BOARD_ALIGNMENT reply 493 | struct msp_board_alignment_t { 494 | int16_t rollDeciDegrees; 495 | int16_t pitchDeciDegrees; 496 | int16_t yawDeciDegrees; 497 | } __attribute__ ((packed)); 498 | 499 | 500 | // values for msp_current_meter_config_t.currentMeterType 501 | #define MSP_CURRENT_SENSOR_NONE 0 502 | #define MSP_CURRENT_SENSOR_ADC 1 503 | #define MSP_CURRENT_SENSOR_VIRTUAL 2 504 | #define MSP_CURRENT_SENSOR_MAX CURRENT_SENSOR_VIRTUAL 505 | 506 | 507 | // MSP_CURRENT_METER_CONFIG reply 508 | struct msp_current_meter_config_t { 509 | int16_t currentMeterScale; 510 | int16_t currentMeterOffset; 511 | uint8_t currentMeterType; // MSP_CURRENT_SENSOR_XXX 512 | uint16_t batteryCapacity; 513 | } __attribute__ ((packed)); 514 | 515 | 516 | // msp_rx_config_t.serialrx_provider 517 | #define MSP_SERIALRX_SPEKTRUM1024 0 518 | #define MSP_SERIALRX_SPEKTRUM2048 1 519 | #define MSP_SERIALRX_SBUS 2 520 | #define MSP_SERIALRX_SUMD 3 521 | #define MSP_SERIALRX_SUMH 4 522 | #define MSP_SERIALRX_XBUS_MODE_B 5 523 | #define MSP_SERIALRX_XBUS_MODE_B_RJ01 6 524 | #define MSP_SERIALRX_IBUS 7 525 | #define MSP_SERIALRX_JETIEXBUS 8 526 | #define MSP_SERIALRX_CRSF 9 527 | 528 | 529 | // msp_rx_config_t.rx_spi_protocol values 530 | #define MSP_SPI_PROT_NRF24RX_V202_250K 0 531 | #define MSP_SPI_PROT_NRF24RX_V202_1M 1 532 | #define MSP_SPI_PROT_NRF24RX_SYMA_X 2 533 | #define MSP_SPI_PROT_NRF24RX_SYMA_X5C 3 534 | #define MSP_SPI_PROT_NRF24RX_CX10 4 535 | #define MSP_SPI_PROT_NRF24RX_CX10A 5 536 | #define MSP_SPI_PROT_NRF24RX_H8_3D 6 537 | #define MSP_SPI_PROT_NRF24RX_INAV 7 538 | 539 | 540 | // MSP_RX_CONFIG reply 541 | struct msp_rx_config_t { 542 | uint8_t serialrx_provider; // one of MSP_SERIALRX_XXX values 543 | uint16_t maxcheck; 544 | uint16_t midrc; 545 | uint16_t mincheck; 546 | uint8_t spektrum_sat_bind; 547 | uint16_t rx_min_usec; 548 | uint16_t rx_max_usec; 549 | uint8_t dummy1; 550 | uint8_t dummy2; 551 | uint16_t dummy3; 552 | uint8_t rx_spi_protocol; // one of MSP_SPI_PROT_XXX values 553 | uint32_t rx_spi_id; 554 | uint8_t rx_spi_rf_channel_count; 555 | } __attribute__ ((packed)); 556 | 557 | 558 | #define MSP_MAX_MAPPABLE_RX_INPUTS 8 559 | 560 | // MSP_RX_MAP reply 561 | struct msp_rx_map_t { 562 | uint8_t rxmap[MSP_MAX_MAPPABLE_RX_INPUTS]; // [0]=roll channel, [1]=pitch channel, [2]=yaw channel, [3]=throttle channel, [3+n]=aux n channel, etc... 563 | } __attribute__ ((packed)); 564 | 565 | 566 | // values for msp_sensor_alignment_t.gyro_align, acc_align, mag_align 567 | #define MSP_SENSOR_ALIGN_CW0_DEG 1 568 | #define MSP_SENSOR_ALIGN_CW90_DEG 2 569 | #define MSP_SENSOR_ALIGN_CW180_DEG 3 570 | #define MSP_SENSOR_ALIGN_CW270_DEG 4 571 | #define MSP_SENSOR_ALIGN_CW0_DEG_FLIP 5 572 | #define MSP_SENSOR_ALIGN_CW90_DEG_FLIP 6 573 | #define MSP_SENSOR_ALIGN_CW180_DEG_FLIP 7 574 | #define MSP_SENSOR_ALIGN_CW270_DEG_FLIP 8 575 | 576 | // MSP_SENSOR_ALIGNMENT reply 577 | struct msp_sensor_alignment_t { 578 | uint8_t gyro_align; // one of MSP_SENSOR_ALIGN_XXX 579 | uint8_t acc_align; // one of MSP_SENSOR_ALIGN_XXX 580 | uint8_t mag_align; // one of MSP_SENSOR_ALIGN_XXX 581 | } __attribute__ ((packed)); 582 | 583 | 584 | // MSP_CALIBRATION_DATA reply 585 | struct msp_calibration_data_t { 586 | int16_t accZeroX; 587 | int16_t accZeroY; 588 | int16_t accZeroZ; 589 | int16_t accGainX; 590 | int16_t accGainY; 591 | int16_t accGainZ; 592 | int16_t magZeroX; 593 | int16_t magZeroY; 594 | int16_t magZeroZ; 595 | } __attribute__ ((packed)); 596 | 597 | 598 | // MSP_SET_HEAD command 599 | struct msp_set_head_t { 600 | int16_t magHoldHeading; // degrees 601 | } __attribute__ ((packed)); 602 | 603 | 604 | // MSP_SET_RAW_RC command 605 | struct msp_set_raw_rc_t { 606 | uint16_t channel[MSP_MAX_SUPPORTED_CHANNELS]; 607 | } __attribute__ ((packed)); 608 | 609 | 610 | // MSP_SET_PID command 611 | typedef msp_pid_t msp_set_pid_t; 612 | 613 | 614 | // MSP_SET_RAW_GPS command 615 | struct msp_set_raw_gps_t { 616 | uint8_t fixType; // MSP_GPS_NO_FIX, MSP_GPS_FIX_2D, MSP_GPS_FIX_3D 617 | uint8_t numSat; 618 | int32_t lat; // 1 / 10000000 deg 619 | int32_t lon; // 1 / 10000000 deg 620 | int16_t alt; // meters 621 | int16_t groundSpeed; // cm/s 622 | } __attribute__ ((packed)); 623 | 624 | 625 | // MSP_SET_WP command 626 | // Special waypoints are 0 and 255. 0 is the RTH position, 255 is the POSHOLD position (lat, lon, alt). 627 | struct msp_set_wp_t { 628 | uint8_t waypointNumber; 629 | uint8_t action; // one of MSP_NAV_STATUS_WAYPOINT_ACTION_XXX 630 | int32_t lat; // decimal degrees latitude * 10000000 631 | int32_t lon; // decimal degrees longitude * 10000000 632 | int32_t alt; // altitude (cm) 633 | int16_t p1; // speed (cm/s) when action is MSP_NAV_STATUS_WAYPOINT_ACTION_WAYPOINT, or "land" (value 1) when action is MSP_NAV_STATUS_WAYPOINT_ACTION_RTH 634 | int16_t p2; // not used 635 | int16_t p3; // not used 636 | uint8_t flag; // 0xa5 = last, otherwise set to 0 637 | } __attribute__ ((packed)); 638 | 639 | 640 | 641 | 642 | ///////////////////////////////////////////////////////////////////////////////////////// 643 | ///////////////////////////////////////////////////////////////////////////////////////// 644 | 645 | class MSP { 646 | 647 | public: 648 | 649 | void begin(Stream & stream, uint32_t timeout = 500); 650 | 651 | // low level functions 652 | 653 | void send(uint8_t messageID, void * payload, uint8_t size); 654 | bool recv(uint8_t * messageID, void * payload, uint8_t maxSize, uint8_t * recvSize); 655 | 656 | bool activityDetected(); 657 | 658 | bool waitFor(uint8_t messageID, void * payload, uint8_t maxSize, uint8_t * recvSize = NULL); 659 | 660 | bool request(uint8_t messageID, void * payload, uint8_t maxSize, uint8_t * recvSize = NULL); 661 | 662 | bool command(uint8_t messageID, void * payload, uint8_t size, bool waitACK = true); 663 | 664 | void reset(); 665 | 666 | // high level functions 667 | 668 | bool getActiveModes(uint32_t * activeModes); 669 | 670 | 671 | private: 672 | 673 | Stream * _stream; 674 | uint32_t _timeout; 675 | 676 | }; 677 | -------------------------------------------------------------------------------- /MSP_OSD.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define MSP_OSD_CONFIG 84 //out message Get osd settings - betaflight 4 | #define MSP_NAME 10 5 | #define MSP_BATTERY_STATE 130 //out message Connected/Disconnected, Voltage, Current Used 6 | 7 | struct msp_osd_config_t { 8 | uint8_t osdflags; 9 | uint8_t video_system; 10 | uint8_t units; 11 | uint8_t rssi_alarm; 12 | uint16_t cap_alarm; 13 | uint8_t old_timer_alarm; 14 | uint8_t osd_item_count; //56 15 | uint16_t alt_alarm; 16 | uint16_t osd_rssi_value_pos; 17 | uint16_t osd_main_batt_voltage_pos; 18 | uint16_t osd_crosshairs_pos; 19 | uint16_t osd_artificial_horizon_pos; 20 | uint16_t osd_horizon_sidebars_pos; 21 | uint16_t osd_item_timer_1_pos; 22 | uint16_t osd_item_timer_2_pos; 23 | uint16_t osd_flymode_pos; 24 | uint16_t osd_craft_name_pos; 25 | uint16_t osd_throttle_pos_pos; 26 | uint16_t osd_vtx_channel_pos; 27 | uint16_t osd_current_draw_pos; 28 | uint16_t osd_mah_drawn_pos; 29 | uint16_t osd_gps_speed_pos; 30 | uint16_t osd_gps_sats_pos; 31 | uint16_t osd_altitude_pos; 32 | uint16_t osd_roll_pids_pos; 33 | uint16_t osd_pitch_pids_pos; 34 | uint16_t osd_yaw_pids_pos; 35 | uint16_t osd_power_pos; 36 | uint16_t osd_pidrate_profile_pos; 37 | uint16_t osd_warnings_pos; 38 | uint16_t osd_avg_cell_voltage_pos; 39 | uint16_t osd_gps_lon_pos; 40 | uint16_t osd_gps_lat_pos; 41 | uint16_t osd_debug_pos; 42 | uint16_t osd_pitch_angle_pos; 43 | uint16_t osd_roll_angle_pos; 44 | uint16_t osd_main_batt_usage_pos; 45 | uint16_t osd_disarmed_pos; 46 | uint16_t osd_home_dir_pos; 47 | uint16_t osd_home_dist_pos; 48 | uint16_t osd_numerical_heading_pos; 49 | uint16_t osd_numerical_vario_pos; 50 | uint16_t osd_compass_bar_pos; 51 | uint16_t osd_esc_tmp_pos; 52 | uint16_t osd_esc_rpm_pos; 53 | uint16_t osd_remaining_time_estimate_pos; 54 | uint16_t osd_rtc_datetime_pos; 55 | uint16_t osd_adjustment_range_pos; 56 | uint16_t osd_core_temperature_pos; 57 | uint16_t osd_anti_gravity_pos; 58 | uint16_t osd_g_force_pos; 59 | uint16_t osd_motor_diag_pos; 60 | uint16_t osd_log_status_pos; 61 | uint16_t osd_flip_arrow_pos; 62 | uint16_t osd_link_quality_pos; 63 | uint16_t osd_flight_dist_pos; 64 | uint16_t osd_stick_overlay_left_pos; 65 | uint16_t osd_stick_overlay_right_pos; 66 | uint16_t osd_display_name_pos; 67 | uint16_t osd_esc_rpm_freq_pos; 68 | uint16_t osd_rate_profile_name_pos; 69 | uint16_t osd_pid_profile_name_pos; 70 | uint16_t osd_profile_name_pos; 71 | uint16_t osd_rssi_dbm_value_pos; 72 | uint16_t osd_rc_channels_pos; 73 | uint8_t osd_stat_count; //24 74 | uint8_t osd_stat_rtc_date_time; 75 | uint8_t osd_stat_timer_1; 76 | uint8_t osd_stat_timer_2; 77 | uint8_t osd_stat_max_speed; 78 | uint8_t osd_stat_max_distance; 79 | uint8_t osd_stat_min_battery; 80 | uint8_t osd_stat_end_battery; 81 | uint8_t osd_stat_battery; 82 | uint8_t osd_stat_min_rssi; 83 | uint8_t osd_stat_max_current; 84 | uint8_t osd_stat_used_mah; 85 | uint8_t osd_stat_max_altitude; 86 | uint8_t osd_stat_blackbox; 87 | uint8_t osd_stat_blackbox_number; 88 | uint8_t osd_stat_max_g_force; 89 | uint8_t osd_stat_max_esc_temp; 90 | uint8_t osd_stat_max_esc_rpm; 91 | uint8_t osd_stat_min_link_quality; 92 | uint8_t osd_stat_flight_distance; 93 | uint8_t osd_stat_max_fft; 94 | uint8_t osd_stat_total_flights; 95 | uint8_t osd_stat_total_time; 96 | uint8_t osd_stat_total_dist; 97 | uint8_t osd_stat_min_rssi_dbm; 98 | uint16_t osd_timer_count; 99 | uint16_t osd_timer_1; 100 | uint16_t osd_timer_2; 101 | uint16_t enabledwarnings; 102 | uint8_t osd_warning_count; // 16 103 | uint32_t enabledwarnings_1_41_plus; 104 | uint8_t osd_profile_count; // 1 105 | uint8_t osdprofileindex; // 1 106 | uint8_t overlay_radio_mode; // 0 107 | } __attribute__ ((packed)); 108 | 109 | struct msp_name_t { 110 | char craft_name[15]; //15 characters max possible displayed in the goggles 111 | } __attribute__ ((packed)); 112 | 113 | struct msp_battery_state_t { 114 | uint8_t batteryCellCount; 115 | uint16_t batteryCapacity; 116 | uint8_t legacyBatteryVoltage; 117 | uint16_t mAhDrawn; 118 | uint16_t amperage; 119 | uint8_t batteryState; 120 | uint16_t batteryVoltage; 121 | } __attribute__ ((packed)); 122 | 123 | // MSP_STATUS_DJI reply 124 | struct msp_status_DJI_t { 125 | uint16_t cycleTime; 126 | uint16_t i2cErrorCounter; 127 | uint16_t sensor; // MSP_STATUS_SENSOR_... 128 | uint32_t flightModeFlags; // see getActiveModes() 129 | uint8_t configProfileIndex; 130 | uint16_t averageSystemLoadPercent; // 0...100 131 | uint16_t armingFlags; //0x0103 or 0x0301 132 | uint8_t accCalibrationAxisFlags; //0 133 | uint8_t DJI_ARMING_DISABLE_FLAGS_COUNT; //25 134 | uint32_t djiPackArmingDisabledFlags; //(1 << 24) 135 | } __attribute__ ((packed)); -------------------------------------------------------------------------------- /OSD_positions_config.h: -------------------------------------------------------------------------------- 1 | //OSD elements positions 2 | //in betaflight configurator set OSD elements to your desired positions and in CLI type "set osd" to retreieve the numbers. 3 | //234 -> not visible. Horizontally 2048-2074(spacing 1), vertically 2048-2528(spacing 32). 26 characters X 15 lines 4 | 5 | #define SAMD21 6 | //#define DEBUG // uncomment this line to debug in Serial Monitor 7 | 8 | #include 9 | 10 | //currently working elements 11 | const uint16_t osd_avg_cell_voltage_pos = 2273; 12 | const uint16_t osd_main_batt_voltage_pos = 2241; 13 | const uint16_t osd_crosshairs_pos = 2254; 14 | const uint16_t osd_craft_name_pos = 2208; 15 | 16 | const uint16_t osd_altitude_pos = 234; 17 | uint16_t osd_gps_sats_pos = 234; 18 | const uint16_t osd_home_dir_pos = 234; 19 | const uint16_t osd_home_dist_pos = 234; 20 | const uint16_t osd_gps_speed_pos = 234; 21 | const uint16_t osd_gps_lat_pos = 234; 22 | const uint16_t osd_gps_lon_pos = 234; 23 | 24 | const uint16_t osd_pitch_angle_pos = 234; 25 | const uint16_t osd_roll_angle_pos = 234; 26 | const uint16_t osd_rssi_value_pos = 234; 27 | const uint16_t osd_display_name_pos = 234; 28 | const uint16_t osd_flymode_pos = 234; 29 | const uint16_t osd_current_draw_pos = 234; 30 | const uint16_t osd_mah_drawn_pos = 234; 31 | const uint16_t osd_numerical_vario_pos = 234; 32 | 33 | //not implemented or not available 34 | const uint16_t osd_throttle_pos_pos = 234; 35 | const uint16_t osd_vtx_channel_pos = 234; 36 | const uint16_t osd_roll_pids_pos = 234; 37 | const uint16_t osd_pitch_pids_pos = 234; 38 | const uint16_t osd_yaw_pids_pos = 234; 39 | const uint16_t osd_power_pos = 234; 40 | const uint16_t osd_pidrate_profile_pos = 234; 41 | const uint16_t osd_warnings_pos = 234; 42 | const uint16_t osd_debug_pos = 234; 43 | const uint16_t osd_artificial_horizon_pos = 234; 44 | const uint16_t osd_horizon_sidebars_pos = 234; 45 | const uint16_t osd_item_timer_1_pos = 234; 46 | const uint16_t osd_item_timer_2_pos = 234; 47 | const uint16_t osd_main_batt_usage_pos = 234; 48 | const uint16_t osd_disarmed_pos = 234; 49 | const uint16_t osd_numerical_heading_pos = 234; 50 | const uint16_t osd_compass_bar_pos = 234; 51 | const uint16_t osd_esc_tmp_pos = 234; 52 | const uint16_t osd_esc_rpm_pos = 234; 53 | const uint16_t osd_remaining_time_estimate_pos = 234; 54 | const uint16_t osd_rtc_datetime_pos = 234; 55 | const uint16_t osd_adjustment_range_pos = 234; 56 | const uint16_t osd_core_temperature_pos = 234; 57 | const uint16_t osd_anti_gravity_pos = 234; 58 | const uint16_t osd_g_force_pos = 234; 59 | const uint16_t osd_motor_diag_pos = 234; 60 | const uint16_t osd_log_status_pos = 234; 61 | const uint16_t osd_flip_arrow_pos = 234; 62 | const uint16_t osd_link_quality_pos = 234; 63 | const uint16_t osd_flight_dist_pos = 234; 64 | const uint16_t osd_stick_overlay_left_pos = 234; 65 | const uint16_t osd_stick_overlay_right_pos = 234; 66 | const uint16_t osd_esc_rpm_freq_pos = 234; 67 | const uint16_t osd_rate_profile_name_pos = 234; 68 | const uint16_t osd_pid_profile_name_pos = 234; 69 | const uint16_t osd_profile_name_pos = 234; 70 | const uint16_t osd_rssi_dbm_value_pos = 234; 71 | const uint16_t osd_rc_channels_pos = 234; -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Arduino DJI 03 RC ARM V2 2 | 3 | This project allows an Arduino XIAO Seeeduino to arm a DJI 03 Air Unit automatically once powered up, and as long as the Goggles are powered up. The intent of which is to allow the full power of the DJI 03 Air Unit to activate without having to use a flight controller. 4 | 5 | Note that this project no longer uses an RC channel to arm, although that feature may come at a later time. 6 | 7 | **Instructions:** 8 | 1. Complete the wiring per below. 9 | 2. Download this entire repository (click on green Code button and then Download Zip). 10 | 3. Extract all files into a folder called "arduino_DJI_03_RC_ARM" exactly that name, no "-main" or anything else. 11 | 4. Open the arduino_DJI_03_RC_ARM.ino file in Arduino IDE. 12 | 5. Install the Seeeduino Board and choose/downgrade to board version 1.7.9. They changed something with SerialUSB that breaks the code after that, I will fix in a future release. 13 | 6. [Upload the arduino_DJI_03_RC_ARM.ino sketch to the Arduino](https://support.arduino.cc/hc/en-us/articles/4733418441116-Upload-a-sketch-in-Arduino-IDE). 14 | 7. Power on your goggles. 15 | 8. Power on the aircraft. 16 | 9. Two seconds after the Air Unit has powered up, it will arm. 17 | 18 | **Caution:** 19 | Please make sure that no more than 5v regulated is being supplied to the Arduino XIAO Seeeduino. The below wiring diagram assumes the receiver is powered from a 5v BEC/UBEC. If the receiver is powered by a different or unregulated voltage then you will need to provide a 5v UBEC between the main battery and the Arduino and remove the connection to the receiver. 20 | 21 | **Voltage reading (optional):** 22 | Add a voltage divider between supply and ground, highest resistance value towards positive (R1), middle going to A0. 23 | Choose resistors in the tens of kiloohms range, with a ratio appropriate for the intended supply voltage. 24 | e.g. R1=33k, R2=5.1k, ratio of 6.471, measurement range 21.3V (3.3V Arduino reference * 6.471) 25 | Set VOLT_DIVIDER to 1024/measurement range e.g. 1024/21.3 = 48, then adjust as necessary if not precise enough. 26 | 27 | ![Arduino DJI 03 RC ARM Wiring](https://i.imgur.com/88jpdS7.png) 28 | -------------------------------------------------------------------------------- /arduino_DJI_03_RC_ARM.ino: -------------------------------------------------------------------------------- 1 | /************************************************************************************ 2 | @file arduino_DJI_03_RC_ARM.ino 3 | @brief Send DJI Arm using XIAO Seeeduino over MSP to keep power level at full. 4 | @author Richard Amiss 5 | 6 | Release Note: 7 | Complete rework for XIAO Seeeduino and MSP libraries. RC input is no longer used in 8 | this version. The AU will simply arm a few seconds after being turned on, as long 9 | as the goggles are on. 10 | 11 | Code: Richard Amiss 12 | Version: 1.1.0 13 | Date: 06/24/23 14 | 15 | Credits: 16 | This software is based on and uses software published by 2022 David Payne: 17 | QLiteOSD, which is based on work by Paul Kurucz (pkuruz):opentelem_to_bst_bridge 18 | as well as software from d3ngit : djihdfpv_mavlink_to_msp_V2 and 19 | crashsalot : VOT_to_DJIFPV 20 | 21 | THIS SOFTWARE IS PROVIDED IN AN "AS IS" CONDITION. NO WARRANTIES, WHETHER EXPRESS, 22 | IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF 23 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. THE 24 | COMPANY SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL OR 25 | CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER. 26 | ************************************************************************************/ 27 | 28 | #include "MSP.h" 29 | #include "MSP_OSD.h" 30 | #include "OSD_positions_config.h" 31 | 32 | #define ANALOG_IN A0 // Voltage Read pin (notice this is now Pin 0, instead of Pin 1) 33 | #define VOLT_DIVIDER 48 // Set to 1024/full scale voltage 34 | //#define DEBUG //uncomment to see diagnostics from USB serial 35 | 36 | #define FC_FIRMWARE_NAME "Betaflight" 37 | #define FC_FIRMWARE_IDENTIFIER "BTFL" 38 | 39 | HardwareSerial &mspSerial = Serial1; 40 | MSP msp; 41 | 42 | // Arm Logic 43 | uint32_t unarmedMillis = 3000; // number of milliseconds to wait before arming, after AU is initialized. Recommend at least 3000 (3 seconds). 44 | 45 | //Voltage and Battery Reading 46 | float averageVoltage = 0; 47 | int sampleVoltageCount = 0; 48 | int lastCount = 0; 49 | 50 | boolean lightOn = true; 51 | 52 | uint8_t *messageID; 53 | void *payload; 54 | uint8_t maxSize; 55 | uint8_t *recvSize; 56 | 57 | //Other 58 | char fcVariant[5] = "BTFL"; 59 | char craftname[15] = "Craft"; 60 | uint32_t previousMillis_MSP = 0; 61 | uint32_t activityDetectedMillis_MSP = 0; 62 | bool activityDetected = false; 63 | const uint32_t next_interval_MSP = 100; 64 | uint32_t general_counter = next_interval_MSP; 65 | uint32_t custom_mode = 0; //flight mode 66 | uint8_t vbat = 0; 67 | uint32_t flightModeFlags = 0x00000002; 68 | uint8_t batteryCellCount = 3; 69 | uint32_t previousFlightMode = custom_mode; 70 | 71 | msp_battery_state_t battery_state = { 0 }; 72 | msp_name_t name = { 0 }; 73 | msp_fc_version_t fc_version = { 0 }; 74 | msp_fc_variant_t fc_variant = { 0 }; 75 | //msp_status_BF_t status_BF = {0}; 76 | msp_status_DJI_t status_DJI = { 0 }; 77 | msp_analog_t analog = { 0 }; 78 | 79 | void setup() { 80 | #ifdef DEBUG 81 | SerialUSB.begin(115200); 82 | //while (!SerialUSB); 83 | #endif 84 | Serial1.begin(115200); 85 | while (!Serial1); 86 | 87 | analogReadResolution(12); // SAMD21 12 bit resolution 0 - 4096 range on Analog pin 88 | 89 | msp.begin(mspSerial); 90 | pinMode(LED_BUILTIN, OUTPUT); 91 | 92 | delay(1000); 93 | 94 | status_DJI.cycleTime = 0x0080; 95 | status_DJI.i2cErrorCounter = 0; 96 | status_DJI.sensor = 0x23; 97 | status_DJI.configProfileIndex = 0; 98 | status_DJI.averageSystemLoadPercent = 7; 99 | status_DJI.accCalibrationAxisFlags = 0; 100 | status_DJI.DJI_ARMING_DISABLE_FLAGS_COUNT = 20; 101 | status_DJI.djiPackArmingDisabledFlags = (1 << 24); 102 | flightModeFlags = 0x00000002; 103 | 104 | #ifdef DEBUG 105 | Serial.println("Initialized"); 106 | #endif 107 | } 108 | 109 | void loop() { 110 | 111 | 112 | if (!activityDetected) { 113 | #ifdef DEBUG 114 | Serial.println("Waiting for AU..."); 115 | #endif 116 | digitalWrite(LED_BUILTIN, LOW); 117 | 118 | // Wait for Air Unit to send data 119 | while(!msp.activityDetected()) { 120 | }; 121 | activityDetected = true; 122 | activityDetectedMillis_MSP = millis(); 123 | #ifdef DEBUG 124 | Serial.println("AU Detected, waiting (unarmedMillis) time till arm"); 125 | #endif 126 | } 127 | 128 | digitalWrite(LED_BUILTIN, HIGH); 129 | 130 | uint32_t currentMillis_MSP = millis(); 131 | 132 | if ((uint32_t)(currentMillis_MSP - previousMillis_MSP) >= next_interval_MSP) { 133 | previousMillis_MSP = currentMillis_MSP; 134 | 135 | if (general_counter % 300 == 0) { // update the altitude and voltage values every 300ms 136 | getVoltageSample(); 137 | if (lightOn) { 138 | digitalWrite(LED_BUILTIN, LOW); 139 | } else { 140 | digitalWrite(LED_BUILTIN, HIGH); 141 | } 142 | lightOn = !lightOn; 143 | } 144 | 145 | if (currentMillis_MSP < (activityDetectedMillis_MSP + unarmedMillis)) { 146 | set_flight_mode_flags(false); 147 | } else { 148 | set_flight_mode_flags(true); 149 | } 150 | 151 | #ifdef DEBUG 152 | //debugPrint(); 153 | #endif 154 | send_msp_to_airunit(); 155 | general_counter += next_interval_MSP; 156 | } 157 | if (custom_mode != previousFlightMode) { 158 | previousFlightMode = custom_mode; 159 | display_flight_mode(); 160 | } 161 | 162 | if (batteryCellCount == 0 && vbat > 0) { 163 | set_battery_cells_number(); 164 | } 165 | 166 | //display flight mode every 10s 167 | if (general_counter % 10000 == 0) { 168 | display_flight_mode(); 169 | } 170 | } 171 | 172 | msp_osd_config_t msp_osd_config = { 0 }; 173 | 174 | void send_osd_config() { 175 | 176 | #ifdef IMPERIAL_UNITS 177 | msp_osd_config.units = 0; 178 | #else 179 | msp_osd_config.units = 1; 180 | #endif 181 | 182 | msp_osd_config.osd_item_count = 56; 183 | msp_osd_config.osd_stat_count = 24; 184 | msp_osd_config.osd_timer_count = 2; 185 | msp_osd_config.osd_warning_count = 16; // 16 186 | msp_osd_config.osd_profile_count = 1; // 1 187 | msp_osd_config.osdprofileindex = 1; // 1 188 | msp_osd_config.overlay_radio_mode = 0; // 0 189 | 190 | msp_osd_config.osd_rssi_value_pos = osd_rssi_value_pos; 191 | msp_osd_config.osd_main_batt_voltage_pos = osd_main_batt_voltage_pos; 192 | msp_osd_config.osd_crosshairs_pos = osd_crosshairs_pos; 193 | msp_osd_config.osd_artificial_horizon_pos = osd_artificial_horizon_pos; 194 | msp_osd_config.osd_horizon_sidebars_pos = osd_horizon_sidebars_pos; 195 | msp_osd_config.osd_item_timer_1_pos = osd_item_timer_1_pos; 196 | msp_osd_config.osd_item_timer_2_pos = osd_item_timer_2_pos; 197 | msp_osd_config.osd_flymode_pos = osd_flymode_pos; 198 | msp_osd_config.osd_craft_name_pos = osd_craft_name_pos; 199 | msp_osd_config.osd_throttle_pos_pos = osd_throttle_pos_pos; 200 | msp_osd_config.osd_vtx_channel_pos = osd_vtx_channel_pos; 201 | msp_osd_config.osd_current_draw_pos = osd_current_draw_pos; 202 | msp_osd_config.osd_mah_drawn_pos = osd_mah_drawn_pos; 203 | msp_osd_config.osd_gps_speed_pos = osd_gps_speed_pos; 204 | msp_osd_config.osd_gps_sats_pos = osd_gps_sats_pos; 205 | msp_osd_config.osd_altitude_pos = osd_altitude_pos; 206 | msp_osd_config.osd_roll_pids_pos = osd_roll_pids_pos; 207 | msp_osd_config.osd_pitch_pids_pos = osd_pitch_pids_pos; 208 | msp_osd_config.osd_yaw_pids_pos = osd_yaw_pids_pos; 209 | msp_osd_config.osd_power_pos = osd_power_pos; 210 | msp_osd_config.osd_pidrate_profile_pos = osd_pidrate_profile_pos; 211 | msp_osd_config.osd_warnings_pos = osd_warnings_pos; 212 | msp_osd_config.osd_avg_cell_voltage_pos = osd_avg_cell_voltage_pos; 213 | msp_osd_config.osd_gps_lon_pos = osd_gps_lon_pos; 214 | msp_osd_config.osd_gps_lat_pos = osd_gps_lat_pos; 215 | msp_osd_config.osd_debug_pos = osd_debug_pos; 216 | msp_osd_config.osd_pitch_angle_pos = osd_pitch_angle_pos; 217 | msp_osd_config.osd_roll_angle_pos = osd_roll_angle_pos; 218 | msp_osd_config.osd_main_batt_usage_pos = osd_main_batt_usage_pos; 219 | msp_osd_config.osd_disarmed_pos = osd_disarmed_pos; 220 | msp_osd_config.osd_home_dir_pos = osd_home_dir_pos; 221 | msp_osd_config.osd_home_dist_pos = osd_home_dist_pos; 222 | msp_osd_config.osd_numerical_heading_pos = osd_numerical_heading_pos; 223 | msp_osd_config.osd_numerical_vario_pos = osd_numerical_vario_pos; 224 | msp_osd_config.osd_compass_bar_pos = osd_compass_bar_pos; 225 | msp_osd_config.osd_esc_tmp_pos = osd_esc_tmp_pos; 226 | msp_osd_config.osd_esc_rpm_pos = osd_esc_rpm_pos; 227 | msp_osd_config.osd_remaining_time_estimate_pos = osd_remaining_time_estimate_pos; 228 | msp_osd_config.osd_rtc_datetime_pos = osd_rtc_datetime_pos; 229 | msp_osd_config.osd_adjustment_range_pos = osd_adjustment_range_pos; 230 | msp_osd_config.osd_core_temperature_pos = osd_core_temperature_pos; 231 | msp_osd_config.osd_anti_gravity_pos = osd_anti_gravity_pos; 232 | msp_osd_config.osd_g_force_pos = osd_g_force_pos; 233 | msp_osd_config.osd_motor_diag_pos = osd_motor_diag_pos; 234 | msp_osd_config.osd_log_status_pos = osd_log_status_pos; 235 | msp_osd_config.osd_flip_arrow_pos = osd_flip_arrow_pos; 236 | msp_osd_config.osd_link_quality_pos = osd_link_quality_pos; 237 | msp_osd_config.osd_flight_dist_pos = osd_flight_dist_pos; 238 | msp_osd_config.osd_stick_overlay_left_pos = osd_stick_overlay_left_pos; 239 | msp_osd_config.osd_stick_overlay_right_pos = osd_stick_overlay_right_pos; 240 | msp_osd_config.osd_display_name_pos = osd_display_name_pos; 241 | msp_osd_config.osd_esc_rpm_freq_pos = osd_esc_rpm_freq_pos; 242 | msp_osd_config.osd_rate_profile_name_pos = osd_rate_profile_name_pos; 243 | msp_osd_config.osd_pid_profile_name_pos = osd_pid_profile_name_pos; 244 | msp_osd_config.osd_profile_name_pos = osd_profile_name_pos; 245 | msp_osd_config.osd_rssi_dbm_value_pos = osd_rssi_dbm_value_pos; 246 | msp_osd_config.osd_rc_channels_pos = osd_rc_channels_pos; 247 | 248 | msp.send(MSP_OSD_CONFIG, &msp_osd_config, sizeof(msp_osd_config)); 249 | } 250 | 251 | 252 | void invert_pos(uint16_t *pos1, uint16_t *pos2) { 253 | uint16_t tmp_pos = *pos1; 254 | *pos1 = *pos2; 255 | *pos2 = tmp_pos; 256 | } 257 | 258 | void set_flight_mode_flags(bool arm) { 259 | if ((flightModeFlags == 0x00000002) && arm) { 260 | flightModeFlags = 0x00000003; // arm 261 | #ifdef DEBUG 262 | Serial.println("ARMING"); 263 | #endif 264 | } else if ((flightModeFlags == 0x00000003) && !arm) { 265 | flightModeFlags = 0x00000002; // disarm 266 | #ifdef DEBUG 267 | Serial.println("DISARMING"); 268 | #endif 269 | } 270 | } 271 | 272 | void display_flight_mode() { 273 | show_text(&craftname); 274 | } 275 | 276 | void send_msp_to_airunit() { 277 | 278 | //MSP_FC_VARIANT 279 | memcpy(fc_variant.flightControlIdentifier, fcVariant, sizeof(fcVariant)); 280 | msp.send(MSP_FC_VARIANT, &fc_variant, sizeof(fc_variant)); 281 | 282 | //MSP_FC_VERSION 283 | fc_version.versionMajor = 4; 284 | fc_version.versionMinor = 1; 285 | fc_version.versionPatchLevel = 1; 286 | msp.send(MSP_FC_VERSION, &fc_version, sizeof(fc_version)); 287 | 288 | //MSP_NAME 289 | memcpy(name.craft_name, craftname, sizeof(craftname)); 290 | msp.send(MSP_NAME, &name, sizeof(name)); 291 | 292 | //MSP_STATUS 293 | status_DJI.flightModeFlags = flightModeFlags; 294 | status_DJI.armingFlags = 0x0303; 295 | msp.send(MSP_STATUS_EX, &status_DJI, sizeof(status_DJI)); 296 | status_DJI.armingFlags = 0x0000; 297 | msp.send(MSP_STATUS, &status_DJI, sizeof(status_DJI)); 298 | 299 | //MSP_BATTERY_STATE 300 | battery_state.amperage = 0; 301 | battery_state.batteryVoltage = vbat * 10; 302 | battery_state.mAhDrawn = 0; 303 | battery_state.batteryCellCount = batteryCellCount; 304 | battery_state.batteryCapacity = 0; 305 | battery_state.batteryState = 0; 306 | battery_state.legacyBatteryVoltage = vbat; 307 | msp.send(MSP_BATTERY_STATE, &battery_state, sizeof(battery_state)); 308 | 309 | //MSP_OSD_CONFIG 310 | send_osd_config(); 311 | } 312 | 313 | void show_text(char (*text)[15]) { 314 | memcpy(craftname, *text, sizeof(craftname)); 315 | } 316 | 317 | void set_battery_cells_number() { 318 | if (vbat < 43) batteryCellCount = 1; 319 | else if (vbat < 85) batteryCellCount = 2; 320 | else if (vbat < 127) batteryCellCount = 3; 321 | else if (vbat < 169) batteryCellCount = 4; 322 | else if (vbat < 211) batteryCellCount = 5; 323 | else if (vbat < 255) batteryCellCount = 6; 324 | } 325 | 326 | 327 | 328 | void getVoltageSample() { 329 | vbat = analogRead(ANALOG_IN)*10/VOLT_DIVIDER; 330 | } 331 | 332 | //*** USED ONLY FOR DEBUG *** 333 | void debugPrint() { 334 | SerialUSB.println("**********************************"); 335 | SerialUSB.print("Flight Mode: "); 336 | SerialUSB.println(flightModeFlags); 337 | SerialUSB.print("Voltage: "); 338 | SerialUSB.println(((double)vbat / 10), 1); 339 | SerialUSB.print("Sample Count / transmit: "); 340 | SerialUSB.println(lastCount); 341 | SerialUSB.print("Battery Cell Count: "); 342 | SerialUSB.println(batteryCellCount); 343 | } 344 | 345 | 346 | 347 | --------------------------------------------------------------------------------