diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 35d43d2243..d6003345c1 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -23,8 +23,8 @@ #include "build_config.h" #include "debug.h" - #include "platform.h" +#include "scheduler.h" #include "common/axis.h" #include "common/color.h" @@ -55,6 +55,7 @@ #include "io/serial.h" #include "io/ledstrip.h" #include "io/flashfs.h" +#include "io/transponder_ir.h" #include "io/asyncfatfs/asyncfatfs.h" #include "telemetry/telemetry.h" @@ -98,236 +99,11 @@ static serialPort_t *mspSerialPort; extern uint16_t cycleTime; // FIXME dependency on mw.c extern uint16_t rssi; // FIXME dependency on mw.c +extern void resetPidProfile(pidProfile_t *pidProfile); void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse); -/** - * 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 returned 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 13 // 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 BETAFLIGHT_IDENTIFIER "BTFL" -#define BASEFLIGHT_IDENTIFIER "BAFL"; - -#define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4 -static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller. - -#define FLIGHT_CONTROLLER_VERSION_LENGTH 3 -#define FLIGHT_CONTROLLER_VERSION_MASK 0xFFF - -static const char * const 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) - -#define MSP_API_VERSION 1 //out message -#define MSP_FC_VARIANT 2 //out message -#define MSP_FC_VERSION 3 //out message -#define MSP_BOARD_INFO 4 //out message -#define MSP_BUILD_INFO 5 //out message - -// -// MSP commands for Cleanflight original features -// -#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 - -// private - only to be used by the configurator, the commands are likely to change -#define MSP_CF_SERIAL_CONFIG 54 -#define MSP_SET_CF_SERIAL_CONFIG 55 - -#define MSP_VOLTAGE_METER_CONFIG 56 -#define MSP_SET_VOLTAGE_METER_CONFIG 57 - -#define MSP_SONAR_ALTITUDE 58 //out message get sonar altitude [cm] - -#define MSP_PID_CONTROLLER 59 -#define MSP_SET_PID_CONTROLLER 60 - -#define MSP_ARMING_CONFIG 61 //out message Returns auto_disarm_delay and disarm_kill_switch parameters -#define MSP_SET_ARMING_CONFIG 62 //in message Sets auto_disarm_delay and disarm_kill_switch parameters - -#define MSP_DATAFLASH_SUMMARY 70 //out message - get description of dataflash chip -#define MSP_DATAFLASH_READ 71 //out message - get content of dataflash chip -#define MSP_DATAFLASH_ERASE 72 //in message - erase dataflash chip - -#define MSP_LOOP_TIME 73 //out message Returns FC cycle time i.e looptime parameter -#define MSP_SET_LOOP_TIME 74 //in message Sets FC cycle time i.e looptime parameter - -#define MSP_FAILSAFE_CONFIG 75 //out message Returns FC Fail-Safe settings -#define MSP_SET_FAILSAFE_CONFIG 76 //in message Sets FC Fail-Safe settings - -#define MSP_RXFAIL_CONFIG 77 //out message Returns RXFAIL settings -#define MSP_SET_RXFAIL_CONFIG 78 //in message Sets RXFAIL settings - -#define MSP_SDCARD_SUMMARY 79 //out message Get the state of the SD card - -#define MSP_BLACKBOX_CONFIG 80 //out message Get blackbox settings -#define MSP_SET_BLACKBOX_CONFIG 81 //in message Set blackbox settings - -// -// 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_BF_CONFIG" and MSP_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead. -#define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere -#define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save - -#define MSP_REBOOT 68 //in message reboot settings - -// DEPRECATED - Use MSP_BUILD_INFO instead -#define MSP_BF_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 mixerMode + 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 servos -#define MSP_MOTOR 104 //out message motors -#define MSP_RC 105 //out message rc channels 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_CONFIGURATIONS 120 //out message All servo configurations. -#define MSP_NAV_STATUS 121 //out message Returns navigation status -#define MSP_NAV_CONFIG 122 //out message Returns navigation parameters -#define MSP_PID_FLOAT 123 //out message P I D Used for Luxfloat - -#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, yaw expo -#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_CONFIGURATION 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_SET_PID_FLOAT 216 //in message P I D used for luxfloat - -// #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 MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration -#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration -#define MSP_SET_1WIRE 243 //in message Sets 1Wire paththrough - -#define INBUF_SIZE 64 +const char * const flightControllerIdentifier = CLEANFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller. typedef struct box_e { const uint8_t boxId; // see boxId_e @@ -392,16 +168,6 @@ static const char pidnames[] = "MAG;" "VEL;"; -typedef enum { - IDLE, - HEADER_START, - HEADER_M, - HEADER_ARROW, - HEADER_SIZE, - HEADER_CMD, - COMMAND_RECEIVED -} mspState_e; - typedef enum { MSP_SDCARD_STATE_NOT_PRESENT = 0, MSP_SDCARD_STATE_FATAL = 1, @@ -410,28 +176,11 @@ typedef enum { MSP_SDCARD_STATE_READY = 4 } mspSDCardState_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_UNIT_TESTED mspPort_t mspPorts[MAX_MSP_PORT_COUNT]; -static mspPort_t mspPorts[MAX_MSP_PORT_COUNT]; - -static mspPort_t *currentPort; -static bufWriter_t *writer; +STATIC_UNIT_TESTED mspPort_t *currentPort; +STATIC_UNIT_TESTED bufWriter_t *writer; static void serialize8(uint8_t a) { @@ -660,12 +409,11 @@ static void serializeDataflashReadReply(uint32_t address, uint8_t size) } #endif -static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort, mspPortUsage_e usage) +static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort) { memset(mspPortToReset, 0, sizeof(mspPort_t)); mspPortToReset->port = serialPort; - mspPortToReset->mspPortUsage = usage; } void mspAllocateSerialPorts(serialConfig_t *serialConfig) @@ -680,14 +428,14 @@ void mspAllocateSerialPorts(serialConfig_t *serialConfig) while (portConfig && portIndex < MAX_MSP_PORT_COUNT) { mspPort_t *mspPort = &mspPorts[portIndex]; - if (mspPort->mspPortUsage != UNUSED_PORT) { + if (mspPort->port) { portIndex++; continue; } serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); if (serialPort) { - resetMspPort(mspPort, serialPort, FOR_GENERAL_MSP); + resetMspPort(mspPort, serialPort); portIndex++; } @@ -723,6 +471,7 @@ void mspInit(serialConfig_t *serialConfig) activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; activeBoxIds[activeBoxIdCount++] = BOXACROPLUS; + if (sensors(SENSOR_BARO)) { activeBoxIds[activeBoxIdCount++] = BOXBARO; } @@ -794,9 +543,53 @@ void mspInit(serialConfig_t *serialConfig) #define IS_ENABLED(mask) (mask == 0 ? 0 : 1) +static uint32_t packFlightModeFlags(void) +{ + uint32_t i, junk, tmp; + + // 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(BOXLEDLOW)) << BOXLEDLOW | + 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(BOXGTUNE)) << BOXGTUNE | + IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR | + IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | + IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXACROPLUS)) << BOXACROPLUS; + + for (i = 0; i < activeBoxIdCount; i++) { + int flag = (tmp & (1 << activeBoxIds[i])); + if (flag) + junk |= 1 << i; + } + + return junk; +} + static bool processOutCommand(uint8_t cmdMSP) { - uint32_t i, tmp, junk; + uint32_t i; #ifdef GPS uint8_t wp_no; @@ -839,7 +632,7 @@ static bool processOutCommand(uint8_t cmdMSP) for (i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) { serialize8(boardIdentifier[i]); } -#ifdef USE_HARDWARE_REVISION_DETECTION +#ifdef NAZE serialize16(hardwareRevision); #else serialize16(0); // No other build targets currently have hardware revision detection. @@ -874,6 +667,20 @@ static bool processOutCommand(uint8_t cmdMSP) serialize32(CAP_DYNBALANCE); // "capability" break; + case MSP_STATUS_EX: + headSerialReply(12); + 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); + serialize32(packFlightModeFlags()); + serialize8(masterConfig.current_profile_index); + //serialize16(averageSystemLoadPercent); + break; + case MSP_STATUS: headSerialReply(11); serialize16(cycleTime); @@ -883,41 +690,7 @@ static bool processOutCommand(uint8_t cmdMSP) 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(BOXLEDLOW)) << BOXLEDLOW | - 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(BOXGTUNE)) << BOXGTUNE | - IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR | - IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | - IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE; - for (i = 0; i < activeBoxIdCount; i++) { - int flag = (tmp & (1 << activeBoxIds[i])); - if (flag) - junk |= 1 << i; - } - serialize32(junk); + serialize32(packFlightModeFlags()); serialize8(masterConfig.current_profile_index); break; case MSP_RAW_IMU: @@ -1053,26 +826,6 @@ static bool processOutCommand(uint8_t cmdMSP) } } break; - case MSP_PID_FLOAT: - headSerialReply(3 * PID_ITEM_COUNT * 2); - for (i = 0; i < 3; i++) { - serialize16(lrintf(currentProfile->pidProfile.P_f[i] * 1000.0f)); - serialize16(lrintf(currentProfile->pidProfile.I_f[i] * 1000.0f)); - serialize16(lrintf(currentProfile->pidProfile.D_f[i] * 1000.0f)); - } - for (i = 3; i < PID_ITEM_COUNT; i++) { - if (i == PIDLEVEL) { - serialize16(lrintf(currentProfile->pidProfile.A_level * 1000.0f)); - serialize16(lrintf(currentProfile->pidProfile.H_level * 1000.0f)); - serialize16(currentProfile->pidProfile.H_sensitivity); - } - else { - serialize16(currentProfile->pidProfile.P8[i]); - serialize16(currentProfile->pidProfile.I8[i]); - serialize16(currentProfile->pidProfile.D8[i]); - } - } - break; case MSP_PIDNAMES: headSerialReply(sizeof(pidnames) - 1); serializeNames(pidnames); @@ -1276,10 +1029,13 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_FAILSAFE_CONFIG: - headSerialReply(4); + headSerialReply(8); serialize8(masterConfig.failsafeConfig.failsafe_delay); serialize8(masterConfig.failsafeConfig.failsafe_off_delay); serialize16(masterConfig.failsafeConfig.failsafe_throttle); + serialize8(masterConfig.failsafeConfig.failsafe_kill_switch); + serialize16(masterConfig.failsafeConfig.failsafe_throttle_low_delay); + serialize8(1); break; case MSP_RXFAIL_CONFIG: @@ -1392,6 +1148,21 @@ static bool processOutCommand(uint8_t cmdMSP) serializeSDCardSummaryReply(); break; + case MSP_TRANSPONDER_CONFIG: +#ifdef TRANSPONDER + headSerialReply(1 + sizeof(masterConfig.transponderData)); + + serialize8(1); //Transponder supported + + for (i = 0; i < sizeof(masterConfig.transponderData); i++) { + serialize8(masterConfig.transponderData[i]); + } +#else + headSerialReply(1); + serialize8(0); // Transponder not supported +#endif + break; + case MSP_BF_BUILD_INFO: headSerialReply(11 + 4 + 4); for (i = 0; i < 11; i++) @@ -1400,6 +1171,27 @@ static bool processOutCommand(uint8_t cmdMSP) serialize32(0); // future exp break; + case MSP_3D: + headSerialReply(2 * 4); + serialize16(masterConfig.flight3DConfig.deadband3d_low); + serialize16(masterConfig.flight3DConfig.deadband3d_high); + serialize16(masterConfig.flight3DConfig.neutral3d); + serialize16(masterConfig.flight3DConfig.deadband3d_throttle); + break; + + case MSP_RC_DEADBAND: + headSerialReply(3); + serialize8(currentProfile->rcControlsConfig.deadband); + serialize8(currentProfile->rcControlsConfig.yaw_deadband); + serialize8(currentProfile->rcControlsConfig.alt_hold_deadband); + break; + case MSP_SENSOR_ALIGNMENT: + headSerialReply(3); + serialize8(masterConfig.sensorAlignmentConfig.gyro_align); + serialize8(masterConfig.sensorAlignmentConfig.acc_align); + serialize8(masterConfig.sensorAlignmentConfig.mag_align); + break; + default: return false; } @@ -1455,9 +1247,10 @@ static bool processInCommand(void) masterConfig.disarm_kill_switch = read8(); break; case MSP_SET_LOOP_TIME: + read16(); break; case MSP_SET_PID_CONTROLLER: - currentProfile->pidProfile.pidController = constrain(read8(), 1, 2); // Temporary configurator compatibility + currentProfile->pidProfile.pidController = constrain(read8(), 1, 2); pidSetController(currentProfile->pidProfile.pidController); break; case MSP_SET_PID: @@ -1486,25 +1279,6 @@ static bool processInCommand(void) } } break; - case MSP_SET_PID_FLOAT: - for (i = 0; i < 3; i++) { - currentProfile->pidProfile.P_f[i] = (float)read16() / 1000.0f; - currentProfile->pidProfile.I_f[i] = (float)read16() / 1000.0f; - currentProfile->pidProfile.D_f[i] = (float)read16() / 1000.0f; - } - for (i = 3; i < PID_ITEM_COUNT; i++) { - if (i == PIDLEVEL) { - currentProfile->pidProfile.A_level = (float)read16() / 1000.0f; - currentProfile->pidProfile.H_level = (float)read16() / 1000.0f; - currentProfile->pidProfile.H_sensitivity = read16(); - } - else { - currentProfile->pidProfile.P8[i] = read16(); - currentProfile->pidProfile.I8[i] = read16(); - currentProfile->pidProfile.D8[i] = read16(); - } - } - break; case MSP_SET_MODE_RANGE: i = read8(); if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) { @@ -1640,6 +1414,29 @@ static bool processInCommand(void) #endif break; + case MSP_SET_3D: + masterConfig.flight3DConfig.deadband3d_low = read16(); + masterConfig.flight3DConfig.deadband3d_high = read16(); + masterConfig.flight3DConfig.neutral3d = read16(); + masterConfig.flight3DConfig.deadband3d_throttle = read16(); + break; + + case MSP_SET_RC_DEADBAND: + currentProfile->rcControlsConfig.deadband = read8(); + currentProfile->rcControlsConfig.yaw_deadband = read8(); + currentProfile->rcControlsConfig.alt_hold_deadband = read8(); + break; + + case MSP_SET_RESET_CURR_PID: + //resetPidProfile(¤tProfile->pidProfile); + break; + + case MSP_SET_SENSOR_ALIGNMENT: + masterConfig.sensorAlignmentConfig.gyro_align = read8(); + masterConfig.sensorAlignmentConfig.acc_align = read8(); + masterConfig.sensorAlignmentConfig.mag_align = read8(); + break; + case MSP_RESET_CONF: if (!ARMING_FLAG(ARMED)) { resetEEPROM(); @@ -1674,6 +1471,21 @@ static bool processInCommand(void) break; #endif +#ifdef TRANSPONDER + case MSP_SET_TRANSPONDER_CONFIG: + if (currentPort->dataSize != sizeof(masterConfig.transponderData)) { + headSerialError(0); + break; + } + + for (i = 0; i < sizeof(masterConfig.transponderData); i++) { + masterConfig.transponderData[i] = read8(); + } + + transponderUpdateData(masterConfig.transponderData); + break; +#endif + #ifdef USE_FLASHFS case MSP_DATAFLASH_ERASE: flashfsEraseCompletely(); @@ -1766,19 +1578,19 @@ static bool processInCommand(void) masterConfig.failsafeConfig.failsafe_delay = read8(); masterConfig.failsafeConfig.failsafe_off_delay = read8(); masterConfig.failsafeConfig.failsafe_throttle = read16(); + masterConfig.failsafeConfig.failsafe_kill_switch = read8(); + masterConfig.failsafeConfig.failsafe_throttle_low_delay = read16(); + //masterConfig.failsafeConfig.failsafe_procedure = read8(); + read8(); break; case MSP_SET_RXFAIL_CONFIG: - { - uint8_t channelCount = currentPort->dataSize / 3; - if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) { - headSerialError(0); - } else { - for (i = 0; i < channelCount; i++) { - masterConfig.rxConfig.failsafe_channel_configurations[i].mode = read8(); - masterConfig.rxConfig.failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(read16()); - } - } + i = read8(); + if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) { + masterConfig.rxConfig.failsafe_channel_configurations[i].mode = read8(); + masterConfig.rxConfig.failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(read16()); + } else { + headSerialError(0); } break; @@ -1958,7 +1770,7 @@ static bool processInCommand(void) return true; } -static void mspProcessReceivedCommand() { +STATIC_UNIT_TESTED void mspProcessReceivedCommand() { if (!(processOutCommand(currentPort->cmdMSP) || processInCommand())) { headSerialError(0); } @@ -1979,7 +1791,7 @@ static bool mspProcessReceivedData(uint8_t c) } 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) { + if (c > MSP_PORT_INBUF_SIZE) { currentPort->c_state = IDLE; } else { @@ -2007,7 +1819,7 @@ static bool mspProcessReceivedData(uint8_t c) return true; } -static void setCurrentPort(mspPort_t *port) +STATIC_UNIT_TESTED void setCurrentPort(mspPort_t *port) { currentPort = port; mspSerialPort = currentPort->port; @@ -2020,7 +1832,7 @@ void mspProcess(void) for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { candidatePort = &mspPorts[portIndex]; - if (candidatePort->mspPortUsage != FOR_GENERAL_MSP) { + if (!candidatePort->port) { continue; } @@ -2028,7 +1840,7 @@ void mspProcess(void) // Big enough to fit a MSP_STATUS in one write. uint8_t buf[sizeof(bufWriter_t) + 20]; writer = bufWriterInit(buf, sizeof(buf), - (bufWrite_t)serialWriteBufShim, currentPort->port); + (bufWrite_t)serialWriteBufShim, currentPort->port); while (serialRxBytesWaiting(mspSerialPort)) { @@ -2055,77 +1867,3 @@ void mspProcess(void) } } } - -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 TELEMETRY_MSP_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); - uint8_t buf[sizeof(bufWriter_t) + 16]; - writer = bufWriterInit(buf, sizeof(buf), - (bufWrite_t)serialWriteBufShim, currentPort->port); - - processOutCommand(mspTelemetryCommandSequence[sequenceIndex]); - tailSerialReply(); - - bufWriterFlush(writer); - - sequenceIndex++; - if (sequenceIndex >= TELEMETRY_MSP_COMMAND_SEQUENCE_ENTRY_COUNT) { - sequenceIndex = 0; - } -} diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index fd26e9981e..d2db79afb5 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -20,13 +20,269 @@ #include "io/serial.h" #include "drivers/serial.h" +/** + * 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 returned 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 client 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 16 // 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 BETAFLIGHT_IDENTIFIER "BTFL" +#define BASEFLIGHT_IDENTIFIER "BAFL"; + +#define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4 +extern const char * const flightControllerIdentifier; + +#define FLIGHT_CONTROLLER_VERSION_LENGTH 3 +#define FLIGHT_CONTROLLER_VERSION_MASK 0xFFF + +static const char * const 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) + +#define MSP_API_VERSION 1 //out message +#define MSP_FC_VARIANT 2 //out message +#define MSP_FC_VERSION 3 //out message +#define MSP_BOARD_INFO 4 //out message +#define MSP_BUILD_INFO 5 //out message + +// +// MSP commands for Cleanflight original features +// +#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 + +// private - only to be used by the configurator, the commands are likely to change +#define MSP_CF_SERIAL_CONFIG 54 +#define MSP_SET_CF_SERIAL_CONFIG 55 + +#define MSP_VOLTAGE_METER_CONFIG 56 +#define MSP_SET_VOLTAGE_METER_CONFIG 57 + +#define MSP_SONAR_ALTITUDE 58 //out message get sonar altitude [cm] + +#define MSP_PID_CONTROLLER 59 +#define MSP_SET_PID_CONTROLLER 60 + +#define MSP_ARMING_CONFIG 61 //out message Returns auto_disarm_delay and disarm_kill_switch parameters +#define MSP_SET_ARMING_CONFIG 62 //in message Sets auto_disarm_delay and disarm_kill_switch parameters + +#define MSP_DATAFLASH_SUMMARY 70 //out message - get description of dataflash chip +#define MSP_DATAFLASH_READ 71 //out message - get content of dataflash chip +#define MSP_DATAFLASH_ERASE 72 //in message - erase dataflash chip + +#define MSP_LOOP_TIME 73 //out message Returns FC cycle time i.e looptime parameter +#define MSP_SET_LOOP_TIME 74 //in message Sets FC cycle time i.e looptime parameter + +#define MSP_FAILSAFE_CONFIG 75 //out message Returns FC Fail-Safe settings +#define MSP_SET_FAILSAFE_CONFIG 76 //in message Sets FC Fail-Safe settings + +#define MSP_RXFAIL_CONFIG 77 //out message Returns RXFAIL settings +#define MSP_SET_RXFAIL_CONFIG 78 //in message Sets RXFAIL settings + +#define MSP_SDCARD_SUMMARY 79 //out message Get the state of the SD card + +#define MSP_BLACKBOX_CONFIG 80 //out message Get blackbox settings +#define MSP_SET_BLACKBOX_CONFIG 81 //in message Set blackbox settings + +#define MSP_TRANSPONDER_CONFIG 82 //in message Get transponder settings +#define MSP_SET_TRANSPONDER_CONFIG 83 //out message Set transponder settings + +// +// 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_BF_CONFIG" and MSP_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead. +#define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere +#define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save + +#define MSP_REBOOT 68 //in message reboot settings + +// DEPRECATED - Use MSP_BUILD_INFO instead +#define MSP_BF_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 mixerMode + 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 servos +#define MSP_MOTOR 104 //out message motors +#define MSP_RC 105 //out message rc channels 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_CONFIGURATIONS 120 //out message All servo configurations. +#define MSP_NAV_STATUS 121 //out message Returns navigation status +#define MSP_NAV_CONFIG 122 //out message Returns navigation parameters +#define MSP_3D 124 //out message Settings needed for reversible ESCs +#define MSP_RC_DEADBAND 125 //out message deadbands for yaw alt pitch roll +#define MSP_SENSOR_ALIGNMENT 126 //out message orientation of acc,gyro,mag + +#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, yaw expo +#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_CONFIGURATION 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_SET_3D 217 //in message Settings needed for reversible ESCs +#define MSP_SET_RC_DEADBAND 218 //in message deadbands for yaw alt pitch roll +#define MSP_SET_RESET_CURR_PID 219 //in message resetting the current pid profile to defaults +#define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag + +// #define MSP_BIND 240 //in message no param +// #define MSP_ALARMS 242 + +#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_STATUS_EX 150 //out message cycletime, errors_count, CPU load, sensor present etc +#define MSP_UID 160 //out message Unique device ID +#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox) +#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_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration +#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration +#define MSP_SET_1WIRE 243 //in message Sets 1Wire paththrough + // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports. #define MAX_MSP_PORT_COUNT 2 -void mspInit(serialConfig_t *serialConfig); +typedef enum { + IDLE, + HEADER_START, + HEADER_M, + HEADER_ARROW, + HEADER_SIZE, + HEADER_CMD, + COMMAND_RECEIVED +} mspState_e; +#define MSP_PORT_INBUF_SIZE 64 + +typedef struct mspPort_s { + serialPort_t *port; // null when port unused. + uint8_t offset; + uint8_t dataSize; + uint8_t checksum; + uint8_t indRX; + uint8_t inBuf[MSP_PORT_INBUF_SIZE]; + mspState_e c_state; + uint8_t cmdMSP; +} mspPort_t; + +void mspInit(serialConfig_t *serialConfig); void mspProcess(void); -void sendMspTelemetry(void); -void mspSetTelemetryPort(serialPort_t *mspTelemetryPort); void mspAllocateSerialPorts(serialConfig_t *serialConfig); void mspReleasePortIfAllocated(serialPort_t *serialPort); diff --git a/src/main/telemetry/msp.c b/src/main/telemetry/msp.c index c15093d7d7..7ba96884de 100644 --- a/src/main/telemetry/msp.c +++ b/src/main/telemetry/msp.c @@ -79,7 +79,7 @@ void handleMSPTelemetry(void) return; } - sendMspTelemetry(); + //sendMspTelemetry(); TODO - Cleanup / fix } void freeMSPTelemetryPort(void) @@ -106,7 +106,7 @@ void configureMSPTelemetryPort(void) if (!mspTelemetryPort) { return; } - mspSetTelemetryPort(mspTelemetryPort); + //mspSetTelemetryPort(mspTelemetryPort); TODO - Cleanup / Fix mspTelemetryEnabled = true; }