/* * This file is part of Cleanflight. * * Cleanflight is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * Cleanflight is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ #include #include #include #include #include #include "build_config.h" #include "platform.h" #include "common/axis.h" #include "common/color.h" #include "common/maths.h" #include "drivers/system.h" #include "drivers/accgyro.h" #include "drivers/serial.h" #include "drivers/bus_i2c.h" #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "flight/flight.h" #include "flight/mixer.h" #include "flight/failsafe.h" #include "flight/navigation.h" #include "rx/rx.h" #include "rx/msp.h" #include "io/escservo.h" #include "io/rc_controls.h" #include "io/gps.h" #include "io/gimbal.h" #include "io/serial.h" #include "io/ledstrip.h" #include "telemetry/telemetry.h" #include "sensors/boardalignment.h" #include "sensors/sensors.h" #include "sensors/battery.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/compass.h" #include "sensors/gyro.h" #include "config/runtime_config.h" #include "config/config.h" #include "config/config_profile.h" #include "config/config_master.h" #include "version.h" #ifdef NAZE #include "hardware_revision.h" #endif #include "serial_msp.h" static serialPort_t *mspSerialPort; extern uint16_t cycleTime; // FIXME dependency on mw.c extern uint16_t rssi; // FIXME dependency on mw.c extern int16_t debug[4]; // FIXME dependency on mw.c /** * MSP Guidelines, emphasis is used to clarify. * * Each FlightController (FC, Server) MUST change the API version when any MSP command is added, deleted, or changed. * * If you fork the FC source code and release your own version, you MUST change the Flight Controller Identifier. * * NEVER release a modified copy of this code that shares the same Flight controller IDENT and API version * if the API doesn't match EXACTLY. * * Consumers of the API (API clients) SHOULD first attempt to get a response from the MSP_API_VERSION command. * If no response is obtained then client MAY try the legacy MSP_IDENT command. * * API consumers should ALWAYS handle communication failures gracefully and attempt to continue * without the information if possible. Clients MAY log/display a suitable message. * * API clients should NOT attempt any communication if they can't handle the API MAJOR VERSION. * * API clients SHOULD attempt communication if the API MINOR VERSION has increased from the time * the API client was written and handle command failures gracefully. Clients MAY disable * functionality that depends on the commands while still leaving other functionality intact. * Clients SHOULD operate in READ-ONLY mode and SHOULD present a warning to the user to state * that the newer API version may cause problems before using API commands that change FC state. * * It is for this reason that each MSP command should be specific as possible, such that changes * to commands break as little functionality as possible. * * API client authors MAY use a compatibility matrix/table when determining if they can support * a given command from a given flight controller at a given api version level. * * Developers MUST NOT create new MSP commands that do more than one thing. * * Failure to follow these guidelines will likely invoke the wrath of developers trying to write tools * that use the API and the users of those tools. */ #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made #define API_VERSION_MINOR 0 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 #define MULTIWII_IDENTIFIER "MWII"; #define CLEANFLIGHT_IDENTIFIER "CLFL" #define BASEFLIGHT_IDENTIFIER "BAFL"; #define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4 static const char *flightControllerIdentifier = CLEANFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller. #define FLIGHT_CONTROLLER_VERSION_LENGTH 3 #define FLIGHT_CONTROLLER_VERSION_MASK 0xFFF const char *boardIdentifier = TARGET_BOARD_IDENTIFIER; #define BOARD_IDENTIFIER_LENGTH 4 // 4 UPPER CASE alpha numeric characters that identify the board being used. #define BOARD_HARDWARE_REVISION_LENGTH 2 // These are baseflight specific flags but they are useless now since MW 2.3 uses the upper 4 bits for the navigation version. #define CAP_PLATFORM_32BIT ((uint32_t)1 << 31) #define CAP_BASEFLIGHT_CONFIG ((uint32_t)1 << 30) // MW 2.3 stores NAVI_VERSION in the top 4 bits of the capability mask. #define CAP_NAVI_VERSION_BIT_4_MSB ((uint32_t)1 << 31) #define CAP_NAVI_VERSION_BIT_3 ((uint32_t)1 << 30) #define CAP_NAVI_VERSION_BIT_2 ((uint32_t)1 << 29) #define CAP_NAVI_VERSION_BIT_1_LSB ((uint32_t)1 << 28) #define CAP_DYNBALANCE ((uint32_t)1 << 2) #define CAP_FLAPS ((uint32_t)1 << 3) #define CAP_NAVCAP ((uint32_t)1 << 4) #define CAP_EXTAUX ((uint32_t)1 << 5) /** * Returns MSP protocol version * API version * Flight Controller Identifier * Flight Controller build version (major, minor, patchlevel) * Board Identifier * Board Hardware Revision * Build Date - "MMM DD YYYY" MMM = Jan/Feb/... * Build Time - "HH:MM:SS" * SCM reference length * SCM reference (git revision, svn commit id) * Additional FC information length * Additional FC information (as decided by the FC, for FC specific tools to use as required) **/ #define MSP_API_VERSION 1 //out message // // MSP commands for Cleanflight original features // #define MSP_CHANNEL_FORWARDING 32 //out message Returns channel forwarding settings #define MSP_SET_CHANNEL_FORWARDING 33 //in message Channel forwarding settings #define MSP_MODE_RANGES 34 //out message Returns all mode ranges #define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range #define MSP_FEATURE 36 #define MSP_SET_FEATURE 37 #define MSP_BOARD_ALIGNMENT 38 #define MSP_SET_BOARD_ALIGNMENT 39 #define MSP_CURRENT_METER_CONFIG 40 #define MSP_SET_CURRENT_METER_CONFIG 41 #define MSP_MIXER 42 #define MSP_SET_MIXER 43 #define MSP_RX_CONFIG 44 #define MSP_SET_RX_CONFIG 45 #define MSP_LED_COLORS 46 #define MSP_SET_LED_COLORS 47 #define MSP_LED_STRIP_CONFIG 48 #define MSP_SET_LED_STRIP_CONFIG 49 #define MSP_RSSI_CONFIG 50 #define MSP_SET_RSSI_CONFIG 51 #define MSP_ADJUSTMENT_RANGES 52 #define MSP_SET_ADJUSTMENT_RANGE 53 // // Baseflight MSP commands (if enabled they exist in Cleanflight) // #define MSP_RX_MAP 64 //out message get channel map (also returns number of channels total) #define MSP_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from MSP_RX_MAP // FIXME - Provided for backwards compatibility with configurator code until configurator is updated. // DEPRECATED - DO NOT USE "MSP_CONFIG" and MSP_SET_CONFIG. In Cleanflight, isolated commands already exist and should be used instead. #define MSP_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere #define MSP_SET_CONFIG 67 //in message baseflight-specific settings save #define MSP_REBOOT 68 //in message reboot settings // DEPRECATED - Use MSP_API_VERSION instead #define MSP_BUILD_INFO 69 //out message build date as well as some space for future expansion // // Multwii original MSP commands // // DEPRECATED - See MSP_API_VERSION and MSP_MIXER #define MSP_IDENT 100 //out message multitype + multiwii version + protocol version + capability variable #define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number #define MSP_RAW_IMU 102 //out message 9 DOF #define MSP_SERVO 103 //out message 8 servos #define MSP_MOTOR 104 //out message 8 motors #define MSP_RC 105 //out message 8 rc chan and more #define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course #define MSP_COMP_GPS 107 //out message distance home, direction home #define MSP_ATTITUDE 108 //out message 2 angles 1 heading #define MSP_ALTITUDE 109 //out message altitude, variometer #define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX #define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID #define MSP_PID 112 //out message P I D coeff (9 are used currently) #define MSP_BOX 113 //out message BOX setup (number is dependant of your setup) #define MSP_MISC 114 //out message powermeter trig #define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI #define MSP_BOXNAMES 116 //out message the aux switch names #define MSP_PIDNAMES 117 //out message the PID names #define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold #define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes #define MSP_SERVO_CONF 120 //out message Servo settings #define MSP_NAV_STATUS 121 //out message Returns navigation status #define MSP_NAV_CONFIG 122 //out message Returns navigation parameters #define MSP_SET_RAW_RC 200 //in message 8 rc chan #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed #define MSP_SET_PID 202 //in message P I D coeff (9 are used currently) #define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup) #define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID #define MSP_ACC_CALIBRATION 205 //in message no param #define MSP_MAG_CALIBRATION 206 //in message no param #define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use #define MSP_RESET_CONF 208 //in message no param #define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags) #define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2) #define MSP_SET_HEAD 211 //in message define a new heading hold direction #define MSP_SET_SERVO_CONF 212 //in message Servo settings #define MSP_SET_MOTOR 214 //in message PropBalance function #define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom // #define MSP_BIND 240 //in message no param #define MSP_EEPROM_WRITE 250 //in message no param #define MSP_DEBUGMSG 253 //out message debug string buffer #define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4 // Additional commands that are not compatible with MultiWii #define MSP_UID 160 //out message Unique device ID #define MSP_ACC_TRIM 240 //out message get acc angle trim values #define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values #define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox) #define INBUF_SIZE 64 typedef struct box_e { const uint8_t boxId; // see boxId_e const char *boxName; // GUI-readable box name const uint8_t permanentId; // } box_t; // FIXME remove ;'s static const box_t const boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXARM, "ARM;", 0 }, { BOXANGLE, "ANGLE;", 1 }, { BOXHORIZON, "HORIZON;", 2 }, { BOXBARO, "BARO;", 3 }, //{ BOXVARIO, "VARIO;", 4 }, { BOXMAG, "MAG;", 5 }, { BOXHEADFREE, "HEADFREE;", 6 }, { BOXHEADADJ, "HEADADJ;", 7 }, { BOXCAMSTAB, "CAMSTAB;", 8 }, { BOXCAMTRIG, "CAMTRIG;", 9 }, { BOXGPSHOME, "GPS HOME;", 10 }, { BOXGPSHOLD, "GPS HOLD;", 11 }, { BOXPASSTHRU, "PASSTHRU;", 12 }, { BOXBEEPERON, "BEEPER;", 13 }, { BOXLEDMAX, "LEDMAX;", 14 }, { BOXLEDLOW, "LEDLOW;", 15 }, { BOXLLIGHTS, "LLIGHTS;", 16 }, { BOXCALIB, "CALIB;", 17 }, { BOXGOV, "GOVERNOR;", 18 }, { BOXOSD, "OSD SW;", 19 }, { BOXTELEMETRY, "TELEMETRY;", 20 }, { BOXAUTOTUNE, "AUTOTUNE;", 21 }, { BOXSONAR, "SONAR;", 22 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; // this is calculated at startup based on enabled features. static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT]; // this is the number of filled indexes in above array static uint8_t activeBoxIdCount = 0; // from mixer.c extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; // cause reboot after MSP processing complete static bool isRebootScheduled = false; static const char pidnames[] = "ROLL;" "PITCH;" "YAW;" "ALT;" "Pos;" "PosR;" "NavR;" "LEVEL;" "MAG;" "VEL;"; typedef enum { IDLE, HEADER_START, HEADER_M, HEADER_ARROW, HEADER_SIZE, HEADER_CMD, } mspState_e; typedef enum { UNUSED_PORT = 0, FOR_GENERAL_MSP, FOR_TELEMETRY } mspPortUsage_e; typedef struct mspPort_s { serialPort_t *port; uint8_t offset; uint8_t dataSize; uint8_t checksum; uint8_t indRX; uint8_t inBuf[INBUF_SIZE]; mspState_e c_state; uint8_t cmdMSP; mspPortUsage_e mspPortUsage; } mspPort_t; static mspPort_t mspPorts[MAX_MSP_PORT_COUNT]; static mspPort_t *currentPort; void serialize32(uint32_t a) { static uint8_t t; t = a; serialWrite(mspSerialPort, t); currentPort->checksum ^= t; t = a >> 8; serialWrite(mspSerialPort, t); currentPort->checksum ^= t; t = a >> 16; serialWrite(mspSerialPort, t); currentPort->checksum ^= t; t = a >> 24; serialWrite(mspSerialPort, t); currentPort->checksum ^= t; } void serialize16(int16_t a) { static uint8_t t; t = a; serialWrite(mspSerialPort, t); currentPort->checksum ^= t; t = a >> 8 & 0xff; serialWrite(mspSerialPort, t); currentPort->checksum ^= t; } void serialize8(uint8_t a) { serialWrite(mspSerialPort, a); currentPort->checksum ^= a; } uint8_t read8(void) { return currentPort->inBuf[currentPort->indRX++] & 0xff; } uint16_t read16(void) { uint16_t t = read8(); t += (uint16_t)read8() << 8; return t; } uint32_t read32(void) { uint32_t t = read16(); t += (uint32_t)read16() << 16; return t; } void headSerialResponse(uint8_t err, uint8_t s) { serialize8('$'); serialize8('M'); serialize8(err ? '!' : '>'); currentPort->checksum = 0; // start calculating a new checksum serialize8(s); serialize8(currentPort->cmdMSP); } void headSerialReply(uint8_t s) { headSerialResponse(0, s); } void headSerialError(uint8_t s) { headSerialResponse(1, s); } void tailSerialReply(void) { serialize8(currentPort->checksum); } void s_struct(uint8_t *cb, uint8_t siz) { headSerialReply(siz); while (siz--) serialize8(*cb++); } void serializeNames(const char *s) { const char *c; for (c = s; *c; c++) serialize8(*c); } const box_t *findBoxByActiveBoxId(uint8_t activeBoxId) { uint8_t boxIndex; const box_t *candidate; for (boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) { candidate = &boxes[boxIndex]; if (candidate->boxId == activeBoxId) { return candidate; } } return NULL; } const box_t *findBoxByPermenantId(uint8_t permenantId) { uint8_t boxIndex; const box_t *candidate; for (boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) { candidate = &boxes[boxIndex]; if (candidate->permanentId == permenantId) { return candidate; } } return NULL; } void serializeBoxNamesReply(void) { int i, activeBoxId, j, flag = 1, count = 0, len; const box_t *box; reset: // in first run of the loop, we grab total size of junk to be sent // then come back and actually send it for (i = 0; i < activeBoxIdCount; i++) { activeBoxId = activeBoxIds[i]; box = findBoxByActiveBoxId(activeBoxId); if (!box) { continue; } len = strlen(box->boxName); if (flag) { count += len; } else { for (j = 0; j < len; j++) serialize8(box->boxName[j]); } } if (flag) { headSerialReply(count); flag = 0; goto reset; } } static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort, mspPortUsage_e usage) { memset(mspPortToReset, 0, sizeof(mspPort_t)); mspPortToReset->port = serialPort; mspPortToReset->mspPortUsage = usage; } // This rate is chosen since softserial supports it. #define MSP_FALLBACK_BAUDRATE 19200 void mspAllocateSerialPorts(serialConfig_t *serialConfig) { serialPort_t *port; uint8_t portIndex; for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { mspPort_t *mspPort = &mspPorts[portIndex]; if (mspPort->mspPortUsage != UNUSED_PORT) { continue; } uint32_t baudRate = serialConfig->msp_baudrate; bool triedFallbackRate = false; do { port = openSerialPort(FUNCTION_MSP, NULL, baudRate, MODE_RXTX, SERIAL_NOT_INVERTED); if (!port) { if (triedFallbackRate) { break; } baudRate = MSP_FALLBACK_BAUDRATE; triedFallbackRate = true; } } while (!port); if (port && portIndex < MAX_MSP_PORT_COUNT) { resetMspPort(mspPort, port, FOR_GENERAL_MSP); } if (!port) { break; } } // XXX this function might help with adding support for MSP on more than one port, if not delete it. const serialPortFunctionList_t *serialPortFunctionList = getSerialPortFunctionList(); UNUSED(serialPortFunctionList); } void mspReleasePortIfAllocated(serialPort_t *serialPort) { uint8_t portIndex; for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { mspPort_t *candidateMspPort = &mspPorts[portIndex]; if (candidateMspPort->port == serialPort) { endSerialPortFunction(serialPort, FUNCTION_MSP); memset(candidateMspPort, 0, sizeof(mspPort_t)); } } } void mspInit(serialConfig_t *serialConfig) { // calculate used boxes based on features and fill availableBoxes[] array memset(activeBoxIds, 0xFF, sizeof(activeBoxIds)); activeBoxIdCount = 0; activeBoxIds[activeBoxIdCount++] = BOXARM; if (sensors(SENSOR_ACC)) { activeBoxIds[activeBoxIdCount++] = BOXANGLE; activeBoxIds[activeBoxIdCount++] = BOXHORIZON; } if (sensors(SENSOR_BARO)) { activeBoxIds[activeBoxIdCount++] = BOXBARO; } if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { activeBoxIds[activeBoxIdCount++] = BOXMAG; activeBoxIds[activeBoxIdCount++] = BOXHEADFREE; activeBoxIds[activeBoxIdCount++] = BOXHEADADJ; } if (feature(FEATURE_SERVO_TILT)) activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB; #ifdef GPS if (feature(FEATURE_GPS)) { activeBoxIds[activeBoxIdCount++] = BOXGPSHOME; activeBoxIds[activeBoxIdCount++] = BOXGPSHOLD; } #endif if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE) activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU; activeBoxIds[activeBoxIdCount++] = BOXBEEPERON; if (feature(FEATURE_INFLIGHT_ACC_CAL)) activeBoxIds[activeBoxIdCount++] = BOXCALIB; activeBoxIds[activeBoxIdCount++] = BOXOSD; if (feature(FEATURE_TELEMETRY && masterConfig.telemetryConfig.telemetry_switch)) activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY; #ifdef AUTOTUNE activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE; #endif if (feature(FEATURE_SONAR)){ activeBoxIds[activeBoxIdCount++] = BOXSONAR; } memset(mspPorts, 0x00, sizeof(mspPorts)); mspAllocateSerialPorts(serialConfig); } #define IS_ENABLED(mask) (mask == 0 ? 0 : 1) static bool processOutCommand(uint8_t cmdMSP) { uint32_t i, tmp, junk; #ifdef GPS uint8_t wp_no; int32_t lat = 0, lon = 0; #endif switch (cmdMSP) { case MSP_API_VERSION: // the components of this command are in an order such that future changes could be made to it without breaking clients. // i.e. most important first. headSerialReply( 1 + // protocol version length API_VERSION_LENGTH + FLIGHT_CONTROLLER_IDENTIFIER_LENGTH + FLIGHT_CONTROLLER_VERSION_LENGTH + BOARD_IDENTIFIER_LENGTH + BOARD_HARDWARE_REVISION_LENGTH + BUILD_DATE_LENGTH + BUILD_TIME_LENGTH + 1 + // scm reference length GIT_SHORT_REVISION_LENGTH + 1 // additional FC specific length // no addition FC specific data yet. ); serialize8(MSP_PROTOCOL_VERSION); serialize8(API_VERSION_MAJOR); serialize8(API_VERSION_MINOR); for (i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) { serialize8(flightControllerIdentifier[i]); } serialize8(FC_VERSION_MAJOR); serialize8(FC_VERSION_MINOR); serialize8(FC_VERSION_PATCH_LEVEL); for (i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) { serialize8(boardIdentifier[i]); } #ifdef NAZE serialize16(hardwareRevision); #else serialize16(0); // No other build targets currently have hardware revision detection. #endif for (i = 0; i < BUILD_DATE_LENGTH; i++) { serialize8(buildDate[i]); } for (i = 0; i < BUILD_TIME_LENGTH; i++) { serialize8(buildTime[i]); } serialize8(GIT_SHORT_REVISION_LENGTH); for (i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) { serialize8(shortGitRevision[i]); } serialize8(0); // No flight controller specific information to follow. break; // DEPRECATED - Use MSP_API_VERSION case MSP_IDENT: headSerialReply(7); serialize8(MW_VERSION); serialize8(masterConfig.mixerConfiguration); // type of multicopter serialize8(MSP_PROTOCOL_VERSION); serialize32(CAP_DYNBALANCE | (masterConfig.airplaneConfig.flaps_speed ? CAP_FLAPS : 0)); // "capability" break; case MSP_STATUS: headSerialReply(11); serialize16(cycleTime); #ifdef USE_I2C serialize16(i2cGetErrorCounter()); #else serialize16(0); #endif serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES // Requires new Multiwii protocol version to fix // It would be preferable to setting the enabled bits based on BOXINDEX. junk = 0; tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE | IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON | IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << BOXBARO | IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG | IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMTRIG)) << BOXCAMTRIG | IS_ENABLED(FLIGHT_MODE(GPS_HOME_MODE)) << BOXGPSHOME | IS_ENABLED(FLIGHT_MODE(GPS_HOLD_MODE)) << BOXGPSHOLD | IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX)) << BOXLEDMAX | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCALIB)) << BOXCALIB | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) << BOXAUTOTUNE | IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR | IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM; for (i = 0; i < activeBoxIdCount; i++) { int flag = (tmp & (1 << activeBoxIds[i])); if (flag) junk |= 1 << i; } serialize32(junk); serialize8(masterConfig.current_profile_index); break; case MSP_RAW_IMU: headSerialReply(18); // Retarded hack until multiwiidorks start using real units for sensor data if (acc_1G > 1024) { for (i = 0; i < 3; i++) serialize16(accSmooth[i] / 8); } else { for (i = 0; i < 3; i++) serialize16(accSmooth[i]); } for (i = 0; i < 3; i++) serialize16(gyroData[i]); for (i = 0; i < 3; i++) serialize16(magADC[i]); break; case MSP_SERVO: s_struct((uint8_t *)&servo, 16); break; case MSP_SERVO_CONF: headSerialReply(56); for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { serialize16(currentProfile->servoConf[i].min); serialize16(currentProfile->servoConf[i].max); serialize16(currentProfile->servoConf[i].middle); serialize8(currentProfile->servoConf[i].rate); } break; case MSP_CHANNEL_FORWARDING: headSerialReply(8); for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { serialize8(currentProfile->servoConf[i].forwardFromChannel); } break; case MSP_MOTOR: s_struct((uint8_t *)motor, 16); break; case MSP_RC: headSerialReply(2 * rxRuntimeConfig.channelCount); for (i = 0; i < rxRuntimeConfig.channelCount; i++) serialize16(rcData[i]); break; case MSP_ATTITUDE: headSerialReply(6); for (i = 0; i < 2; i++) serialize16(inclination.raw[i]); serialize16(heading); break; case MSP_ALTITUDE: headSerialReply(6); serialize32(EstAlt); serialize16(vario); break; case MSP_ANALOG: headSerialReply(7); serialize8((uint8_t)constrain(vbat, 0, 255)); serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamphours drawn from battery serialize16(rssi); if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) { serialize16((uint16_t)constrain((abs(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps } else serialize16((uint16_t)constrain(abs(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps break; case MSP_RC_TUNING: headSerialReply(7); serialize8(currentControlRateProfile->rcRate8); serialize8(currentControlRateProfile->rcExpo8); serialize8(currentControlRateProfile->rollPitchRate); serialize8(currentControlRateProfile->yawRate); serialize8(currentControlRateProfile->dynThrPID); serialize8(currentControlRateProfile->thrMid8); serialize8(currentControlRateProfile->thrExpo8); break; case MSP_PID: headSerialReply(3 * PID_ITEM_COUNT); if (currentProfile->pidController == 2) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid for (i = 0; i < 3; i++) { serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 250)); serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 250)); serialize8(constrain(lrintf(currentProfile->pidProfile.D_f[i] * 1000.0f), 0, 100)); } for (i = 3; i < PID_ITEM_COUNT; i++) { if (i == PIDLEVEL) { serialize8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 250)); serialize8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 250)); serialize8(0); } else { serialize8(currentProfile->pidProfile.P8[i]); serialize8(currentProfile->pidProfile.I8[i]); serialize8(currentProfile->pidProfile.D8[i]); } } } else { for (i = 0; i < PID_ITEM_COUNT; i++) { serialize8(currentProfile->pidProfile.P8[i]); serialize8(currentProfile->pidProfile.I8[i]); serialize8(currentProfile->pidProfile.D8[i]); } } break; case MSP_PIDNAMES: headSerialReply(sizeof(pidnames) - 1); serializeNames(pidnames); break; case MSP_MODE_RANGES: headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT); for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { modeActivationCondition_t *mac = ¤tProfile->modeActivationConditions[i]; const box_t *box = &boxes[mac->modeId]; serialize8(box->permanentId); serialize8(mac->auxChannelIndex); serialize8(mac->range.startStep); serialize8(mac->range.endStep); } break; case MSP_ADJUSTMENT_RANGES: headSerialReply(MAX_ADJUSTMENT_RANGE_COUNT * ( 1 + // adjustment index/slot 1 + // aux channel index 1 + // start step 1 + // end step 1 + // adjustment function 1 // aux switch channel index )); for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { adjustmentRange_t *adjRange = ¤tProfile->adjustmentRanges[i]; serialize8(adjRange->adjustmentIndex); serialize8(adjRange->auxChannelIndex); serialize8(adjRange->range.startStep); serialize8(adjRange->range.endStep); serialize8(adjRange->adjustmentFunction); serialize8(adjRange->auxSwitchChannelIndex); } break; case MSP_BOXNAMES: serializeBoxNamesReply(); break; case MSP_BOXIDS: headSerialReply(activeBoxIdCount); for (i = 0; i < activeBoxIdCount; i++) { const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]); if (!box) { continue; } serialize8(box->permanentId); } break; case MSP_MISC: headSerialReply(2 * 6 + 4 + 2 + 4); serialize16(masterConfig.rxConfig.midrc); serialize16(masterConfig.escAndServoConfig.minthrottle); serialize16(masterConfig.escAndServoConfig.maxthrottle); serialize16(masterConfig.escAndServoConfig.mincommand); serialize16(currentProfile->failsafeConfig.failsafe_throttle); #ifdef GPS serialize8(masterConfig.gpsConfig.provider); // gps_type serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t serialize8(masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas #else serialize8(0); // gps_type serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t serialize8(0); // gps_ubx_sbas #endif serialize8(masterConfig.batteryConfig.multiwiiCurrentMeterOutput); serialize8(masterConfig.rxConfig.rssi_channel); serialize8(0); serialize16(currentProfile->mag_declination / 10); serialize8(masterConfig.batteryConfig.vbatscale); serialize8(masterConfig.batteryConfig.vbatmincellvoltage); serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage); serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage); break; case MSP_MOTOR_PINS: headSerialReply(8); for (i = 0; i < 8; i++) serialize8(i + 1); break; #ifdef GPS case MSP_RAW_GPS: headSerialReply(16); serialize8(STATE(GPS_FIX)); serialize8(GPS_numSat); serialize32(GPS_coord[LAT]); serialize32(GPS_coord[LON]); serialize16(GPS_altitude); serialize16(GPS_speed); serialize16(GPS_ground_course); break; case MSP_COMP_GPS: headSerialReply(5); serialize16(GPS_distanceToHome); serialize16(GPS_directionToHome); serialize8(GPS_update & 1); break; case MSP_WP: wp_no = read8(); // get the wp number headSerialReply(18); if (wp_no == 0) { lat = GPS_home[LAT]; lon = GPS_home[LON]; } else if (wp_no == 16) { lat = GPS_hold[LAT]; lon = GPS_hold[LON]; } serialize8(wp_no); serialize32(lat); serialize32(lon); serialize32(AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps serialize16(0); // heading will come here (deg) serialize16(0); // time to stay (ms) will come here serialize8(0); // nav flag will come here break; case MSP_GPSSVINFO: headSerialReply(1 + (GPS_numCh * 4)); serialize8(GPS_numCh); for (i = 0; i < GPS_numCh; i++){ serialize8(GPS_svinfo_chn[i]); serialize8(GPS_svinfo_svid[i]); serialize8(GPS_svinfo_quality[i]); serialize8(GPS_svinfo_cno[i]); } break; #endif case MSP_DEBUG: headSerialReply(8); // make use of this crap, output some useful QA statistics //debug[3] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock] for (i = 0; i < 4; i++) serialize16(debug[i]); // 4 variables are here for general monitoring purpose break; // Additional commands that are not compatible with MultiWii case MSP_ACC_TRIM: headSerialReply(4); serialize16(currentProfile->accelerometerTrims.values.pitch); serialize16(currentProfile->accelerometerTrims.values.roll); break; case MSP_UID: headSerialReply(12); serialize32(U_ID_0); serialize32(U_ID_1); serialize32(U_ID_2); break; case MSP_FEATURE: headSerialReply(4); serialize32(featureMask()); break; case MSP_BOARD_ALIGNMENT: headSerialReply(3); serialize16(masterConfig.boardAlignment.rollDegrees); serialize16(masterConfig.boardAlignment.pitchDegrees); serialize16(masterConfig.boardAlignment.yawDegrees); break; case MSP_CURRENT_METER_CONFIG: headSerialReply(4); serialize16(masterConfig.batteryConfig.currentMeterScale); serialize16(masterConfig.batteryConfig.currentMeterOffset); break; case MSP_MIXER: headSerialReply(1); serialize8(masterConfig.mixerConfiguration); break; case MSP_RX_CONFIG: headSerialReply(7); serialize8(masterConfig.rxConfig.serialrx_provider); serialize16(masterConfig.rxConfig.maxcheck); serialize16(masterConfig.rxConfig.midrc); serialize16(masterConfig.rxConfig.mincheck); serialize8(masterConfig.rxConfig.spektrum_sat_bind); break; case MSP_RSSI_CONFIG: headSerialReply(1); serialize8(masterConfig.rxConfig.rssi_channel); break; case MSP_RX_MAP: headSerialReply(MAX_MAPPABLE_RX_INPUTS); for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) serialize8(masterConfig.rxConfig.rcmap[i]); break; case MSP_CONFIG: headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 2 + 2); serialize8(masterConfig.mixerConfiguration); serialize32(featureMask()); serialize8(masterConfig.rxConfig.serialrx_provider); serialize16(masterConfig.boardAlignment.rollDegrees); serialize16(masterConfig.boardAlignment.pitchDegrees); serialize16(masterConfig.boardAlignment.yawDegrees); serialize16(masterConfig.batteryConfig.currentMeterScale); serialize16(masterConfig.batteryConfig.currentMeterOffset); break; #ifdef LED_STRIP case MSP_LED_COLORS: headSerialReply(CONFIGURABLE_COLOR_COUNT * 4); for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { hsvColor_t *color = &masterConfig.colors[i]; serialize16(color->h); serialize8(color->s); serialize8(color->v); } break; case MSP_LED_STRIP_CONFIG: headSerialReply(MAX_LED_STRIP_LENGTH * 4); for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; serialize16((ledConfig->flags & LED_DIRECTION_MASK) >> LED_DIRECTION_BIT_OFFSET); serialize16((ledConfig->flags & LED_FUNCTION_MASK) >> LED_FUNCTION_BIT_OFFSET); serialize8(GET_LED_X(ledConfig)); serialize8(GET_LED_Y(ledConfig)); } break; #endif case MSP_BUILD_INFO: headSerialReply(11 + 4 + 4); for (i = 0; i < 11; i++) serialize8(buildDate[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc serialize32(0); // future exp serialize32(0); // future exp break; default: return false; } return true; } static bool processInCommand(void) { uint32_t i; uint16_t tmp; #ifdef GPS uint8_t wp_no; int32_t lat = 0, lon = 0, alt = 0; #endif switch (currentPort->cmdMSP) { case MSP_SELECT_SETTING: if (!ARMING_FLAG(ARMED)) { masterConfig.current_profile_index = read8(); if (masterConfig.current_profile_index > 2) { masterConfig.current_profile_index = 0; } writeEEPROM(); readEEPROM(); } break; case MSP_SET_HEAD: magHold = read16(); break; case MSP_SET_RAW_RC: // FIXME need support for more than 8 channels for (i = 0; i < 8; i++) rcData[i] = read16(); rxMspFrameRecieve(); break; case MSP_SET_ACC_TRIM: currentProfile->accelerometerTrims.values.pitch = read16(); currentProfile->accelerometerTrims.values.roll = read16(); break; case MSP_SET_PID: if (currentProfile->pidController == 2) { for (i = 0; i < 3; i++) { currentProfile->pidProfile.P_f[i] = (float)read8() / 10.0f; currentProfile->pidProfile.I_f[i] = (float)read8() / 100.0f; currentProfile->pidProfile.D_f[i] = (float)read8() / 1000.0f; } for (i = 3; i < PID_ITEM_COUNT; i++) { if (i == PIDLEVEL) { currentProfile->pidProfile.A_level = (float)read8() / 10.0f; currentProfile->pidProfile.H_level = (float)read8() / 10.0f; read8(); } else { currentProfile->pidProfile.P8[i] = read8(); currentProfile->pidProfile.I8[i] = read8(); currentProfile->pidProfile.D8[i] = read8(); } } } else { for (i = 0; i < PID_ITEM_COUNT; i++) { currentProfile->pidProfile.P8[i] = read8(); currentProfile->pidProfile.I8[i] = read8(); currentProfile->pidProfile.D8[i] = read8(); } } break; case MSP_SET_MODE_RANGE: i = read8(); if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) { modeActivationCondition_t *mac = ¤tProfile->modeActivationConditions[i]; i = read8(); const box_t *box = findBoxByPermenantId(i); if (box) { mac->modeId = box->boxId; mac->auxChannelIndex = read8(); mac->range.startStep = read8(); mac->range.endStep = read8(); } else { headSerialError(0); } } else { headSerialError(0); } break; case MSP_SET_ADJUSTMENT_RANGE: i = read8(); if (i < MAX_ADJUSTMENT_RANGE_COUNT) { adjustmentRange_t *adjRange = ¤tProfile->adjustmentRanges[i]; i = read8(); if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) { adjRange->adjustmentIndex = i; adjRange->auxChannelIndex = read8(); adjRange->range.startStep = read8(); adjRange->range.endStep = read8(); adjRange->adjustmentFunction = read8(); adjRange->auxSwitchChannelIndex = read8(); } else { headSerialError(0); } } else { headSerialError(0); } break; case MSP_SET_RC_TUNING: currentControlRateProfile->rcRate8 = read8(); currentControlRateProfile->rcExpo8 = read8(); currentControlRateProfile->rollPitchRate = read8(); currentControlRateProfile->yawRate = read8(); currentControlRateProfile->dynThrPID = read8(); currentControlRateProfile->thrMid8 = read8(); currentControlRateProfile->thrExpo8 = read8(); break; case MSP_SET_MISC: tmp = read16(); if (tmp < 1600 && tmp > 1400) masterConfig.rxConfig.midrc = tmp; masterConfig.escAndServoConfig.minthrottle = read16(); masterConfig.escAndServoConfig.maxthrottle = read16(); masterConfig.escAndServoConfig.mincommand = read16(); currentProfile->failsafeConfig.failsafe_throttle = read16(); #ifdef GPS masterConfig.gpsConfig.provider = read8(); // gps_type read8(); // gps_baudrate masterConfig.gpsConfig.sbasMode = read8(); // gps_ubx_sbas #else read8(); // gps_type read8(); // gps_baudrate read8(); // gps_ubx_sbas #endif masterConfig.batteryConfig.multiwiiCurrentMeterOutput = read8(); masterConfig.rxConfig.rssi_channel = read8(); read8(); currentProfile->mag_declination = read16() * 10; masterConfig.batteryConfig.vbatscale = read8(); // actual vbatscale as intended masterConfig.batteryConfig.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI masterConfig.batteryConfig.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI masterConfig.batteryConfig.vbatwarningcellvoltage = read8(); // vbatlevel when buzzer starts to alert break; case MSP_SET_MOTOR: for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8 motor_disarmed[i] = read16(); break; case MSP_SET_SERVO_CONF: for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { currentProfile->servoConf[i].min = read16(); currentProfile->servoConf[i].max = read16(); // provide temporary support for old clients that try and send a channel index instead of a servo middle uint16_t potentialServoMiddleOrChannelToForward = read16(); if (potentialServoMiddleOrChannelToForward < MAX_SUPPORTED_SERVOS) { currentProfile->servoConf[i].forwardFromChannel = potentialServoMiddleOrChannelToForward; } if (potentialServoMiddleOrChannelToForward >= PWM_RANGE_MIN && potentialServoMiddleOrChannelToForward <= PWM_RANGE_MAX) { currentProfile->servoConf[i].middle = potentialServoMiddleOrChannelToForward; } currentProfile->servoConf[i].rate = read8(); } break; case MSP_SET_CHANNEL_FORWARDING: for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { currentProfile->servoConf[i].forwardFromChannel = read8(); } break; case MSP_RESET_CONF: if (!ARMING_FLAG(ARMED)) { resetEEPROM(); readEEPROM(); } break; case MSP_ACC_CALIBRATION: if (!ARMING_FLAG(ARMED)) accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); break; case MSP_MAG_CALIBRATION: if (!ARMING_FLAG(ARMED)) ENABLE_STATE(CALIBRATE_MAG); break; case MSP_EEPROM_WRITE: if (ARMING_FLAG(ARMED)) { headSerialError(0); return true; } writeEEPROM(); readEEPROM(); break; #ifdef GPS case MSP_SET_RAW_GPS: if (read8()) { ENABLE_STATE(GPS_FIX); } else { DISABLE_STATE(GPS_FIX); } GPS_numSat = read8(); GPS_coord[LAT] = read32(); GPS_coord[LON] = read32(); GPS_altitude = read16(); GPS_speed = read16(); GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers break; case MSP_SET_WP: wp_no = read8(); //get the wp number lat = read32(); lon = read32(); alt = read32(); // to set altitude (cm) read16(); // future: to set heading (deg) read16(); // future: to set time to stay (ms) read8(); // future: to set nav flag if (wp_no == 0) { GPS_home[LAT] = lat; GPS_home[LON] = lon; DISABLE_FLIGHT_MODE(GPS_HOME_MODE); // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS ENABLE_STATE(GPS_FIX_HOME); if (alt != 0) AltHold = alt; // temporary implementation to test feature with apps } else if (wp_no == 16) { // OK with SERIAL GPS -- NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS GPS_hold[LAT] = lat; GPS_hold[LON] = lon; if (alt != 0) AltHold = alt; // temporary implementation to test feature with apps nav_mode = NAV_MODE_WP; GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]); } break; #endif case MSP_SET_FEATURE: featureClearAll(); featureSet(read32()); // features bitmap break; case MSP_SET_BOARD_ALIGNMENT: masterConfig.boardAlignment.rollDegrees = read16(); masterConfig.boardAlignment.pitchDegrees = read16(); masterConfig.boardAlignment.yawDegrees = read16(); break; case MSP_SET_CURRENT_METER_CONFIG: masterConfig.batteryConfig.currentMeterScale = read16(); masterConfig.batteryConfig.currentMeterOffset = read16(); break; case MSP_SET_MIXER: masterConfig.mixerConfiguration = read8(); break; case MSP_SET_RX_CONFIG: masterConfig.rxConfig.serialrx_provider = read8(); masterConfig.rxConfig.maxcheck = read16(); masterConfig.rxConfig.midrc = read16(); masterConfig.rxConfig.mincheck = read16(); masterConfig.rxConfig.spektrum_sat_bind = read8(); break; case MSP_SET_RSSI_CONFIG: masterConfig.rxConfig.rssi_channel = read8(); break; case MSP_SET_RX_MAP: for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { masterConfig.rxConfig.rcmap[i] = read8(); } break; case MSP_SET_CONFIG: #ifdef CJMCU masterConfig.mixerConfiguration = read8(); // multitype #else read8(); // multitype #endif featureClearAll(); featureSet(read32()); // features bitmap masterConfig.rxConfig.serialrx_provider = read8(); // serialrx_type masterConfig.boardAlignment.rollDegrees = read16(); // board_align_roll masterConfig.boardAlignment.pitchDegrees = read16(); // board_align_pitch masterConfig.boardAlignment.yawDegrees = read16(); // board_align_yaw masterConfig.batteryConfig.currentMeterScale = read16(); masterConfig.batteryConfig.currentMeterOffset = read16(); break; #ifdef LED_STRIP case MSP_SET_LED_COLORS: for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { hsvColor_t *color = &masterConfig.colors[i]; color->h = read16(); color->s = read8(); color->v = read8(); } break; case MSP_SET_LED_STRIP_CONFIG: for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; uint16_t mask; // currently we're storing directions and functions in a uint16 (flags) // the msp uses 2 x uint16_t to cater for future expansion mask = read16(); ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK; mask = read16(); ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK; mask = read8(); ledConfig->xy = CALCULATE_LED_X(mask); mask = read8(); ledConfig->xy |= CALCULATE_LED_Y(mask); } break; #endif case MSP_REBOOT: isRebootScheduled = true; break; default: // we do not know how to handle the (valid) message, indicate error MSP $M! return false; } headSerialReply(0); return true; } static void mspProcessPort(void) { uint8_t c; while (serialTotalBytesWaiting(mspSerialPort)) { c = serialRead(mspSerialPort); if (currentPort->c_state == IDLE) { currentPort->c_state = (c == '$') ? HEADER_START : IDLE; if (currentPort->c_state == IDLE && !ARMING_FLAG(ARMED)) evaluateOtherData(c); // if not armed evaluate all other incoming serial data } else if (currentPort->c_state == HEADER_START) { currentPort->c_state = (c == 'M') ? HEADER_M : IDLE; } else if (currentPort->c_state == HEADER_M) { currentPort->c_state = (c == '<') ? HEADER_ARROW : IDLE; } else if (currentPort->c_state == HEADER_ARROW) { if (c > INBUF_SIZE) { // now we are expecting the payload size currentPort->c_state = IDLE; continue; } currentPort->dataSize = c; currentPort->offset = 0; currentPort->checksum = 0; currentPort->indRX = 0; currentPort->checksum ^= c; currentPort->c_state = HEADER_SIZE; // the command is to follow } else if (currentPort->c_state == HEADER_SIZE) { currentPort->cmdMSP = c; currentPort->checksum ^= c; currentPort->c_state = HEADER_CMD; } else if (currentPort->c_state == HEADER_CMD && currentPort->offset < currentPort->dataSize) { currentPort->checksum ^= c; currentPort->inBuf[currentPort->offset++] = c; } else if (currentPort->c_state == HEADER_CMD && currentPort->offset >= currentPort->dataSize) { if (currentPort->checksum == c) { // compare calculated and transferred checksum // we got a valid packet, evaluate it if (!(processOutCommand(currentPort->cmdMSP) || processInCommand())) { headSerialError(0); } tailSerialReply(); } currentPort->c_state = IDLE; } } } void setCurrentPort(mspPort_t *port) { currentPort = port; mspSerialPort = currentPort->port; } void mspProcess(void) { uint8_t portIndex; mspPort_t *candidatePort; for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { candidatePort = &mspPorts[portIndex]; if (candidatePort->mspPortUsage != FOR_GENERAL_MSP) { continue; } setCurrentPort(candidatePort); mspProcessPort(); if (isRebootScheduled) { // pause a little while to allow response to be sent while (!isSerialTransmitBufferEmpty(candidatePort->port)) { delay(50); } systemReset(); } } } static const uint8_t mspTelemetryCommandSequence[] = { MSP_BOXNAMES, // repeat boxnames, in case the first transmission was lost or never received. MSP_STATUS, MSP_IDENT, MSP_RAW_IMU, MSP_ALTITUDE, MSP_RAW_GPS, MSP_RC, MSP_MOTOR_PINS, MSP_ATTITUDE, MSP_SERVO }; #define MSP_TELEMETRY_COMMAND_SEQUENCE_ENTRY_COUNT (sizeof(mspTelemetryCommandSequence) / sizeof(mspTelemetryCommandSequence[0])) static mspPort_t *mspTelemetryPort = NULL; void mspSetTelemetryPort(serialPort_t *serialPort) { uint8_t portIndex; mspPort_t *candidatePort = NULL; mspPort_t *matchedPort = NULL; // find existing telemetry port for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { candidatePort = &mspPorts[portIndex]; if (candidatePort->mspPortUsage == FOR_TELEMETRY) { matchedPort = candidatePort; break; } } if (!matchedPort) { // find unused port for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { candidatePort = &mspPorts[portIndex]; if (candidatePort->mspPortUsage == UNUSED_PORT) { matchedPort = candidatePort; break; } } } mspTelemetryPort = matchedPort; if (!mspTelemetryPort) { return; } resetMspPort(mspTelemetryPort, serialPort, FOR_TELEMETRY); } void sendMspTelemetry(void) { static uint32_t sequenceIndex = 0; if (!mspTelemetryPort) { return; } setCurrentPort(mspTelemetryPort); processOutCommand(mspTelemetryCommandSequence[sequenceIndex]); tailSerialReply(); sequenceIndex++; if (sequenceIndex >= MSP_TELEMETRY_COMMAND_SEQUENCE_ENTRY_COUNT) { sequenceIndex = 0; } }