From b7ee04127dd2c25b5a974468560631f58b675340 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 2 May 2020 19:21:48 +1200 Subject: [PATCH] Removed the unused setting 'mag_declination'. --- src/main/cli/settings.c | 1 - src/main/msp/msp.c | 12 ------------ src/main/msp/msp_protocol.h | 4 ++-- src/main/sensors/compass.c | 8 +------- src/main/sensors/compass.h | 3 --- src/main/target/COLIBRI_RACE/i2c_bst.c | 4 ++-- 6 files changed, 5 insertions(+), 27 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 8669b1372..563dbd402 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -686,7 +686,6 @@ const clivalue_t valueTable[] = { { "mag_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_i2c_address) }, { "mag_spi_device", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_spi_device) }, { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MAG_HARDWARE }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_hardware) }, - { "mag_declination", VAR_INT16 | MASTER_VALUE, .config.minmax = { -18000, 18000 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_declination) }, { "mag_calibration", VAR_INT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = XYZ_AXIS_COUNT, PG_COMPASS_CONFIG, offsetof(compassConfig_t, magZero.raw) }, #endif diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 5171cb850..f602c7b63 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1327,12 +1327,6 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) #endif break; -#ifdef USE_MAG - case MSP_COMPASS_CONFIG: - sbufWriteU16(dst, compassConfig()->mag_declination / 10); - break; -#endif - #if defined(USE_ESC_SENSOR) // Deprecated in favor of MSP_MOTOR_TELEMETY as of API version 1.42 case MSP_ESC_SENSOR_DATA: @@ -2358,12 +2352,6 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, #endif #endif -#ifdef USE_MAG - case MSP_SET_COMPASS_CONFIG: - compassConfigMutable()->mag_declination = sbufReadU16(src) * 10; - break; -#endif - case MSP_SET_MOTOR: for (int i = 0; i < getMotorCount(); i++) { motor_disarmed[i] = motorConvertFromExternal(sbufReadU16(src)); diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 1497d0b38..15156e5e5 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -277,7 +277,7 @@ #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 +//DEPRECATED - #define MSP_COMPASS_CONFIG 133 //out message Compass configuration #define MSP_ESC_SENSOR_DATA 134 //out message Extra ESC data from 32-Bit ESCs (Temperature, RPM) #define MSP_GPS_RESCUE 135 //out message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed, sanityChecks and minSats #define MSP_GPS_RESCUE_PIDS 136 //out message GPS Rescues's throttleP and velocity PIDS + yaw P @@ -309,7 +309,7 @@ #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 +//DEPRECATED - #define MSP_SET_COMPASS_CONFIG 224 //out message Compass configuration #define MSP_SET_GPS_RESCUE 225 //in message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed, sanityChecks and minSats #define MSP_SET_GPS_RESCUE_PIDS 226 //in message GPS Rescues's throttleP and velocity PIDS + yaw P #define MSP_SET_VTXTABLE_BAND 227 //in message set vtxTable band/channel data (one band at a time) diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 2be2970b5..2592be4f1 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -63,13 +63,12 @@ static flightDynamicsTrims_t magZeroTempMax; magDev_t magDev; mag_t mag; // mag access functions -PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 2); +PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 3); void pgResetFn_compassConfig(compassConfig_t *compassConfig) { compassConfig->mag_alignment = ALIGN_DEFAULT; memset(&compassConfig->mag_customAlignment, 0x00, sizeof(compassConfig->mag_customAlignment)); - compassConfig->mag_declination = 0; compassConfig->mag_hardware = MAG_DEFAULT; // Generate a reasonable default for backward compatibility @@ -289,8 +288,6 @@ bool compassDetect(magDev_t *dev, sensor_align_e *alignment) bool compassInit(void) { // initialize and calibration. turn on led during mag calibration (calibration routine blinks it) - // calculate magnetic declination - mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. sensor_align_e alignment; @@ -298,9 +295,6 @@ bool compassInit(void) return false; } - const int16_t deg = compassConfig()->mag_declination / 100; - const int16_t min = compassConfig()->mag_declination % 100; - mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units LED1_ON; magDev.init(&magDev); LED1_OFF; diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 1250d925c..2be455313 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -41,14 +41,11 @@ typedef enum { typedef struct mag_s { float magADC[XYZ_AXIS_COUNT]; - float magneticDeclination; } mag_t; extern mag_t mag; typedef struct compassConfig_s { - int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/ - // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. uint8_t mag_alignment; // mag alignment uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device uint8_t mag_bustype; diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index b96a1de9c..4b377e416 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -370,7 +370,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite8(rxConfig()->rssi_channel); bstWrite8(0); - bstWrite16(compassConfig()->mag_declination / 10); + bstWrite16(0); // was mag_declination / 10 bstWrite8(voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale); bstWrite8((batteryConfig()->vbatmincellvoltage + 5) / 10); @@ -518,7 +518,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) rxConfigMutable()->rssi_channel = bstRead8(); bstRead8(); - compassConfigMutable()->mag_declination = bstRead16() * 10; + bstRead16(); // was mag_declination / 10 voltageSensorADCConfigMutable(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale = bstRead8(); // actual vbatscale as intended batteryConfigMutable()->vbatmincellvoltage = bstRead8() * 10; // vbatlevel_warn1 in MWC2.3 GUI