From f4ff951813a2c2dc6301927a78422abc6fe9fb92 Mon Sep 17 00:00:00 2001 From: Hydra Date: Sun, 19 Mar 2017 15:23:37 +0000 Subject: [PATCH 1/9] CF/BF - delete some unused code from BST. --- src/main/target/COLIBRI_RACE/i2c_bst.c | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index eab7e6747..8f8df5611 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -248,10 +248,10 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define BST_RESET_CONF 208 //in message no param #define BST_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags) #define BST_SELECT_SETTING 210 //in message Select Setting Number (0-2) -#define BST_SET_HEAD 211 //in message define a new heading hold direction +//#define BST_SET_HEAD 211 //in message define a new heading hold direction // unused #define BST_SET_SERVO_CONFIGURATION 212 //in message Servo settings #define BST_SET_MOTOR 214 //in message PropBalance function -#define BST_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom +//#define BST_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom // unused // #define BST_BIND 240 //in message no param @@ -1008,9 +1008,6 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) changePidProfile(bstRead8()); } break; - case BST_SET_HEAD: - magHold = bstRead16(); - break; case BST_SET_RAW_RC: { uint8_t channelCount = bstReadDataSize() / sizeof(uint16_t); From e7cbfcc8c054e62b6f21b2573cdeee360f6121e0 Mon Sep 17 00:00:00 2001 From: Hydra Date: Sun, 19 Mar 2017 15:25:05 +0000 Subject: [PATCH 2/9] CF/BF - Delete BST_SET_RAW_RC - it was missing a break statement and thus was clearly unused. --- src/main/target/COLIBRI_RACE/i2c_bst.c | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 8f8df5611..578308d6b 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -1008,21 +1008,6 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) changePidProfile(bstRead8()); } break; - case BST_SET_RAW_RC: - { - uint8_t channelCount = bstReadDataSize() / sizeof(uint16_t); - if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) { - ret = BST_FAILED; - } else { - uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT]; - - for (i = 0; i < channelCount; i++) { - frame[i] = bstRead16(); - } - - rxMspFrameReceive(frame, channelCount); - } - } case BST_SET_ACC_TRIM: accelerometerConfigMutable()->accelerometerTrims.values.pitch = bstRead16(); accelerometerConfigMutable()->accelerometerTrims.values.roll = bstRead16(); From e9aaa4387e88bb69ec259ef22171c918c1e89bcd Mon Sep 17 00:00:00 2001 From: Hydra Date: Sun, 19 Mar 2017 15:28:13 +0000 Subject: [PATCH 3/9] CF/BF - Remove RX_MSP from Colibri race target. --- src/main/target/COLIBRI_RACE/target.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index d2701c17d..b09b63efd 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -25,6 +25,8 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT +#undef USE_RX_MSP // never used. + #define LED0 PC15 #define LED1 PC14 #define LED2 PC13 From 5d602d69ff96ab892f47438f275cd6f3a506046c Mon Sep 17 00:00:00 2001 From: Hydra Date: Sun, 19 Mar 2017 18:42:06 +0000 Subject: [PATCH 4/9] CF/BF - Delete old/unused MSP commands. Cleaning MSP naming a little. The commands that are deleted have been deprecated for a LONG time. They have long been superceed with more specific commands. Fix voltage meter config to include id and type in the subframe. --- src/main/fc/fc_core.c | 8 ++ src/main/fc/fc_core.h | 3 + src/main/fc/fc_msp.c | 154 ++++++++---------------------------- src/main/msp/msp_protocol.h | 52 +++++++----- 4 files changed, 77 insertions(+), 140 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 2ecfa88d2..b7765226e 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -94,7 +94,10 @@ enum { #define AIRMODE_THOTTLE_THRESHOLD 1350 // Make configurable in the future. ~35% throttle should be fine +#if defined(GPS) || defined(MAG) int16_t magHold; +#endif + int16_t headFreeModeHold; uint8_t motorControlEnable = false; @@ -261,6 +264,7 @@ static void updateInflightCalibrationState(void) } } +#if defined(GPS) || defined(MAG) void updateMagHold(void) { if (ABS(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) { @@ -275,6 +279,8 @@ void updateMagHold(void) } else magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); } +#endif + void processRx(timeUs_t currentTimeUs) { @@ -411,6 +417,7 @@ void processRx(timeUs_t currentTimeUs) #if defined(ACC) || defined(MAG) if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { +#if defined(GPS) || defined(MAG) if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { ENABLE_FLIGHT_MODE(MAG_MODE); @@ -419,6 +426,7 @@ void processRx(timeUs_t currentTimeUs) } else { DISABLE_FLIGHT_MODE(MAG_MODE); } +#endif if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index e96621b6d..e242886a1 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -20,7 +20,10 @@ #include "common/time.h" #include "config/parameter_group.h" +#if defined(GPS) || defined(MAG) extern int16_t magHold; +#endif + extern bool isRXDataNew; extern int16_t headFreeModeHold; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 2cd9eeeff..6559238dd 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -596,14 +596,6 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteData(dst, shortGitRevision, GIT_SHORT_REVISION_LENGTH); break; - // DEPRECATED - Use MSP_API_VERSION - case MSP_IDENT: - sbufWriteU8(dst, MW_VERSION); - sbufWriteU8(dst, mixerConfig()->mixerMode); - sbufWriteU8(dst, MSP_PROTOCOL_VERSION); - sbufWriteU32(dst, CAP_DYNBALANCE); // "capability" - break; - case MSP_STATUS_EX: sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID)); #ifdef USE_I2C @@ -739,10 +731,6 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, armingConfig()->disarm_kill_switch); break; - case MSP_LOOP_TIME: - sbufWriteU16(dst, (uint16_t)gyro.targetLooptime); - break; - case MSP_RC_TUNING: sbufWriteU8(dst, currentControlRateProfile->rcRate8); sbufWriteU8(dst, currentControlRateProfile->rcExpo8); @@ -812,44 +800,24 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn } break; - case MSP_MISC: - sbufWriteU16(dst, rxConfig()->midrc); - + case MSP_MOTOR_CONFIG: sbufWriteU16(dst, motorConfig()->minthrottle); sbufWriteU16(dst, motorConfig()->maxthrottle); sbufWriteU16(dst, motorConfig()->mincommand); + break; - sbufWriteU16(dst, failsafeConfig()->failsafe_throttle); - -#ifdef GPS - sbufWriteU8(dst, gpsConfig()->provider); // gps_type - sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t - sbufWriteU8(dst, gpsConfig()->sbasMode); // gps_ubx_sbas -#else - sbufWriteU8(dst, 0); // gps_type - sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t - sbufWriteU8(dst, 0); // gps_ubx_sbas -#endif - sbufWriteU8(dst, 0); // was multiwiiCurrentMeterOutput - sbufWriteU8(dst, rxConfig()->rssi_channel); - sbufWriteU8(dst, 0); - +#ifdef MAG + case MSP_COMPASS_CONFIG: sbufWriteU16(dst, compassConfig()->mag_declination / 10); - - sbufWriteU8(dst, 0); // was vbatscale - sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage); - sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage); - sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage); - break; - - case MSP_MOTOR_PINS: - // FIXME This is hardcoded and should not be. - for (int i = 0; i < 8; i++) { - sbufWriteU8(dst, i + 1); - } break; +#endif #ifdef GPS + case MSP_GPS_CONFIG: + sbufWriteU8(dst, gpsConfig()->provider); // gps_type + sbufWriteU8(dst, gpsConfig()->sbasMode); // gps_ubx_sbas + break; + case MSP_RAW_GPS: sbufWriteU8(dst, STATE(GPS_FIX)); sbufWriteU8(dst, GPS_numSat); @@ -886,7 +854,6 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn } break; - // Additional commands that are not compatible with MultiWii case MSP_ACC_TRIM: sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.pitch); sbufWriteU16(dst, accelerometerConfig()->accelerometerTrims.values.roll); @@ -898,7 +865,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU32(dst, U_ID_2); break; - case MSP_FEATURE: + case MSP_FEATURE_CONFIG: sbufWriteU32(dst, featureMask()); break; @@ -1002,7 +969,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, batteryConfig()->currentMeterSource); break; - case MSP_MIXER: + case MSP_MIXER_CONFIG: sbufWriteU8(dst, mixerConfig()->mixerMode); break; @@ -1047,21 +1014,6 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteData(dst, rxConfig()->rcmap, MAX_MAPPABLE_RX_INPUTS); break; - case MSP_BF_CONFIG: - sbufWriteU8(dst, mixerConfig()->mixerMode); - - sbufWriteU32(dst, featureMask()); - - sbufWriteU8(dst, rxConfig()->serialrx_provider); - - sbufWriteU16(dst, boardAlignment()->rollDegrees); - sbufWriteU16(dst, boardAlignment()->pitchDegrees); - sbufWriteU16(dst, boardAlignment()->yawDegrees); - - sbufWriteU16(dst, 0); // was currentMeterScale, see MSP_CURRENT_METER_CONFIG - sbufWriteU16(dst, 0); //was currentMeterOffset, see MSP_CURRENT_METER_CONFIG - break; - case MSP_CF_SERIAL_CONFIG: for (int i = 0; i < SERIAL_PORT_COUNT; i++) { if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) { @@ -1169,12 +1121,6 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #endif break; - case MSP_BF_BUILD_INFO: - sbufWriteData(dst, buildDate, 11); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc - sbufWriteU32(dst, 0); // future exp - sbufWriteU32(dst, 0); // future exp - break; - case MSP_3D: sbufWriteU16(dst, flight3DConfig()->deadband3d_low); sbufWriteU16(dst, flight3DConfig()->deadband3d_high); @@ -1359,9 +1305,11 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) } break; - case MSP_SET_HEAD: +#if defined(GPS) || defined(MAG) + case MSP_SET_HEADING: magHold = sbufReadU16(src); break; +#endif case MSP_SET_RAW_RC: #ifdef USE_RX_MSP @@ -1388,10 +1336,6 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) armingConfigMutable()->disarm_kill_switch = sbufReadU8(src); break; - case MSP_SET_LOOP_TIME: - sbufReadU16(src); - break; - case MSP_SET_PID_CONTROLLER: break; @@ -1470,37 +1414,27 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) } break; - case MSP_SET_MISC: - rxConfigMutable()->midrc = sbufReadU16(src); + case MSP_SET_MOTOR_CONFIG: motorConfigMutable()->minthrottle = sbufReadU16(src); motorConfigMutable()->maxthrottle = sbufReadU16(src); motorConfigMutable()->mincommand = sbufReadU16(src); - - failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src); - -#ifdef GPS - gpsConfigMutable()->provider = sbufReadU8(src); // gps_type - sbufReadU8(src); // gps_baudrate - gpsConfigMutable()->sbasMode = sbufReadU8(src); // gps_ubx_sbas -#else - sbufReadU8(src); // gps_type - sbufReadU8(src); // gps_baudrate - sbufReadU8(src); // gps_ubx_sbas -#endif - sbufReadU8(src); // legacy - was multiwiiCurrentMeterOutput - rxConfigMutable()->rssi_channel = sbufReadU8(src); - sbufReadU8(src); - - compassConfigMutable()->mag_declination = sbufReadU16(src) * 10; - - sbufReadU8(src); // legacy - was vbatscale - batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI - batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI - batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert break; +#ifdef GPS + case MSP_SET_GPS_CONFIG: + gpsConfigMutable()->provider = sbufReadU8(src); // gps_type + gpsConfigMutable()->sbasMode = sbufReadU8(src); // gps_ubx_sbas + break; +#endif + +#ifdef MAG + case MSP_SET_COMPASS_CONFIG: + compassConfigMutable()->mag_declination = sbufReadU16(src) * 10; + break; +#endif + case MSP_SET_MOTOR: - for (int i = 0; i < 8; i++) { // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8 + for (int i = 0; i < getMotorCount(); i++) { motor_disarmed[i] = convertExternalToMotor(sbufReadU16(src)); } break; @@ -1821,7 +1755,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) } break; #endif - case MSP_SET_FEATURE: + case MSP_SET_FEATURE_CONFIG: featureClearAll(); featureSet(sbufReadU32(src)); // features bitmap break; @@ -1885,11 +1819,13 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) batteryConfigMutable()->currentMeterSource = sbufReadU8(src); break; + case MSP_SET_MIXER_CONFIG: #ifndef USE_QUAD_MIXER_ONLY - case MSP_SET_MIXER: mixerConfigMutable()->mixerMode = sbufReadU8(src); - break; +#else + sbufReadU8(src); #endif + break; case MSP_SET_RX_CONFIG: rxConfigMutable()->serialrx_provider = sbufReadU8(src); @@ -1945,26 +1881,6 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) } break; - case MSP_SET_BF_CONFIG: -#ifdef USE_QUAD_MIXER_ONLY - sbufReadU8(src); // mixerMode ignored -#else - mixerConfigMutable()->mixerMode = sbufReadU8(src); // mixerMode -#endif - - featureClearAll(); - featureSet(sbufReadU32(src)); // features bitmap - - rxConfigMutable()->serialrx_provider = sbufReadU8(src); // serialrx_type - - boardAlignmentMutable()->rollDegrees = sbufReadU16(src); // board_align_roll - boardAlignmentMutable()->pitchDegrees = sbufReadU16(src); // board_align_pitch - boardAlignmentMutable()->yawDegrees = sbufReadU16(src); // board_align_yaw - - sbufReadU16(src); // was currentMeterScale, see MSP_SET_CURRENT_METER_CONFIG - sbufReadU16(src); // was currentMeterOffset see MSP_SET_CURRENT_METER_CONFIG - break; - case MSP_SET_CF_SERIAL_CONFIG: { uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4); diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 830bf36f4..94321ea7b 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -111,8 +111,8 @@ #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_FEATURE_CONFIG 36 +#define MSP_SET_FEATURE_CONFIG 37 #define MSP_BOARD_ALIGNMENT 38 #define MSP_SET_BOARD_ALIGNMENT 39 @@ -120,8 +120,8 @@ #define MSP_CURRENT_METER_CONFIG 40 #define MSP_SET_CURRENT_METER_CONFIG 41 -#define MSP_MIXER 42 -#define MSP_SET_MIXER 43 +#define MSP_MIXER_CONFIG 42 +#define MSP_SET_MIXER_CONFIG 43 #define MSP_RX_CONFIG 44 #define MSP_SET_RX_CONFIG 45 @@ -159,23 +159,22 @@ #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 +// Use more specific commands, e.g. MSP_FEATURE_CONFIG, MSP_BATTERY_CONFIG, etc. +// DEPRECATED -#define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere +// DEPRECATED -#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 - +// Use MSP_BUILD_INFO instead +// DEPRECATED - #define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion #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 +// No-longer needed +// DEPRECATED - #define MSP_LOOP_TIME 73 //out message Returns FC cycle time i.e looptime parameter // DEPRECATED +// DEPRECATED - #define MSP_SET_LOOP_TIME 74 //in message Sets FC cycle time i.e looptime parameter // DEPRECATED #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 @@ -226,8 +225,8 @@ // 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 +// See MSP_API_VERSION and MSP_MIXER_CONFIG +//DEPRECATED - #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 @@ -242,9 +241,12 @@ #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 +// Legacy Multiicommand that was never used. +//DEPRECATED - #define MSP_BOX 113 //out message BOX setup (number is dependant of your setup) +// Legacy command that was under constant change due to the naming vagueness, avoid at all costs - use more specific commands instead. +//DEPRECATED - #define MSP_MISC 114 //out message powermeter trig +// Legacy Multiicommand that was never used and always wrong +//DEPRECATED - #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 @@ -259,19 +261,24 @@ #define MSP_VOLTAGE_METERS 128 //out message Voltage (per meter) #define MSP_CURRENT_METERS 129 //out message Amperage (per meter) #define MSP_BATTERY_STATE 130 //out message Connected/Disconnected, Voltage, Current Used +#define MSP_MOTOR_CONFIG 131 //out message Motor configuration (min/max throttle, etc) +#define MSP_GPS_CONFIG 132 //out message GPS configuration +#define MSP_COMPASS_CONFIG 133 //out message Compass configuration #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) +// Legacy multiiwii command that was never used. +//DEPRECATED - #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 +// Legacy command that was under constant change due to the naming vagueness, avoid at all costs - use more specific commands instead. +//DEPRECATED - #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_HEADING 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 @@ -280,6 +287,9 @@ #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_SET_LED_STRIP_MODECOLOR 221 //in message Set LED strip mode_color settings +#define MSP_SET_MOTOR_CONFIG 222 //out message Motor configuration (min/max throttle, etc) +#define MSP_SET_GPS_CONFIG 223 //out message GPS configuration +#define MSP_SET_COMPASS_CONFIG 224 //out message Compass configuration // #define MSP_BIND 240 //in message no param // #define MSP_ALARMS 242 From 601b302c5b744457b0dbac40652664bbdb75d9c2 Mon Sep 17 00:00:00 2001 From: Hydra Date: Sun, 19 Mar 2017 19:08:02 +0000 Subject: [PATCH 5/9] CF/BF - Rename MSP_3D to MSP_MOTOR_3D_CONFIG. now it's really obvious that 3d throttle deadband really doesn't belong in it. If you need 3D specific ESC/MOTOR configuration use this. If yo need specific 3D RX configuration use other commands like MSP_RC_DEADBAND, etc. --- src/main/fc/fc_msp.c | 4 ++-- src/main/msp/msp_protocol.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 6559238dd..b68cc83b7 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1121,7 +1121,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #endif break; - case MSP_3D: + case MSP_MOTOR_3D_CONFIG: sbufWriteU16(dst, flight3DConfig()->deadband3d_low); sbufWriteU16(dst, flight3DConfig()->deadband3d_high); sbufWriteU16(dst, flight3DConfig()->neutral3d); @@ -1478,7 +1478,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #endif break; - case MSP_SET_3D: + case MSP_SET_MOTOR_3D_CONFIG: flight3DConfigMutable()->deadband3d_low = sbufReadU16(src); flight3DConfigMutable()->deadband3d_high = sbufReadU16(src); flight3DConfigMutable()->neutral3d = sbufReadU16(src); diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 94321ea7b..0bf3f2da9 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -254,7 +254,7 @@ #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_MOTOR_3D_CONFIG 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_LED_STRIP_MODECOLOR 127 //out message Get LED strip mode_color settings @@ -282,7 +282,7 @@ #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_MOTOR_3D_CONFIG 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 From 1c33cb1cf2a627af2af943dd4e4541bc36e99e43 Mon Sep 17 00:00:00 2001 From: Hydra Date: Sun, 19 Mar 2017 21:12:33 +0000 Subject: [PATCH 6/9] CF/BF - Rename MSP_BOARD_ALIGNMENT to MSP_BOARD_ALIGNMENT_CONFIG --- src/main/fc/fc_msp.c | 4 ++-- src/main/msp/msp_protocol.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index b68cc83b7..00a3e12c4 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -869,7 +869,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU32(dst, featureMask()); break; - case MSP_BOARD_ALIGNMENT: + case MSP_BOARD_ALIGNMENT_CONFIG: sbufWriteU16(dst, boardAlignment()->rollDegrees); sbufWriteU16(dst, boardAlignment()->pitchDegrees); sbufWriteU16(dst, boardAlignment()->yawDegrees); @@ -1760,7 +1760,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) featureSet(sbufReadU32(src)); // features bitmap break; - case MSP_SET_BOARD_ALIGNMENT: + case MSP_SET_BOARD_ALIGNMENT_CONFIG: boardAlignmentMutable()->rollDegrees = sbufReadU16(src); boardAlignmentMutable()->pitchDegrees = sbufReadU16(src); boardAlignmentMutable()->yawDegrees = sbufReadU16(src); diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 0bf3f2da9..48ff9d2f1 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -114,8 +114,8 @@ #define MSP_FEATURE_CONFIG 36 #define MSP_SET_FEATURE_CONFIG 37 -#define MSP_BOARD_ALIGNMENT 38 -#define MSP_SET_BOARD_ALIGNMENT 39 +#define MSP_BOARD_ALIGNMENT_CONFIG 38 +#define MSP_SET_BOARD_ALIGNMENT_CONFIG 39 #define MSP_CURRENT_METER_CONFIG 40 #define MSP_SET_CURRENT_METER_CONFIG 41 From a60a79eb820d114096dac45541eff9c3abcbcf42 Mon Sep 17 00:00:00 2001 From: Hydra Date: Mon, 20 Mar 2017 18:15:43 +0000 Subject: [PATCH 7/9] CF/BF - Delete unused BST code. "[20/03/2017, 13:46:12] Remo Masina: This is a list of al the frames we currently use: BST_MAG_CALIBRATION, BST_ACC_CALIBRATION, BST_SET_PID, BST_SET_RC_TUNING, BST_SET_LOOP_TIME, BST_SELECT_SETTING, BST_SET_RX_MAP, BST_SET_MISC, BST_SET_DEADBAND, BST_SET_FC_FILTERS, BST_SET_FEATURE, BST_SET_RX_CONFIG, BST_SET_MODE_RANGE, BST_SET_LED_COLORS, BST_EEPROM_WRITE, BST_REBOOT, BST_DISARM, BST_ENABLE_ARM, BST_PID, BST_STATUS, BST_RC_TUNING, BST_LOOP_TIME, BST_RX_MAP, BST_MISC, BST_DEADBAND, BST_FC_FILTERS, BST_FEATURE, BST_RX_CONFIG" See here: https://github.com/cleanflight/cleanflight/pull/2664#issuecomment-287764706 --- src/main/target/COLIBRI_RACE/i2c_bst.c | 741 +------------------------ 1 file changed, 8 insertions(+), 733 deletions(-) diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 578308d6b..e108a5ed3 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -120,8 +120,6 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; // // MSP commands for Cleanflight original features // -#define BST_BATTERY_CONFIG 32 -#define BST_SET_BATTERY_CONFIG 33 #define BST_MODE_RANGES 34 //out message Returns all mode ranges #define BST_SET_MODE_RANGE 35 //in message Sets a single mode range @@ -129,15 +127,6 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define BST_FEATURE 36 #define BST_SET_FEATURE 37 -#define BST_BOARD_ALIGNMENT 38 -#define BST_SET_BOARD_ALIGNMENT 39 - -#define BST_CURRENT_METER_CONFIG 40 -#define BST_SET_CURRENT_METER_CONFIG 41 - -#define BST_MIXER 42 -#define BST_SET_MIXER 43 - #define BST_RX_CONFIG 44 #define BST_SET_RX_CONFIG 45 @@ -147,55 +136,17 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define BST_LED_STRIP_CONFIG 48 #define BST_SET_LED_STRIP_CONFIG 49 -#define BST_RSSI_CONFIG 50 -#define BST_SET_RSSI_CONFIG 51 - -#define BST_ADJUSTMENT_RANGES 52 -#define BST_SET_ADJUSTMENT_RANGE 53 - -// private - only to be used by the configurator, the commands are likely to change -#define BST_CF_SERIAL_CONFIG 54 -#define BST_SET_CF_SERIAL_CONFIG 55 - -#define BST_VOLTAGE_METER_CONFIG 56 -#define BST_SET_VOLTAGE_METER_CONFIG 57 - -#define BST_SONAR_ALTITUDE 58 //out message get sonar altitude [cm] - -#define BST_PID_CONTROLLER 59 -#define BST_SET_PID_CONTROLLER 60 - -#define BST_ARMING_CONFIG 61 //out message Returns auto_disarm_delay and disarm_kill_switch parameters -#define BST_SET_ARMING_CONFIG 62 //in message Sets auto_disarm_delay and disarm_kill_switch parameters - -#define BST_DATAFLASH_SUMMARY 80 //out message - get description of dataflash chip -#define BST_DATAFLASH_READ 81 //out message - get content of dataflash chip -#define BST_DATAFLASH_ERASE 82 //in message - erase dataflash chip - #define BST_LOOP_TIME 83 //out message Returns FC cycle time i.e looptime parameter #define BST_SET_LOOP_TIME 84 //in message Sets FC cycle time i.e looptime parameter -#define BST_FAILSAFE_CONFIG 85 //out message Returns FC Fail-Safe settings -#define BST_SET_FAILSAFE_CONFIG 86 //in message Sets FC Fail-Safe settings - -#define BST_RXFAIL_CONFIG 87 //out message Returns RXFAIL settings -#define BST_SET_RXFAIL_CONFIG 88 //in message Sets RXFAIL settings - // // Baseflight MSP commands (if enabled they exist in Cleanflight) // #define BST_RX_MAP 64 //out message get channel map (also returns number of channels total) #define BST_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from BST_RX_MAP -// FIXME - Provided for backwards compatibility with configurator code until configurator is updated. -// DEPRECATED - DO NOT USE "BST_BF_CONFIG" and BST_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead. -#define BST_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere -#define BST_SET_BF_CONFIG 67 //in message baseflight-specific settings save - #define BST_REBOOT 68 //in message reboot settings -// DEPRECATED - Use BST_BUILD_INFO instead -#define BST_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion #define BST_DISARM 70 //in message to disarm #define BST_ENABLE_ARM 71 //in message to enable arm @@ -206,68 +157,18 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define BST_FC_FILTERS 74 //out message #define BST_SET_FC_FILTERS 75 //in message -// -// Multwii original MSP commands -// - -// DEPRECATED - See BST_API_VERSION and BST_MIXER -#define BST_IDENT 100 //out message mixerMode + multiwii version + protocol version + capability variable - #define BST_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number -#define BST_RAW_IMU 102 //out message 9 DOF -#define BST_SERVO 103 //out message servos -#define BST_MOTOR 104 //out message motor -#define BST_RC 105 //out message rc channels and more -#define BST_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course -#define BST_COMP_GPS 107 //out message distance home, direction home -#define BST_ATTITUDE 108 //out message 2 angles 1 heading -#define BST_ALTITUDE 109 //out message altitude, variometer -#define BST_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX #define BST_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID #define BST_PID 112 //out message P I D coeff (9 are used currently) -#define BST_BOX 113 //out message BOX setup (number is dependant of your setup) #define BST_MISC 114 //out message powermeter trig -#define BST_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI -#define BST_BOXNAMES 116 //out message the aux switch names -#define BST_PIDNAMES 117 //out message the PID names -#define BST_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 BST_BOXIDS 119 //out message get the permanent IDs associated to BOXes -#define BST_SERVO_CONFIGURATIONS 120 //out message All servo configurations. -#define BST_NAV_STATUS 121 //out message Returns navigation status -#define BST_NAV_CONFIG 122 //out message Returns navigation parameters - -#define BST_SET_RAW_RC 200 //in message 8 rc chan -#define BST_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed #define BST_SET_PID 202 //in message P I D coeff (9 are used currently) -#define BST_SET_BOX 203 //in message BOX setup (number is dependant of your setup) -#define BST_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo #define BST_ACC_CALIBRATION 205 //in message no param #define BST_MAG_CALIBRATION 206 //in message no param #define BST_SET_MISC 207 //in message powermeter trig + 8 free for future use -#define BST_RESET_CONF 208 //in message no param -#define BST_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags) #define BST_SELECT_SETTING 210 //in message Select Setting Number (0-2) -//#define BST_SET_HEAD 211 //in message define a new heading hold direction // unused -#define BST_SET_SERVO_CONFIGURATION 212 //in message Servo settings -#define BST_SET_MOTOR 214 //in message PropBalance function -//#define BST_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom // unused - -// #define BST_BIND 240 //in message no param - #define BST_EEPROM_WRITE 250 //in message no param -#define BST_DEBUGMSG 253 //out message debug string buffer -#define BST_DEBUG 254 //out message debug1,debug2,debug3,debug4 - -// Additional commands that are not compatible with MultiWii -#define BST_UID 160 //out message Unique device ID -#define BST_ACC_TRIM 240 //out message get acc angle trim values -#define BST_SET_ACC_TRIM 239 //in message set acc angle trim values -#define BST_GPSSVINFO 164 //out message get Signal Strength (only U-Blox) -#define BST_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration -#define BST_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration - extern volatile uint8_t CRC8; extern volatile bool coreProReady; @@ -281,18 +182,6 @@ extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; // cause reboot after BST processing complete static bool isRebootScheduled = false; -static const char pidnames[] = - "ROLL;" - "PITCH;" - "YAW;" - "ALT;" - "Pos;" - "PosR;" - "NavR;" - "LEVEL;" - "MAG;" - "VEL;"; - #define BOARD_IDENTIFIER_LENGTH 4 typedef struct box_e { @@ -391,32 +280,6 @@ static uint8_t bstReadCRC(void) return readData[readData[1]+1]; } -static void s_struct(uint8_t *cb, uint8_t siz) -{ - while (siz--) - bstWrite8(*cb++); -} - -static void bstWriteNames(const char *s) -{ - const char *c; - for (c = s; *c; c++) - bstWrite8(*c); -} - -static 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; -} - static const box_t *findBoxByPermenantId(uint8_t permenantId) { uint8_t boxIndex; @@ -430,95 +293,22 @@ static const box_t *findBoxByPermenantId(uint8_t permenantId) return NULL; } -static void bstWriteBoxNamesReply(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++) - bstWrite8(box->boxName[j]); - } - } - - if (flag) { - flag = 0; - goto reset; - } -} - -static void bstWriteDataflashSummaryReply(void) -{ -#ifdef USE_FLASHFS - const flashGeometry_t *geometry = flashfsGetGeometry(); - bstWrite8(flashfsIsReady() ? 1 : 0); - bstWrite32(geometry->sectors); - bstWrite32(geometry->totalSize); - bstWrite32(flashfsGetOffset()); // Effectively the current number of bytes stored on the volume -#else - bstWrite8(0); - bstWrite32(0); - bstWrite32(0); - bstWrite32(0); -#endif -} - -#ifdef USE_FLASHFS -static void bstWriteDataflashReadReply(uint32_t address, uint8_t size) -{ - uint8_t buffer[128]; - int bytesRead; - - if (size > sizeof(buffer)) { - size = sizeof(buffer); - } - - bstWrite32(address); - - // bytesRead will be lower than that requested if we reach end of volume - bytesRead = flashfsReadAbs(address, buffer, size); - - for (int i = 0; i < bytesRead; i++) { - bstWrite8(buffer[i]); - } -} -#endif - #define IS_ENABLED(mask) (mask == 0 ? 0 : 1) /*************************************************************************************************/ #define BST_USB_COMMANDS 0x0A -#define BST_GENERAL_HEARTBEAT 0x0B -#define BST_USB_DEVICE_INFO_REQUEST 0x04 //Handshake -#define BST_USB_DEVICE_INFO_FRAME 0x05 //Handshake -#define BST_READ_COMMANDS 0x26 -#define BST_WRITE_COMMANDS 0x25 -#define BST_PASSED 0x01 -#define BST_FAILED 0x00 +#define BST_GENERAL_HEARTBEAT 0x0B +#define BST_USB_DEVICE_INFO_REQUEST 0x04 //Handshake +#define BST_USB_DEVICE_INFO_FRAME 0x05 //Handshake +#define BST_READ_COMMANDS 0x26 +#define BST_WRITE_COMMANDS 0x25 +#define BST_PASSED 0x01 +#define BST_FAILED 0x00 static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) { uint32_t i, tmp, junk; -#ifdef GPS - uint8_t wp_no; - int32_t lat = 0, lon = 0; -#endif - switch(bstRequest) { case BST_API_VERSION: bstWrite8(BST_PROTOCOL_VERSION); @@ -553,14 +343,6 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite8(shortGitRevision[i]); } break; - // DEPRECATED - Use MSP_API_VERSION - case BST_IDENT: - bstWrite8(MW_VERSION); - bstWrite8(mixerConfig()->mixerMode); - bstWrite8(BST_PROTOCOL_VERSION); - bstWrite32(CAP_DYNBALANCE); // "capability" - break; - case BST_STATUS: bstWrite16(getTaskDeltaTime(TASK_GYROPID)); #ifdef USE_I2C @@ -605,87 +387,6 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite32(junk); bstWrite8(getCurrentPidProfileIndex()); break; - case BST_RAW_IMU: - { - // Hack scale due to choice of units for sensor data in multiwii - uint8_t scale = (acc.dev.acc_1G > 1024) ? 8 : 1; - - for (i = 0; i < 3; i++) { - bstWrite16(acc.accSmooth[i] / scale); - } - for (i = 0; i < 3; i++) { - bstWrite16(gyroRateDps(i)); - } - for (i = 0; i < 3; i++) { - bstWrite16(mag.magADC[i]); - } - } - break; -#ifdef USE_SERVOS - case BST_SERVO: - s_struct((uint8_t *)&servo, MAX_SUPPORTED_SERVOS * 2); - break; - case BST_SERVO_CONFIGURATIONS: - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - bstWrite16(servoParams(i)->min); - bstWrite16(servoParams(i)->max); - bstWrite16(servoParams(i)->middle); - bstWrite8(servoParams(i)->rate); - bstWrite8(servoParams(i)->angleAtMin); - bstWrite8(servoParams(i)->angleAtMax); - bstWrite8(servoParams(i)->forwardFromChannel); - bstWrite32(servoParams(i)->reversedSources); - } - break; - case BST_SERVO_MIX_RULES: - for (i = 0; i < MAX_SERVO_RULES; i++) { - bstWrite8(customServoMixers(i)->targetChannel); - bstWrite8(customServoMixers(i)->inputSource); - bstWrite8(customServoMixers(i)->rate); - bstWrite8(customServoMixers(i)->speed); - bstWrite8(customServoMixers(i)->min); - bstWrite8(customServoMixers(i)->max); - bstWrite8(customServoMixers(i)->box); - } - break; -#endif - case BST_MOTOR: - s_struct((uint8_t *)motor, 16); - break; - case BST_RC: - for (i = 0; i < rxRuntimeConfig.channelCount; i++) - bstWrite16(rcData[i]); - break; - case BST_ATTITUDE: - for (i = 0; i < 2; i++) - bstWrite16(attitude.raw[i]); - //bstWrite16(heading); - break; - case BST_ALTITUDE: -#if defined(BARO) || defined(SONAR) - bstWrite32(altitudeHoldGetEstimatedAltitude()); -#else - bstWrite32(0); -#endif - bstWrite16(vario); - break; - case BST_SONAR_ALTITUDE: -#if defined(SONAR) - bstWrite32(sonarGetLatestAltitude()); -#else - bstWrite32(0); -#endif - break; - case BST_ANALOG: - bstWrite8((uint8_t)constrain(getBatteryVoltage(), 0, 255)); - bstWrite16((uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery - bstWrite16(rssi); - bstWrite16((int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A - break; - case BST_ARMING_CONFIG: - bstWrite8(armingConfig()->auto_disarm_delay); - bstWrite8(armingConfig()->disarm_kill_switch); - break; case BST_LOOP_TIME: //bstWrite16(masterConfig.looptime); bstWrite16(getTaskDeltaTime(TASK_GYROPID)); @@ -711,11 +412,6 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) } pidInitConfig(currentPidProfile); break; - case BST_PIDNAMES: - bstWriteNames(pidnames); - break; - case BST_PID_CONTROLLER: - break; case BST_MODE_RANGES: for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { const modeActivationCondition_t *mac = modeActivationConditions(i); @@ -726,29 +422,6 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite8(mac->range.endStep); } break; - case BST_ADJUSTMENT_RANGES: - for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { - const adjustmentRange_t *adjRange = adjustmentRanges(i); - bstWrite8(adjRange->adjustmentIndex); - bstWrite8(adjRange->auxChannelIndex); - bstWrite8(adjRange->range.startStep); - bstWrite8(adjRange->range.endStep); - bstWrite8(adjRange->adjustmentFunction); - bstWrite8(adjRange->auxSwitchChannelIndex); - } - break; - case BST_BOXNAMES: - bstWriteBoxNamesReply(); - break; - case BST_BOXIDS: - for (i = 0; i < activeBoxIdCount; i++) { - const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]); - if (!box) { - continue; - } - bstWrite8(box->permanentId); - } - break; case BST_MISC: bstWrite16(rxConfig()->midrc); @@ -778,102 +451,11 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite8(batteryConfig()->vbatmaxcellvoltage); bstWrite8(batteryConfig()->vbatwarningcellvoltage); break; - case BST_MOTOR_PINS: - // FIXME This is hardcoded and should not be. - for (i = 0; i < 8; i++) - bstWrite8(i + 1); - break; -#ifdef GPS - case BST_RAW_GPS: - bstWrite8(STATE(GPS_FIX)); - bstWrite8(GPS_numSat); - bstWrite32(GPS_coord[LAT]); - bstWrite32(GPS_coord[LON]); - bstWrite16(GPS_altitude); - bstWrite16(GPS_speed); - bstWrite16(GPS_ground_course); - break; - case BST_COMP_GPS: - bstWrite16(GPS_distanceToHome); - bstWrite16(GPS_directionToHome); - bstWrite8(GPS_update & 1); - break; - case BST_WP: - wp_no = bstRead8(); // get the wp number - 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]; - } - bstWrite8(wp_no); - bstWrite32(lat); - bstWrite32(lon); - bstWrite32(AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps - bstWrite16(0); // heading will come here (deg) - bstWrite16(0); // time to stay (ms) will come here - bstWrite8(0); // nav flag will come here - break; - case BST_GPSSVINFO: - bstWrite8(GPS_numCh); - for (i = 0; i < GPS_numCh; i++){ - bstWrite8(GPS_svinfo_chn[i]); - bstWrite8(GPS_svinfo_svid[i]); - bstWrite8(GPS_svinfo_quality[i]); - bstWrite8(GPS_svinfo_cno[i]); - } - break; -#endif - case BST_DEBUG: - // output some useful QA statistics - // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock] - - for (i = 0; i < DEBUG16_VALUE_COUNT; i++) - bstWrite16(debug[i]); // 4 variables are here for general monitoring purpose - break; - - // Additional commands that are not compatible with MultiWii - case BST_ACC_TRIM: - bstWrite16(accelerometerConfig()->accelerometerTrims.values.pitch); - bstWrite16(accelerometerConfig()->accelerometerTrims.values.roll); - break; - - case BST_UID: - bstWrite32(U_ID_0); - bstWrite32(U_ID_1); - bstWrite32(U_ID_2); - break; case BST_FEATURE: bstWrite32(featureMask()); break; - case BST_BOARD_ALIGNMENT: - bstWrite16(boardAlignment()->rollDegrees); - bstWrite16(boardAlignment()->pitchDegrees); - bstWrite16(boardAlignment()->yawDegrees); - break; - - - case BST_VOLTAGE_METER_CONFIG: - bstWrite8(voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale); - bstWrite8(batteryConfig()->vbatmincellvoltage); - bstWrite8(batteryConfig()->vbatmaxcellvoltage); - bstWrite8(batteryConfig()->vbatwarningcellvoltage); - break; - - case BST_CURRENT_METER_CONFIG: - bstWrite16(currentSensorADCConfig()->scale); - bstWrite16(currentSensorADCConfig()->offset); - bstWrite8(batteryConfig()->currentMeterSource); - bstWrite16(batteryConfig()->batteryCapacity); - break; - - case BST_MIXER: - bstWrite8(mixerConfig()->mixerMode); - break; - case BST_RX_CONFIG: bstWrite8(rxConfig()->serialrx_provider); bstWrite16(rxConfig()->maxcheck); @@ -884,56 +466,11 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite16(rxConfig()->rx_max_usec); break; - case BST_FAILSAFE_CONFIG: - bstWrite8(failsafeConfig()->failsafe_delay); - bstWrite8(failsafeConfig()->failsafe_off_delay); - bstWrite16(failsafeConfig()->failsafe_throttle); - break; - - case BST_RXFAIL_CONFIG: - for (i = NON_AUX_CHANNEL_COUNT; i < rxRuntimeConfig.channelCount; i++) { - bstWrite8(rxFailsafeChannelConfigs(i)->mode); - bstWrite16(RXFAIL_STEP_TO_CHANNEL_VALUE(rxFailsafeChannelConfigs(i)->step)); - } - break; - - case BST_RSSI_CONFIG: - bstWrite8(rxConfig()->rssi_channel); - break; - case BST_RX_MAP: for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) bstWrite8(rxConfig()->rcmap[i]); break; - case BST_BF_CONFIG: - bstWrite8(mixerConfig()->mixerMode); - - bstWrite32(featureMask()); - - bstWrite8(rxConfig()->serialrx_provider); - - bstWrite16(boardAlignment()->rollDegrees); - bstWrite16(boardAlignment()->pitchDegrees); - bstWrite16(boardAlignment()->yawDegrees); - - bstWrite16(currentSensorADCConfig()->scale); - bstWrite16(currentSensorADCConfig()->offset); - break; - - case BST_CF_SERIAL_CONFIG: - for (i = 0; i < SERIAL_PORT_COUNT; i++) { - if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) { - continue; - }; - bstWrite8(serialConfig()->portConfigs[i].identifier); - bstWrite16(serialConfig()->portConfigs[i].functionMask); - bstWrite8(serialConfig()->portConfigs[i].msp_baudrateIndex); - bstWrite8(serialConfig()->portConfigs[i].gps_baudrateIndex); - bstWrite8(serialConfig()->portConfigs[i].telemetry_baudrateIndex); - bstWrite8(serialConfig()->portConfigs[i].blackbox_baudrateIndex); - } - break; #ifdef LED_STRIP case BST_LED_COLORS: @@ -952,27 +489,6 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) } break; #endif - - case BST_DATAFLASH_SUMMARY: - bstWriteDataflashSummaryReply(); - break; - -#ifdef USE_FLASHFS - case BST_DATAFLASH_READ: - { - uint32_t readAddress = bstRead32(); - - bstWriteDataflashReadReply(readAddress, 128); - } - break; -#endif - - case BST_BF_BUILD_INFO: - for (i = 0; i < 11; i++) - bstWrite8(buildDate[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc - bstWrite32(0); // future exp - bstWrite32(0); // future exp - break; case BST_DEADBAND: bstWrite8(rcControlsConfig()->alt_hold_deadband); bstWrite8(rcControlsConfig()->alt_hold_fast_change); @@ -994,12 +510,6 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) { uint32_t i; uint16_t tmp; - uint8_t rate; - -#ifdef GPS - uint8_t wp_no; - int32_t lat = 0, lon = 0, alt = 0; -#endif bool ret = BST_PASSED; switch(bstWriteCommand) { @@ -1008,20 +518,10 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) changePidProfile(bstRead8()); } break; - case BST_SET_ACC_TRIM: - accelerometerConfigMutable()->accelerometerTrims.values.pitch = bstRead16(); - accelerometerConfigMutable()->accelerometerTrims.values.roll = bstRead16(); - break; - case BST_SET_ARMING_CONFIG: - armingConfigMutable()->auto_disarm_delay = bstRead8(); - armingConfigMutable()->disarm_kill_switch = bstRead8(); - break; case BST_SET_LOOP_TIME: //masterConfig.looptime = bstRead16(); bstRead16(); break; - case BST_SET_PID_CONTROLLER: - break; case BST_SET_PID: for (i = 0; i < PID_ITEM_COUNT; i++) { currentPidProfile->P8[i] = bstRead8(); @@ -1049,48 +549,6 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) ret = BST_FAILED; } break; - case BST_SET_ADJUSTMENT_RANGE: - i = bstRead8(); - if (i < MAX_ADJUSTMENT_RANGE_COUNT) { - adjustmentRange_t *adjRange = adjustmentRangesMutable(i); - i = bstRead8(); - if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) { - adjRange->adjustmentIndex = i; - adjRange->auxChannelIndex = bstRead8(); - adjRange->range.startStep = bstRead8(); - adjRange->range.endStep = bstRead8(); - adjRange->adjustmentFunction = bstRead8(); - adjRange->auxSwitchChannelIndex = bstRead8(); - } else { - ret = BST_FAILED; - } - } else { - ret = BST_FAILED; - } - break; - case BST_SET_RC_TUNING: - if (bstReadDataSize() >= 10) { - currentControlRateProfile->rcRate8 = bstRead8(); - currentControlRateProfile->rcExpo8 = bstRead8(); - for (i = 0; i < 3; i++) { - rate = bstRead8(); - currentControlRateProfile->rates[i] = MIN(rate, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); - } - rate = bstRead8(); - currentControlRateProfile->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX); - currentControlRateProfile->thrMid8 = bstRead8(); - currentControlRateProfile->thrExpo8 = bstRead8(); - currentControlRateProfile->tpa_breakpoint = bstRead16(); - if (bstReadDataSize() >= 11) { - currentControlRateProfile->rcYawExpo8 = bstRead8(); - } - if (bstReadDataSize() >= 12) { - currentControlRateProfile->rcYawRate8 = bstRead8(); - } - } else { - ret = BST_FAILED; - } - break; case BST_SET_MISC: tmp = bstRead16(); if (tmp < 1600 && tmp > 1400) @@ -1122,54 +580,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) batteryConfigMutable()->vbatmaxcellvoltage = bstRead8(); // vbatlevel_warn2 in MWC2.3 GUI batteryConfigMutable()->vbatwarningcellvoltage = bstRead8(); // vbatlevel when buzzer starts to alert break; - case BST_SET_MOTOR: - for (i = 0; i < 8; i++) // FIXME should this use MAX_MOTORS or MAX_SUPPORTED_MOTORS instead of 8 - motor_disarmed[i] = convertExternalToMotor(bstRead16()); - break; - case BST_SET_SERVO_CONFIGURATION: -#ifdef USE_SERVOS - if (bstReadDataSize() != 1 + sizeof(servoParam_t)) { - ret = BST_FAILED; - break; - } - i = bstRead8(); - if (i >= MAX_SUPPORTED_SERVOS) { - ret = BST_FAILED; - } else { - servoParamsMutable(i)->min = bstRead16(); - servoParamsMutable(i)->max = bstRead16(); - servoParamsMutable(i)->middle = bstRead16(); - servoParamsMutable(i)->rate = bstRead8(); - servoParamsMutable(i)->angleAtMin = bstRead8(); - servoParamsMutable(i)->angleAtMax = bstRead8(); - servoParamsMutable(i)->forwardFromChannel = bstRead8(); - servoParamsMutable(i)->reversedSources = bstRead32(); - } -#endif - break; - case BST_SET_SERVO_MIX_RULE: -#ifdef USE_SERVOS - i = bstRead8(); - if (i >= MAX_SERVO_RULES) { - ret = BST_FAILED; - } else { - customServoMixersMutable(i)->targetChannel = bstRead8(); - customServoMixersMutable(i)->inputSource = bstRead8(); - customServoMixersMutable(i)->rate = bstRead8(); - customServoMixersMutable(i)->speed = bstRead8(); - customServoMixersMutable(i)->min = bstRead8(); - customServoMixersMutable(i)->max = bstRead8(); - customServoMixersMutable(i)->box = bstRead8(); - loadCustomServoMixer(); - } -#endif - break; - case BST_RESET_CONF: - if (!ARMING_FLAG(ARMED)) { - resetEEPROM(); - readEEPROM(); - } - break; + case BST_ACC_CALIBRATION: if (!ARMING_FLAG(ARMED)) accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); @@ -1188,50 +599,6 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) writeEEPROM(); readEEPROM(); break; -#ifdef USE_FLASHFS - case BST_DATAFLASH_ERASE: - flashfsEraseCompletely(); - break; -#endif -#ifdef GPS - case BST_SET_RAW_GPS: - if (bstRead8()) { - ENABLE_STATE(GPS_FIX); - } else { - DISABLE_STATE(GPS_FIX); - } - GPS_numSat = bstRead8(); - GPS_coord[LAT] = bstRead32(); - GPS_coord[LON] = bstRead32(); - GPS_altitude = bstRead16(); - GPS_speed = bstRead16(); - GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers - break; - case BST_SET_WP: - wp_no = bstRead8(); //get the wp number - lat = bstRead32(); - lon = bstRead32(); - alt = bstRead32(); // to set altitude (cm) - bstRead16(); // future: to set heading (deg) - bstRead16(); // future: to set time to stay (ms) - bstRead8(); // 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 BST_SET_FEATURE: featureClearAll(); featureSet(bstRead32()); // features bitmap @@ -1243,30 +610,6 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) } #endif break; - case BST_SET_BOARD_ALIGNMENT: - boardAlignmentMutable()->rollDegrees = bstRead16(); - boardAlignmentMutable()->pitchDegrees = bstRead16(); - boardAlignmentMutable()->yawDegrees = bstRead16(); - break; - case BST_SET_VOLTAGE_METER_CONFIG: - voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale = bstRead8(); // actual vbatscale as intended - batteryConfigMutable()->vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI - batteryConfigMutable()->vbatmaxcellvoltage = bstRead8(); // vbatlevel_warn2 in MWC2.3 GUI - batteryConfigMutable()->vbatwarningcellvoltage = bstRead8(); // vbatlevel when buzzer starts to alert - break; - case BST_SET_CURRENT_METER_CONFIG: - currentSensorADCConfigMutable()->scale = bstRead16(); - currentSensorADCConfigMutable()->offset = bstRead16(); - batteryConfigMutable()->currentMeterSource = bstRead8(); - batteryConfigMutable()->batteryCapacity = bstRead16(); - break; - -#ifndef USE_QUAD_MIXER_ONLY - case BST_SET_MIXER: - mixerConfigMutable()->mixerMode = bstRead8(); - break; -#endif - case BST_SET_RX_CONFIG: rxConfigMutable()->serialrx_provider = bstRead8(); rxConfigMutable()->maxcheck = bstRead16(); @@ -1278,80 +621,12 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) rxConfigMutable()->rx_max_usec = bstRead16(); } break; - case BST_SET_FAILSAFE_CONFIG: - failsafeConfigMutable()->failsafe_delay = bstRead8(); - failsafeConfigMutable()->failsafe_off_delay = bstRead8(); - failsafeConfigMutable()->failsafe_throttle = bstRead16(); - break; - case BST_SET_RXFAIL_CONFIG: - { - uint8_t channelCount = bstReadDataSize() / 3; - if (channelCount > MAX_AUX_CHANNEL_COUNT) { - ret = BST_FAILED; - } else { - for (i = NON_AUX_CHANNEL_COUNT; i < channelCount; i++) { - rxFailsafeChannelConfigsMutable(i)->mode = bstRead8(); - rxFailsafeChannelConfigsMutable(i)->step = CHANNEL_VALUE_TO_RXFAIL_STEP(bstRead16()); - } - } - } - break; - case BST_SET_RSSI_CONFIG: - rxConfigMutable()->rssi_channel = bstRead8(); - break; case BST_SET_RX_MAP: for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { rxConfigMutable()->rcmap[i] = bstRead8(); } break; - case BST_SET_BF_CONFIG: -#ifdef USE_QUAD_MIXER_ONLY - bstRead8(); // mixerMode ignored -#else - mixerConfigMutable()->mixerMode = bstRead8(); // mixerMode -#endif - - featureClearAll(); - featureSet(bstRead32()); // features bitmap - - rxConfigMutable()->serialrx_provider = bstRead8(); // serialrx_type - - boardAlignmentMutable()->rollDegrees = bstRead16(); // board_align_roll - boardAlignmentMutable()->pitchDegrees = bstRead16(); // board_align_pitch - boardAlignmentMutable()->yawDegrees = bstRead16(); // board_align_yaw - - currentSensorADCConfigMutable()->scale = bstRead16(); - currentSensorADCConfigMutable()->offset = bstRead16(); - break; - case BST_SET_CF_SERIAL_CONFIG: - { - uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4); - if (bstReadDataSize() % portConfigSize != 0) { - ret = BST_FAILED; - break; - } - - uint8_t remainingPortsInPacket = bstReadDataSize() / portConfigSize; - - while (remainingPortsInPacket--) { - uint8_t identifier = bstRead8(); - - serialPortConfig_t *portConfig = serialFindPortConfiguration(identifier); - if (!portConfig) { - ret = BST_FAILED; - break; - } - - portConfig->identifier = identifier; - portConfig->functionMask = bstRead16(); - portConfig->msp_baudrateIndex = bstRead8(); - portConfig->gps_baudrateIndex = bstRead8(); - portConfig->telemetry_baudrateIndex = bstRead8(); - portConfig->blackbox_baudrateIndex = bstRead8(); - } - } - break; #ifdef LED_STRIP case BST_SET_LED_COLORS: //for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { From 5ab2117a692398267463676029e869a9b3d6a7d3 Mon Sep 17 00:00:00 2001 From: Hydra Date: Mon, 20 Mar 2017 21:12:27 +0000 Subject: [PATCH 8/9] CF/BF - Further isolate BST to the Colibri_Race target. Remove even more unused code. It was partly isolated before, but now it's even clearer that only this target uses it. --- src/main/fc/fc_init.c | 8 - src/main/target/COLIBRI_RACE/bus_bst.h | 23 +- .../target/COLIBRI_RACE/bus_bst_stm32f30x.c | 10 +- src/main/target/COLIBRI_RACE/i2c_bst.c | 211 ++++-------------- src/main/target/COLIBRI_RACE/target.c | 14 ++ src/main/target/COLIBRI_RACE/target.h | 1 + 6 files changed, 73 insertions(+), 194 deletions(-) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 6b36697bc..334152bf1 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -65,10 +65,6 @@ #include "drivers/exti.h" #include "drivers/vtx_soft_spi_rtc6705.h" -#ifdef USE_BST -#include "bus_bst.h" -#endif - #include "fc/config.h" #include "fc/fc_init.h" #include "fc/fc_msp.h" @@ -316,10 +312,6 @@ void init(void) initInverters(); #endif -#ifdef USE_BST - bstInit(BST_DEVICE); -#endif - #ifdef TARGET_BUS_INIT targetBusInit(); #else diff --git a/src/main/target/COLIBRI_RACE/bus_bst.h b/src/main/target/COLIBRI_RACE/bus_bst.h index a61078412..d2ead1e54 100644 --- a/src/main/target/COLIBRI_RACE/bus_bst.h +++ b/src/main/target/COLIBRI_RACE/bus_bst.h @@ -17,22 +17,15 @@ #pragma once -#define TBS_CORE_PNP_PRO 0x80 -#define RESERVED 0x8A -#define PNP_PRO_DIDITAL_CURRENT_SENSOR 0xC0 -#define PNP_PRO_GPS 0xC2 -#define TSB_BLACKBOX 0xC4 -#define CLEANFLIGHT_FC 0xC8 -#define CROSSFIRE_UHF_RECEIVER 0xEC +#define BST_BUFFER_SIZE 128 -#define GPS_POSITION_FRAME_ID 0x02 -#define GPS_TIME_FRAME_ID 0x03 -#define FC_ATTITUDE_FRAME_ID 0x1E -#define RC_CHANNEL_FRAME_ID 0x15 -#define CROSSFIRE_RSSI_FRAME_ID 0x14 -#define CLEANFLIGHT_MODE_FRAME_ID 0x20 - -#define DATA_BUFFER_SIZE 128 +#define I2C_ADDR_TBS_CORE_PNP_PRO 0x80 +#define I2C_ADDR_RESERVED 0x8A +#define I2C_ADDR_PNP_PRO_DIDITAL_CURRENT_SENSOR 0xC0 +#define I2C_ADDR_PNP_PRO_GPS 0xC2 +#define I2C_ADDR_TSB_BLACKBOX 0xC4 +#define I2C_ADDR_CROSSFIRE_UHF_RECEIVER 0xEC +#define I2C_ADDR_CLEANFLIGHT_FC 0xC8 typedef enum BSTDevice { BSTDEV_1, diff --git a/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c b/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c index 725e9076d..338186507 100644 --- a/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c +++ b/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c @@ -60,17 +60,17 @@ volatile bool coreProReady = false; // BST TimeoutUserCallback /////////////////////////////////////////////////////////////////////////////// -uint8_t dataBuffer[DATA_BUFFER_SIZE] = {0}; +uint8_t dataBuffer[BST_BUFFER_SIZE] = {0}; uint8_t dataBufferPointer = 0; uint8_t bstWriteDataLen = 0; uint32_t micros(void); -uint8_t writeData[DATA_BUFFER_SIZE] = {0}; +uint8_t writeData[BST_BUFFER_SIZE] = {0}; uint8_t currentWriteBufferPointer = 0; bool receiverAddress = false; -uint8_t readData[DATA_BUFFER_SIZE] = {0}; +uint8_t readData[BST_BUFFER_SIZE] = {0}; uint8_t bufferPointer = 0; bool cleanflight_data_ready = false; @@ -221,7 +221,7 @@ void bstInitPort(I2C_TypeDef *BSTx/*, uint8_t Address*/) BST_InitStructure.I2C_Mode = I2C_Mode_I2C; BST_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable; BST_InitStructure.I2C_DigitalFilter = 0x00; - BST_InitStructure.I2C_OwnAddress1 = CLEANFLIGHT_FC; + BST_InitStructure.I2C_OwnAddress1 = I2C_ADDR_CLEANFLIGHT_FC; BST_InitStructure.I2C_Ack = I2C_Ack_Enable; BST_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; BST_InitStructure.I2C_Timing = 0x30E0257A; // 100 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. @@ -269,7 +269,7 @@ void bstInitPort(I2C_TypeDef *BSTx/*, uint8_t Address*/) BST_InitStructure.I2C_Mode = I2C_Mode_I2C; BST_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable; BST_InitStructure.I2C_DigitalFilter = 0x00; - BST_InitStructure.I2C_OwnAddress1 = CLEANFLIGHT_FC; + BST_InitStructure.I2C_OwnAddress1 = I2C_ADDR_CLEANFLIGHT_FC; BST_InitStructure.I2C_Ack = I2C_Ack_Enable; BST_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; BST_InitStructure.I2C_Timing = 0x30E0257A; // 100 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index e108a5ed3..314241660 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -75,99 +75,48 @@ #include "bus_bst.h" #include "i2c_bst.h" -#define BST_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 BASEFLIGHT_IDENTIFIER "BAFL"; - -#define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4 -static const char * const 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 - -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 BST_API_VERSION 1 //out message -#define BST_FC_VARIANT 2 //out message -#define BST_FC_VERSION 3 //out message -#define BST_BOARD_INFO 4 //out message -#define BST_BUILD_INFO 5 //out message +#define GPS_POSITION_FRAME_ID 0x02 +#define GPS_TIME_FRAME_ID 0x03 +#define FC_ATTITUDE_FRAME_ID 0x1E +#define RC_CHANNEL_FRAME_ID 0x15 +#define CROSSFIRE_RSSI_FRAME_ID 0x14 +#define CLEANFLIGHT_MODE_FRAME_ID 0x20 // // MSP commands for Cleanflight original features // -#define BST_MODE_RANGES 34 //out message Returns all mode ranges -#define BST_SET_MODE_RANGE 35 //in message Sets a single mode range - +#define BST_MODE_RANGES 34 //out message Returns all mode ranges +#define BST_SET_MODE_RANGE 35 //in message Sets a single mode range #define BST_FEATURE 36 #define BST_SET_FEATURE 37 - #define BST_RX_CONFIG 44 #define BST_SET_RX_CONFIG 45 - #define BST_LED_COLORS 46 #define BST_SET_LED_COLORS 47 - #define BST_LED_STRIP_CONFIG 48 #define BST_SET_LED_STRIP_CONFIG 49 - -#define BST_LOOP_TIME 83 //out message Returns FC cycle time i.e looptime parameter -#define BST_SET_LOOP_TIME 84 //in message Sets FC cycle time i.e looptime parameter - -// -// Baseflight MSP commands (if enabled they exist in Cleanflight) -// -#define BST_RX_MAP 64 //out message get channel map (also returns number of channels total) -#define BST_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from BST_RX_MAP - -#define BST_REBOOT 68 //in message reboot settings - - -#define BST_DISARM 70 //in message to disarm -#define BST_ENABLE_ARM 71 //in message to enable arm - -#define BST_DEADBAND 72 //out message -#define BST_SET_DEADBAND 73 //in message - -#define BST_FC_FILTERS 74 //out message -#define BST_SET_FC_FILTERS 75 //in message - - -#define BST_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number -#define BST_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID -#define BST_PID 112 //out message P I D coeff (9 are used currently) -#define BST_MISC 114 //out message powermeter trig -#define BST_SET_PID 202 //in message P I D coeff (9 are used currently) -#define BST_ACC_CALIBRATION 205 //in message no param -#define BST_MAG_CALIBRATION 206 //in message no param -#define BST_SET_MISC 207 //in message powermeter trig + 8 free for future use -#define BST_SELECT_SETTING 210 //in message Select Setting Number (0-2) -#define BST_EEPROM_WRITE 250 //in message no param +#define BST_LOOP_TIME 83 //out message Returns FC cycle time i.e looptime parameter +#define BST_SET_LOOP_TIME 84 //in message Sets FC cycle time i.e looptime parameter +#define BST_RX_MAP 64 //out message Get channel map (also returns number of channels total) +#define BST_SET_RX_MAP 65 //in message Set rx map, numchannels to set comes from BST_RX_MAP +#define BST_REBOOT 68 //in message Reboot +#define BST_DISARM 70 //in message Disarm +#define BST_ENABLE_ARM 71 //in message Enable arm +#define BST_DEADBAND 72 //out message +#define BST_SET_DEADBAND 73 //in message +#define BST_FC_FILTERS 74 //out message +#define BST_SET_FC_FILTERS 75 //in message +#define BST_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number +#define BST_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID +#define BST_PID 112 //out message P I D coeff (9 are used currently) +#define BST_MISC 114 //out message powermeter trig +#define BST_SET_PID 202 //in message P I D coeff (9 are used currently) +#define BST_ACC_CALIBRATION 205 //in message no param +#define BST_MAG_CALIBRATION 206 //in message no param +#define BST_SET_MISC 207 //in message powermeter trig + 8 free for future use +#define BST_SELECT_SETTING 210 //in message Select Setting Number (0-2) +#define BST_EEPROM_WRITE 250 //in message no param extern volatile uint8_t CRC8; extern volatile bool coreProReady; @@ -182,8 +131,6 @@ extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; // cause reboot after BST processing complete static bool isRebootScheduled = false; -#define BOARD_IDENTIFIER_LENGTH 4 - typedef struct box_e { const uint8_t boxId; // see boxId_e const char *boxName; // GUI-readable box name @@ -223,8 +170,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; -extern uint8_t readData[DATA_BUFFER_SIZE]; -extern uint8_t writeData[DATA_BUFFER_SIZE]; +extern uint8_t readData[BST_BUFFER_SIZE]; +extern uint8_t writeData[BST_BUFFER_SIZE]; /*************************************************************************************************/ uint8_t writeBufferPointer = 1; @@ -310,39 +257,6 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) uint32_t i, tmp, junk; switch(bstRequest) { - case BST_API_VERSION: - bstWrite8(BST_PROTOCOL_VERSION); - - bstWrite8(API_VERSION_MAJOR); - bstWrite8(API_VERSION_MINOR); - break; - case BST_FC_VARIANT: - for (i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) { - bstWrite8(flightControllerIdentifier[i]); - } - break; - case BST_FC_VERSION: - bstWrite8(FC_VERSION_MAJOR); - bstWrite8(FC_VERSION_MINOR); - bstWrite8(FC_VERSION_PATCH_LEVEL); - break; - case BST_BOARD_INFO: - for (i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) { - bstWrite8(boardIdentifier[i]); - } - break; - case BST_BUILD_INFO: - for (i = 0; i < BUILD_DATE_LENGTH; i++) { - bstWrite8(buildDate[i]); - } - for (i = 0; i < BUILD_TIME_LENGTH; i++) { - bstWrite8(buildTime[i]); - } - - for (i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) { - bstWrite8(shortGitRevision[i]); - } - break; case BST_STATUS: bstWrite16(getTaskDeltaTime(TASK_GYROPID)); #ifdef USE_I2C @@ -388,7 +302,6 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite8(getCurrentPidProfileIndex()); break; case BST_LOOP_TIME: - //bstWrite16(masterConfig.looptime); bstWrite16(getTaskDeltaTime(TASK_GYROPID)); break; case BST_RC_TUNING: @@ -502,7 +415,6 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) // we do not know how to handle the (valid) message, indicate error BST return false; } - //bstSlaveWrite(writeData); return true; } @@ -519,7 +431,6 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) } break; case BST_SET_LOOP_TIME: - //masterConfig.looptime = bstRead16(); bstRead16(); break; case BST_SET_PID: @@ -560,15 +471,15 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) failsafeConfigMutable()->failsafe_throttle = bstRead16(); - #ifdef GPS +#ifdef GPS gpsConfigMutable()->provider = bstRead8(); // gps_type bstRead8(); // gps_baudrate gpsConfigMutable()->sbasMode = bstRead8(); // gps_ubx_sbas - #else +#else bstRead8(); // gps_type bstRead8(); // gps_baudrate bstRead8(); // gps_ubx_sbas - #endif +#endif bstRead8(); // legacy - was multiwiiCurrentMeterOutput rxConfigMutable()->rssi_channel = bstRead8(); bstRead8(); @@ -593,7 +504,6 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) if (ARMING_FLAG(ARMED)) { ret = BST_FAILED; bstWrite8(ret); - //bstSlaveWrite(writeData); return ret; } writeEEPROM(); @@ -677,9 +587,10 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) ret = BST_FAILED; } bstWrite8(ret); - //bstSlaveWrite(writeData); + if(ret == BST_FAILED) return false; + return true; } @@ -694,25 +605,26 @@ static bool bstSlaveUSBCommandFeedback(/*uint8_t bstFeedback*/) bstWrite8(FC_VERSION_MINOR); //Firmware ID bstWrite8(0x00); bstWrite8(0x00); - //bstSlaveWrite(writeData); return true; } /*************************************************************************************************/ #define BST_RESET_TIME 1.2*1000*1000 //micro-seconds + uint32_t resetBstTimer = 0; bool needResetCheck = true; + extern bool cleanflight_data_ready; void bstProcessInCommand(void) { readBufferPointer = 2; - if(bstCurrentAddress() == CLEANFLIGHT_FC) { + if(bstCurrentAddress() == I2C_ADDR_CLEANFLIGHT_FC) { if(bstReadCRC() == CRC8 && bstRead8()==BST_USB_COMMANDS) { uint8_t i; writeBufferPointer = 1; cleanflight_data_ready = false; - for(i = 0; i < DATA_BUFFER_SIZE; i++) { + for(i = 0; i < BST_BUFFER_SIZE; i++) { writeData[i] = 0; } switch (bstRead8()) { @@ -780,9 +692,10 @@ void taskBstMasterProcess(timeUs_t currentTimeUs) sendCounter = 0; next20hzUpdateAt_1 = currentTimeUs + UPDATE_AT_20HZ; } - +#ifdef GPS if(sensors(SENSOR_GPS) && !bstWriteBusy()) writeGpsPositionPrameToBST(); +#endif } bstMasterWriteLoop(); @@ -795,7 +708,7 @@ void taskBstMasterProcess(timeUs_t currentTimeUs) /*************************************************************************************************/ static uint8_t masterWriteBufferPointer; -static uint8_t masterWriteData[DATA_BUFFER_SIZE]; +static uint8_t masterWriteData[BST_BUFFER_SIZE]; static void bstMasterStartBuffer(uint8_t address) { @@ -831,9 +744,9 @@ static uint16_t alt = 0; static uint8_t numOfSat = 0; #endif +#ifdef GPS bool writeGpsPositionPrameToBST(void) { -#ifdef GPS if((lat != GPS_coord[LAT]) || (lon != GPS_coord[LON]) || (alt != GPS_altitude) || (numOfSat != GPS_numSat)) { lat = GPS_coord[LAT]; lon = GPS_coord[LON]; @@ -858,10 +771,8 @@ bool writeGpsPositionPrameToBST(void) return bstMasterWrite(masterWriteData); } else return false; -#else - return true; -#endif } +#endif bool writeRollPitchYawToBST(void) { @@ -892,37 +803,6 @@ bool writeRCChannelToBST(void) bool writeFCModeToBST(void) { -#ifdef CLEANFLIGHT_FULL_STATUS_SET - uint32_t tmp = 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; - bstMasterStartBuffer(PUBLIC_ADDRESS); - bstMasterWrite8(CLEANFLIGHT_MODE_FRAME_ID); - bstMasterWrite32(tmp); - bstMasterWrite16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); -#else uint8_t tmp = 0; tmp = IS_ENABLED(ARMING_FLAG(ARMED)) | IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << 1 | @@ -937,7 +817,6 @@ bool writeFCModeToBST(void) bstMasterWrite8(CLEANFLIGHT_MODE_FRAME_ID); bstMasterWrite8(tmp); bstMasterWrite8(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); -#endif return bstMasterWrite(masterWriteData); } diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index 8cfeb6edb..ad5833508 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -23,6 +23,11 @@ #include "drivers/dma.h" #include "drivers/timer.h" +#ifdef USE_BST +#include "bus_bst.h" +#endif + + const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_PPM, 0, GPIO_AF_6, NULL, 0 }, // PWM1 - PA8 { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_2, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM2 - PC6 @@ -37,3 +42,12 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_1, NULL, 0 }, // PWM11 - PB15 { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM_USE_LED, 1, GPIO_AF_1, DMA1_Channel3, DMA1_CH3_HANDLER }, // PWM11 - PB15 }; + + +#ifdef USE_BST +void targetBusInit(void) +{ + bstInit(BST_DEVICE); +} +#endif + diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index b09b63efd..2570d83f5 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -22,6 +22,7 @@ #define BST_DEVICE_NAME_LENGTH 12 #define TARGET_CONFIG #define TARGET_VALIDATECONFIG +#define TARGET_BUS_INIT #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT From a45e974c383958af52c390504548d30f84449ea3 Mon Sep 17 00:00:00 2001 From: Hydra Date: Tue, 21 Mar 2017 21:39:57 +0000 Subject: [PATCH 9/9] CF/BF - Restore BST_API_VERSION and BST_BUILD_INFO based on updated requirements from TBS engineers. Larry Ho: " BST_MAG_CALIBRATION, BST_ACC_CALIBRATION, BST_SET_PID, BST_SET_RC_TUNING, BST_SET_LOOP_TIME, BST_SELECT_SETTING, BST_SET_RX_MAP, BST_SET_MISC, BST_SET_DEADBAND, BST_SET_FC_FILTERS, BST_SET_FEATURE, BST_SET_RX_CONFIG, BST_SET_MODE_RANGE, BST_SET_LED_COLORS, BST_SET_LED_STRIP_CONFIG, BST_EEPROM_WRITE, BST_REBOOT, BST_DISARM, BST_ENABLE_ARM, BST_PID, BST_STATUS, BST_RC_TUNING, BST_LOOP_TIME, BST_RX_MAP, BST_MISC, BST_DEADBAND, BST_FC_FILTERS, BST_FEATURE, BST_RX_CONFIG, BST_LED_COLORS, BST_LED_STRIP_CONFIG, BST_API_VERSION" There was a required to keep `BST_BF_BUILD_INFO`, which is not on the list above, but has been deprecated for ages. Since it was never used we will keep the replacement `BS_BUILD_INFO` instead. Dominic Clifton: "I will keep BST_BUILD_INFO and BST_API_VERSION and all the ones on your list above." --- src/main/target/COLIBRI_RACE/i2c_bst.c | 31 ++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 314241660..32797d489 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -82,10 +82,23 @@ #define CROSSFIRE_RSSI_FRAME_ID 0x14 #define CLEANFLIGHT_MODE_FRAME_ID 0x20 +#define BST_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 + // // MSP commands for Cleanflight original features // +#define BST_API_VERSION 1 //out message +#define BST_FC_VARIANT 2 //out message +#define BST_FC_VERSION 3 //out message +#define BST_BOARD_INFO 4 //out message +#define BST_BUILD_INFO 5 //out message + #define BST_MODE_RANGES 34 //out message Returns all mode ranges #define BST_SET_MODE_RANGE 35 //in message Sets a single mode range #define BST_FEATURE 36 @@ -257,6 +270,24 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) uint32_t i, tmp, junk; switch(bstRequest) { + case BST_API_VERSION: + bstWrite8(BST_PROTOCOL_VERSION); + + bstWrite8(API_VERSION_MAJOR); + bstWrite8(API_VERSION_MINOR); + break; + case BST_BUILD_INFO: + for (i = 0; i < BUILD_DATE_LENGTH; i++) { + bstWrite8(buildDate[i]); + } + for (i = 0; i < BUILD_TIME_LENGTH; i++) { + bstWrite8(buildTime[i]); + } + + for (i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) { + bstWrite8(shortGitRevision[i]); + } + break; case BST_STATUS: bstWrite16(getTaskDeltaTime(TASK_GYROPID)); #ifdef USE_I2C