From f8d0dd98f76b6ce3111ad369f755460b7e21b5c9 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 21 Apr 2014 23:08:25 +0100 Subject: [PATCH] Split config_storage.h into config_master.h and config_profile.h to separate concerns and help reduce and clarify dependencies. cfg and mcfg are too similarly named and are not obvious. Renamed cfg to currentProfile and mcfg to masterConfig. This will increase source code line length somewhat but that's ok; lines of code doing too much are now easier to spot. --- src/config.c | 309 +++++++++++----------- src/{config_storage.h => config_master.h} | 63 +---- src/config_profile.h | 57 ++++ src/flight_imu.c | 48 ++-- src/flight_mixer.c | 102 +++---- src/gps_common.c | 44 +-- src/main.c | 45 ++-- src/mw.c | 134 +++++----- src/mw.h | 3 +- src/sensors_acceleration.c | 54 ++-- src/sensors_barometer.c | 8 +- src/sensors_compass.c | 12 +- src/sensors_gyro.c | 2 +- src/sensors_initialisation.c | 38 +-- src/serial_cli.c | 282 ++++++++++---------- src/serial_msp.c | 124 ++++----- src/telemetry_common.c | 20 +- src/telemetry_frsky.c | 4 +- 18 files changed, 677 insertions(+), 672 deletions(-) rename src/{config_storage.h => config_master.h} (51%) create mode 100644 src/config_profile.h diff --git a/src/config.c b/src/config.c index 7ea48adf2c..af88505aaf 100755 --- a/src/config.c +++ b/src/config.c @@ -30,7 +30,8 @@ #include "runtime_config.h" #include "config.h" -#include "config_storage.h" +#include "config_profile.h" +#include "config_master.h" void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h @@ -41,8 +42,8 @@ void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h #define FLASH_PAGE_SIZE ((uint16_t)0x400) #define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 2)) // use the last 2 KB for storage -master_t mcfg; // master config struct with data independent from profiles -config_t cfg; // profile config struct +master_t masterConfig; // master config struct with data independent from profiles +profile_t currentProfile; // profile config struct static const uint8_t EEPROM_CONF_VERSION = 64; static void resetConf(void); @@ -81,12 +82,12 @@ static bool isEEPROMContentValid(void) void activateConfig(void) { - generatePitchCurve(&cfg.controlRateConfig); - generateThrottleCurve(&cfg.controlRateConfig, mcfg.minthrottle, mcfg.maxthrottle); + generatePitchCurve(¤tProfile.controlRateConfig); + generateThrottleCurve(¤tProfile.controlRateConfig, masterConfig.minthrottle, masterConfig.maxthrottle); - setPIDController(cfg.pidController); + setPIDController(currentProfile.pidController); gpsSetPIDs(); - useFailsafeConfig(&cfg.failsafeConfig); + useFailsafeConfig(¤tProfile.failsafeConfig); } void readEEPROM(void) @@ -96,11 +97,11 @@ void readEEPROM(void) failureMode(10); // Read flash - memcpy(&mcfg, (char *)FLASH_WRITE_ADDR, sizeof(master_t)); + memcpy(&masterConfig, (char *)FLASH_WRITE_ADDR, sizeof(master_t)); // Copy current profile - if (mcfg.current_profile > 2) // sanity check - mcfg.current_profile = 0; - memcpy(&cfg, &mcfg.profile[mcfg.current_profile], sizeof(config_t)); + if (masterConfig.current_profile > 2) // sanity check + masterConfig.current_profile = 0; + memcpy(¤tProfile, &masterConfig.profile[masterConfig.current_profile], sizeof(profile_t)); activateConfig(); } @@ -115,7 +116,7 @@ void readEEPROMAndNotify(void) void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex) { // copy current in-memory profile to stored configuration - memcpy(&mcfg.profile[profileSlotIndex], &cfg, sizeof(config_t)); + memcpy(&masterConfig.profile[profileSlotIndex], ¤tProfile, sizeof(profile_t)); } void writeEEPROM(void) @@ -125,12 +126,12 @@ void writeEEPROM(void) int8_t attemptsRemaining = 3; // prepare checksum/version constants - mcfg.version = EEPROM_CONF_VERSION; - mcfg.size = sizeof(master_t); - mcfg.magic_be = 0xBE; - mcfg.magic_ef = 0xEF; - mcfg.chk = 0; // erase checksum before recalculating - mcfg.chk = calculateChecksum((const uint8_t *)&mcfg, sizeof(master_t)); + masterConfig.version = EEPROM_CONF_VERSION; + masterConfig.size = sizeof(master_t); + masterConfig.magic_be = 0xBE; + masterConfig.magic_ef = 0xEF; + masterConfig.chk = 0; // erase checksum before recalculating + masterConfig.chk = calculateChecksum((const uint8_t *)&masterConfig, sizeof(master_t)); // write it FLASH_Unlock(); @@ -139,7 +140,7 @@ void writeEEPROM(void) status = FLASH_ErasePage(FLASH_WRITE_ADDR); for (wordOffset = 0; wordOffset < sizeof(master_t) && status == FLASH_COMPLETE; wordOffset += 4) { - status = FLASH_ProgramWord(FLASH_WRITE_ADDR + wordOffset, *(uint32_t *) ((char *)&mcfg + wordOffset)); + status = FLASH_ProgramWord(FLASH_WRITE_ADDR + wordOffset, *(uint32_t *) ((char *)&masterConfig + wordOffset)); } if (status == FLASH_COMPLETE) { break; @@ -175,193 +176,193 @@ static void resetConf(void) int8_t servoRates[8] = { 30, 30, 100, 100, 100, 100, 100, 100 }; // Clear all configuration - memset(&mcfg, 0, sizeof(master_t)); - memset(&cfg, 0, sizeof(config_t)); + memset(&masterConfig, 0, sizeof(master_t)); + memset(¤tProfile, 0, sizeof(profile_t)); - mcfg.version = EEPROM_CONF_VERSION; - mcfg.mixerConfiguration = MULTITYPE_QUADX; + masterConfig.version = EEPROM_CONF_VERSION; + masterConfig.mixerConfiguration = MULTITYPE_QUADX; featureClearAll(); featureSet(FEATURE_VBAT); // global settings - mcfg.current_profile = 0; // default profile - mcfg.gyro_cmpf_factor = 600; // default MWC - mcfg.gyro_cmpfm_factor = 250; // default MWC - mcfg.gyro_lpf = 42; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead - mcfg.accZero[0] = 0; - mcfg.accZero[1] = 0; - mcfg.accZero[2] = 0; - mcfg.gyro_align = ALIGN_DEFAULT; - mcfg.acc_align = ALIGN_DEFAULT; - mcfg.mag_align = ALIGN_DEFAULT; - mcfg.boardAlignment.rollDegrees = 0; - mcfg.boardAlignment.pitchDegrees = 0; - mcfg.boardAlignment.yawDegrees = 0; - mcfg.acc_hardware = ACC_DEFAULT; // default/autodetect - mcfg.max_angle_inclination = 500; // 50 degrees - mcfg.yaw_control_direction = 1; - mcfg.moron_threshold = 32; - mcfg.batteryConfig.vbatscale = 110; - mcfg.batteryConfig.vbatmaxcellvoltage = 43; - mcfg.batteryConfig.vbatmincellvoltage = 33; - mcfg.power_adc_channel = 0; - mcfg.telemetry_provider = TELEMETRY_PROVIDER_FRSKY; - mcfg.telemetry_port = TELEMETRY_PORT_UART; - mcfg.telemetry_switch = 0; - mcfg.rxConfig.serialrx_type = 0; - mcfg.rxConfig.midrc = 1500; - mcfg.rxConfig.mincheck = 1100; - mcfg.rxConfig.maxcheck = 1900; - mcfg.retarded_arm = 0; // disable arm/disarm on roll left/right - mcfg.flaps_speed = 0; - mcfg.fixedwing_althold_dir = 1; + masterConfig.current_profile = 0; // default profile + masterConfig.gyro_cmpf_factor = 600; // default MWC + masterConfig.gyro_cmpfm_factor = 250; // default MWC + masterConfig.gyro_lpf = 42; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead + masterConfig.accZero[0] = 0; + masterConfig.accZero[1] = 0; + masterConfig.accZero[2] = 0; + masterConfig.gyro_align = ALIGN_DEFAULT; + masterConfig.acc_align = ALIGN_DEFAULT; + masterConfig.mag_align = ALIGN_DEFAULT; + masterConfig.boardAlignment.rollDegrees = 0; + masterConfig.boardAlignment.pitchDegrees = 0; + masterConfig.boardAlignment.yawDegrees = 0; + masterConfig.acc_hardware = ACC_DEFAULT; // default/autodetect + masterConfig.max_angle_inclination = 500; // 50 degrees + masterConfig.yaw_control_direction = 1; + masterConfig.moron_threshold = 32; + masterConfig.batteryConfig.vbatscale = 110; + masterConfig.batteryConfig.vbatmaxcellvoltage = 43; + masterConfig.batteryConfig.vbatmincellvoltage = 33; + masterConfig.power_adc_channel = 0; + masterConfig.telemetry_provider = TELEMETRY_PROVIDER_FRSKY; + masterConfig.telemetry_port = TELEMETRY_PORT_UART; + masterConfig.telemetry_switch = 0; + masterConfig.rxConfig.serialrx_type = 0; + masterConfig.rxConfig.midrc = 1500; + masterConfig.rxConfig.mincheck = 1100; + masterConfig.rxConfig.maxcheck = 1900; + masterConfig.retarded_arm = 0; // disable arm/disarm on roll left/right + masterConfig.flaps_speed = 0; + masterConfig.fixedwing_althold_dir = 1; // Motor/ESC/Servo - mcfg.minthrottle = 1150; - mcfg.maxthrottle = 1850; - mcfg.mincommand = 1000; - mcfg.deadband3d_low = 1406; - mcfg.deadband3d_high = 1514; - mcfg.neutral3d = 1460; - mcfg.deadband3d_throttle = 50; - mcfg.motor_pwm_rate = 400; - mcfg.servo_pwm_rate = 50; + masterConfig.minthrottle = 1150; + masterConfig.maxthrottle = 1850; + masterConfig.mincommand = 1000; + masterConfig.deadband3d_low = 1406; + masterConfig.deadband3d_high = 1514; + masterConfig.neutral3d = 1460; + masterConfig.deadband3d_throttle = 50; + masterConfig.motor_pwm_rate = 400; + masterConfig.servo_pwm_rate = 50; // gps/nav stuff - mcfg.gps_type = GPS_NMEA; - mcfg.gps_baudrate = GPS_BAUD_115200; + masterConfig.gps_type = GPS_NMEA; + masterConfig.gps_baudrate = GPS_BAUD_115200; // serial (USART1) baudrate - mcfg.serialConfig.port1_baudrate = 115200; - mcfg.serialConfig.softserial_baudrate = 9600; - mcfg.serialConfig.softserial_1_inverted = 0; - mcfg.serialConfig.softserial_2_inverted = 0; - mcfg.serialConfig.reboot_character = 'R'; + masterConfig.serialConfig.port1_baudrate = 115200; + masterConfig.serialConfig.softserial_baudrate = 9600; + masterConfig.serialConfig.softserial_1_inverted = 0; + masterConfig.serialConfig.softserial_2_inverted = 0; + masterConfig.serialConfig.reboot_character = 'R'; - mcfg.looptime = 3500; - mcfg.emfAvoidance = 0; - mcfg.rssi_aux_channel = 0; + masterConfig.looptime = 3500; + masterConfig.emfAvoidance = 0; + masterConfig.rssi_aux_channel = 0; - cfg.pidController = 0; - cfg.P8[ROLL] = 40; - cfg.I8[ROLL] = 30; - cfg.D8[ROLL] = 23; - cfg.P8[PITCH] = 40; - cfg.I8[PITCH] = 30; - cfg.D8[PITCH] = 23; - cfg.P8[YAW] = 85; - cfg.I8[YAW] = 45; - cfg.D8[YAW] = 0; - cfg.P8[PIDALT] = 50; - cfg.I8[PIDALT] = 0; - cfg.D8[PIDALT] = 0; - cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100; - cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100; - cfg.D8[PIDPOS] = 0; - cfg.P8[PIDPOSR] = 20; // POSHOLD_RATE_P * 10; - cfg.I8[PIDPOSR] = 8; // POSHOLD_RATE_I * 100; - cfg.D8[PIDPOSR] = 45; // POSHOLD_RATE_D * 1000; - cfg.P8[PIDNAVR] = 14; // NAV_P * 10; - cfg.I8[PIDNAVR] = 20; // NAV_I * 100; - cfg.D8[PIDNAVR] = 80; // NAV_D * 1000; - cfg.P8[PIDLEVEL] = 90; - cfg.I8[PIDLEVEL] = 10; - cfg.D8[PIDLEVEL] = 100; - cfg.P8[PIDMAG] = 40; - cfg.P8[PIDVEL] = 120; - cfg.I8[PIDVEL] = 45; - cfg.D8[PIDVEL] = 1; - cfg.controlRateConfig.rcRate8 = 90; - cfg.controlRateConfig.rcExpo8 = 65; - cfg.controlRateConfig.rollPitchRate = 0; - cfg.controlRateConfig.yawRate = 0; - cfg.dynThrPID = 0; - cfg.tpaBreakPoint = 1500; - cfg.controlRateConfig.thrMid8 = 50; - cfg.controlRateConfig.thrExpo8 = 0; + currentProfile.pidController = 0; + currentProfile.P8[ROLL] = 40; + currentProfile.I8[ROLL] = 30; + currentProfile.D8[ROLL] = 23; + currentProfile.P8[PITCH] = 40; + currentProfile.I8[PITCH] = 30; + currentProfile.D8[PITCH] = 23; + currentProfile.P8[YAW] = 85; + currentProfile.I8[YAW] = 45; + currentProfile.D8[YAW] = 0; + currentProfile.P8[PIDALT] = 50; + currentProfile.I8[PIDALT] = 0; + currentProfile.D8[PIDALT] = 0; + currentProfile.P8[PIDPOS] = 11; // POSHOLD_P * 100; + currentProfile.I8[PIDPOS] = 0; // POSHOLD_I * 100; + currentProfile.D8[PIDPOS] = 0; + currentProfile.P8[PIDPOSR] = 20; // POSHOLD_RATE_P * 10; + currentProfile.I8[PIDPOSR] = 8; // POSHOLD_RATE_I * 100; + currentProfile.D8[PIDPOSR] = 45; // POSHOLD_RATE_D * 1000; + currentProfile.P8[PIDNAVR] = 14; // NAV_P * 10; + currentProfile.I8[PIDNAVR] = 20; // NAV_I * 100; + currentProfile.D8[PIDNAVR] = 80; // NAV_D * 1000; + currentProfile.P8[PIDLEVEL] = 90; + currentProfile.I8[PIDLEVEL] = 10; + currentProfile.D8[PIDLEVEL] = 100; + currentProfile.P8[PIDMAG] = 40; + currentProfile.P8[PIDVEL] = 120; + currentProfile.I8[PIDVEL] = 45; + currentProfile.D8[PIDVEL] = 1; + currentProfile.controlRateConfig.rcRate8 = 90; + currentProfile.controlRateConfig.rcExpo8 = 65; + currentProfile.controlRateConfig.rollPitchRate = 0; + currentProfile.controlRateConfig.yawRate = 0; + currentProfile.dynThrPID = 0; + currentProfile.tpaBreakPoint = 1500; + currentProfile.controlRateConfig.thrMid8 = 50; + currentProfile.controlRateConfig.thrExpo8 = 0; // for (i = 0; i < CHECKBOXITEMS; i++) // cfg.activate[i] = 0; - cfg.angleTrim[0] = 0; - cfg.angleTrim[1] = 0; - cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. - cfg.acc_lpf_factor = 4; - cfg.accz_deadband = 40; - cfg.accxy_deadband = 40; - cfg.baro_tab_size = 21; - cfg.baro_noise_lpf = 0.6f; - cfg.baro_cf_vel = 0.985f; - cfg.baro_cf_alt = 0.965f; - cfg.acc_unarmedcal = 1; + currentProfile.angleTrim[0] = 0; + currentProfile.angleTrim[1] = 0; + currentProfile.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. + currentProfile.acc_lpf_factor = 4; + currentProfile.accz_deadband = 40; + currentProfile.accxy_deadband = 40; + currentProfile.baro_tab_size = 21; + currentProfile.baro_noise_lpf = 0.6f; + currentProfile.baro_cf_vel = 0.985f; + currentProfile.baro_cf_alt = 0.965f; + currentProfile.acc_unarmedcal = 1; // Radio - parseRcChannels("AETR1234", &mcfg.rxConfig); - cfg.deadband = 0; - cfg.yawdeadband = 0; - cfg.alt_hold_throttle_neutral = 40; - cfg.alt_hold_fast_change = 1; - cfg.throttle_correction_value = 0; // could 10 with althold or 40 for fpv - cfg.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv + parseRcChannels("AETR1234", &masterConfig.rxConfig); + currentProfile.deadband = 0; + currentProfile.yawdeadband = 0; + currentProfile.alt_hold_throttle_neutral = 40; + currentProfile.alt_hold_fast_change = 1; + currentProfile.throttle_correction_value = 0; // could 10 with althold or 40 for fpv + currentProfile.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv // Failsafe Variables - cfg.failsafeConfig.failsafe_delay = 10; // 1sec - cfg.failsafeConfig.failsafe_off_delay = 200; // 20sec - cfg.failsafeConfig.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people. - cfg.failsafeConfig.failsafe_detect_threshold = 985; // any of first 4 channels below this value will trigger failsafe + currentProfile.failsafeConfig.failsafe_delay = 10; // 1sec + currentProfile.failsafeConfig.failsafe_off_delay = 200; // 20sec + currentProfile.failsafeConfig.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people. + currentProfile.failsafeConfig.failsafe_detect_threshold = 985; // any of first 4 channels below this value will trigger failsafe // servos for (i = 0; i < 8; i++) { - cfg.servoConf[i].min = DEFAULT_SERVO_MIN; - cfg.servoConf[i].max = DEFAULT_SERVO_MAX; - cfg.servoConf[i].middle = DEFAULT_SERVO_MIDDLE; - cfg.servoConf[i].rate = servoRates[i]; - cfg.servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; + currentProfile.servoConf[i].min = DEFAULT_SERVO_MIN; + currentProfile.servoConf[i].max = DEFAULT_SERVO_MAX; + currentProfile.servoConf[i].middle = DEFAULT_SERVO_MIDDLE; + currentProfile.servoConf[i].rate = servoRates[i]; + currentProfile.servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; } - cfg.yaw_direction = 1; - cfg.tri_unarmed_servo = 1; + currentProfile.yaw_direction = 1; + currentProfile.tri_unarmed_servo = 1; // gimbal - cfg.gimbal_flags = GIMBAL_NORMAL; + currentProfile.gimbal_flags = GIMBAL_NORMAL; // gps/nav stuff - cfg.gps_wp_radius = 200; - cfg.gps_lpf = 20; - cfg.nav_slew_rate = 30; - cfg.nav_controls_heading = 1; - cfg.nav_speed_min = 100; - cfg.nav_speed_max = 300; - cfg.ap_mode = 40; + currentProfile.gps_wp_radius = 200; + currentProfile.gps_lpf = 20; + currentProfile.nav_slew_rate = 30; + currentProfile.nav_controls_heading = 1; + currentProfile.nav_speed_min = 100; + currentProfile.nav_speed_max = 300; + currentProfile.ap_mode = 40; // custom mixer. clear by defaults. for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) - mcfg.customMixer[i].throttle = 0.0f; + masterConfig.customMixer[i].throttle = 0.0f; // copy default config into all 3 profiles for (i = 0; i < 3; i++) - memcpy(&mcfg.profile[i], &cfg, sizeof(config_t)); + memcpy(&masterConfig.profile[i], ¤tProfile, sizeof(profile_t)); } bool feature(uint32_t mask) { - return mcfg.enabledFeatures & mask; + return masterConfig.enabledFeatures & mask; } void featureSet(uint32_t mask) { - mcfg.enabledFeatures |= mask; + masterConfig.enabledFeatures |= mask; } void featureClear(uint32_t mask) { - mcfg.enabledFeatures &= ~(mask); + masterConfig.enabledFeatures &= ~(mask); } void featureClearAll() { - mcfg.enabledFeatures = 0; + masterConfig.enabledFeatures = 0; } uint32_t featureMask(void) { - return mcfg.enabledFeatures; + return masterConfig.enabledFeatures; } diff --git a/src/config_storage.h b/src/config_master.h similarity index 51% rename from src/config_storage.h rename to src/config_master.h index 46cd000ccd..2ce2aed513 100644 --- a/src/config_storage.h +++ b/src/config_master.h @@ -1,62 +1,5 @@ #pragma once -typedef struct config_t { - uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671 - uint8_t P8[PID_ITEM_COUNT]; - uint8_t I8[PID_ITEM_COUNT]; - uint8_t D8[PID_ITEM_COUNT]; - - controlRateConfig_t controlRateConfig; - - uint8_t dynThrPID; - uint16_t tpaBreakPoint; // Breakpoint where TPA is activated - int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/ - int16_t angleTrim[ANGLE_INDEX_COUNT]; // accelerometer trim - - // sensor-related stuff - uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter - uint8_t accz_deadband; // set the acc deadband for z-Axis, this ignores small accelerations - uint8_t accxy_deadband; // set the acc deadband for xy-Axis - uint8_t baro_tab_size; // size of baro filter array - float baro_noise_lpf; // additional LPF to reduce baro noise - float baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity) - float baro_cf_alt; // apply CF to use ACC for height estimation - uint8_t acc_unarmedcal; // turn automatic acc compensation on/off - - uint16_t activate[CHECKBOX_ITEM_COUNT]; // activate switches - - // Radio/ESC-related configuration - uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero. - uint8_t yawdeadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero. - uint8_t alt_hold_throttle_neutral; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40 - uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_throttle_neutral; when enabled, altitude changes slowly proportional to stick movement - uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg - uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle. - - // Servo-related stuff - servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration - - // Failsafe related configuration - failsafeConfig_t failsafeConfig; - - // mixer-related configuration - int8_t yaw_direction; - uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed - - // gimbal-related configuration - uint8_t gimbal_flags; // in servotilt mode, various things that affect stuff - - // gps-related stuff - uint16_t gps_wp_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm) - uint8_t gps_lpf; // Low pass filter cut frequency for derivative calculation (default 20Hz) - uint8_t nav_slew_rate; // Adds a rate control to nav output, will smoothen out nav angle spikes - uint8_t nav_controls_heading; // copter faces toward the navigation point, maghold must be enabled for it - uint16_t nav_speed_min; // cm/sec - uint16_t nav_speed_max; // cm/sec - uint16_t ap_mode; // Temporarily Disables GPS_HOLD_MODE to be able to make it possible to adjust the Hold-position when moving the sticks, creating a deadspan for GPS -} config_t; - - // System-wide typedef struct master_t { uint8_t version; @@ -116,12 +59,12 @@ typedef struct master_t { uint8_t telemetry_provider; // See TelemetryProvider enum. uint8_t telemetry_port; // See TelemetryPort enum. uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed. - config_t profile[3]; // 3 separate profiles + profile_t profile[3]; // 3 separate profiles uint8_t current_profile; // currently loaded profile uint8_t magic_ef; // magic number, should be 0xEF uint8_t chk; // XOR checksum } master_t; -extern master_t mcfg; -extern config_t cfg; +extern master_t masterConfig; +extern profile_t currentProfile; diff --git a/src/config_profile.h b/src/config_profile.h new file mode 100644 index 0000000000..5a6a849295 --- /dev/null +++ b/src/config_profile.h @@ -0,0 +1,57 @@ +#pragma once + +typedef struct profile_s { + uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671 + uint8_t P8[PID_ITEM_COUNT]; + uint8_t I8[PID_ITEM_COUNT]; + uint8_t D8[PID_ITEM_COUNT]; + + controlRateConfig_t controlRateConfig; + + uint8_t dynThrPID; + uint16_t tpaBreakPoint; // Breakpoint where TPA is activated + int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/ + int16_t angleTrim[ANGLE_INDEX_COUNT]; // accelerometer trim + + // sensor-related stuff + uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter + uint8_t accz_deadband; // set the acc deadband for z-Axis, this ignores small accelerations + uint8_t accxy_deadband; // set the acc deadband for xy-Axis + uint8_t baro_tab_size; // size of baro filter array + float baro_noise_lpf; // additional LPF to reduce baro noise + float baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity) + float baro_cf_alt; // apply CF to use ACC for height estimation + uint8_t acc_unarmedcal; // turn automatic acc compensation on/off + + uint16_t activate[CHECKBOX_ITEM_COUNT]; // activate switches + + // Radio/ESC-related configuration + uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero. + uint8_t yawdeadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero. + uint8_t alt_hold_throttle_neutral; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40 + uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_throttle_neutral; when enabled, altitude changes slowly proportional to stick movement + uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg + uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle. + + // Servo-related stuff + servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration + + // Failsafe related configuration + failsafeConfig_t failsafeConfig; + + // mixer-related configuration + int8_t yaw_direction; + uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed + + // gimbal-related configuration + uint8_t gimbal_flags; // in servotilt mode, various things that affect stuff + + // gps-related stuff + uint16_t gps_wp_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm) + uint8_t gps_lpf; // Low pass filter cut frequency for derivative calculation (default 20Hz) + uint8_t nav_slew_rate; // Adds a rate control to nav output, will smoothen out nav angle spikes + uint8_t nav_controls_heading; // copter faces toward the navigation point, maghold must be enabled for it + uint16_t nav_speed_min; // cm/sec + uint16_t nav_speed_max; // cm/sec + uint16_t ap_mode; // Temporarily Disables GPS_HOLD_MODE to be able to make it possible to adjust the Hold-position when moving the sticks, creating a deadspan for GPS +} profile_t; diff --git a/src/flight_imu.c b/src/flight_imu.c index 60a4d89376..8a99d5ea7f 100644 --- a/src/flight_imu.c +++ b/src/flight_imu.c @@ -28,7 +28,8 @@ #include "failsafe.h" #include "runtime_config.h" #include "config.h" -#include "config_storage.h" +#include "config_profile.h" +#include "config_master.h" int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT]; @@ -75,7 +76,7 @@ void imuInit(void) { smallAngle = lrintf(acc_1G * cosf(RAD * 25.0f)); accVelScale = 9.80665f / acc_1G / 10000.0f; - throttleAngleScale = (1800.0f / M_PI) * (900.0f / cfg.throttle_correction_angle); + throttleAngleScale = (1800.0f / M_PI) * (900.0f / currentProfile.throttle_correction_angle); #ifdef MAG // if mag sensor is enabled, use it @@ -99,7 +100,7 @@ void computeIMU(void) accADC[Z] = 0; } - if (mcfg.mixerConfiguration == MULTITYPE_TRI) { + if (masterConfig.mixerConfiguration == MULTITYPE_TRI) { gyroData[GI_YAW] = (gyroYawSmooth * 2 + gyroADC[GI_YAW]) / 3; gyroYawSmooth = gyroData[GI_YAW]; gyroData[GI_ROLL] = gyroADC[GI_ROLL]; @@ -124,9 +125,6 @@ void computeIMU(void) // // ************************************************** -#define INV_GYR_CMPF_FACTOR (1.0f / ((float)mcfg.gyro_cmpf_factor + 1.0f)) -#define INV_GYR_CMPFM_FACTOR (1.0f / ((float)mcfg.gyro_cmpfm_factor + 1.0f)) - typedef struct fp_vector { float X; float Y; @@ -241,7 +239,7 @@ void acc_calc(uint32_t deltaT) rotateAnglesV(&accel_ned.V, &rpy); - if (cfg.acc_unarmedcal == 1) { + if (currentProfile.acc_unarmedcal == 1) { if (!f.ARMED) { accZoffset -= accZoffset / 64; accZoffset += accel_ned.V.Z; @@ -253,9 +251,9 @@ void acc_calc(uint32_t deltaT) accz_smooth = accz_smooth + (deltaT / (fc_acc + deltaT)) * (accel_ned.V.Z - accz_smooth); // low pass filter // apply Deadband to reduce integration drift and vibration influence - accSum[X] += applyDeadband(lrintf(accel_ned.V.X), cfg.accxy_deadband); - accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), cfg.accxy_deadband); - accSum[Z] += applyDeadband(lrintf(accz_smooth), cfg.accz_deadband); + accSum[X] += applyDeadband(lrintf(accel_ned.V.X), currentProfile.accxy_deadband); + accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), currentProfile.accxy_deadband); + accSum[Z] += applyDeadband(lrintf(accz_smooth), currentProfile.accz_deadband); // sum up Values for later integration to get velocity and distance accTimeSum += deltaT; @@ -308,8 +306,8 @@ static void getEstimatedAttitude(void) // Initialization for (axis = 0; axis < 3; axis++) { deltaGyroAngle[axis] = gyroADC[axis] * scale; - if (cfg.acc_lpf_factor > 0) { - accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / cfg.acc_lpf_factor)) + accADC[axis] * (1.0f / cfg.acc_lpf_factor); + if (currentProfile.acc_lpf_factor > 0) { + accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / currentProfile.acc_lpf_factor)) + accADC[axis] * (1.0f / currentProfile.acc_lpf_factor); accSmooth[axis] = accLPF[axis]; } else { accSmooth[axis] = accADC[axis]; @@ -329,14 +327,20 @@ static void getEstimatedAttitude(void) // Apply complimentary filter (Gyro drift correction) // If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation. // To do that, we just skip filter, as EstV already rotated by Gyro + + float invGyroComplimentaryFilterFactor = (1.0f / ((float)masterConfig.gyro_cmpf_factor + 1.0f)); + if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) { for (axis = 0; axis < 3; axis++) - EstG.A[axis] = (EstG.A[axis] * (float)mcfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR; + EstG.A[axis] = (EstG.A[axis] * (float)masterConfig.gyro_cmpf_factor + accSmooth[axis]) * invGyroComplimentaryFilterFactor; } + // FIXME what does the _M_ mean? + float invGyroComplimentaryFilter_M_Factor = (1.0f / ((float)masterConfig.gyro_cmpfm_factor + 1.0f)); + if (sensors(SENSOR_MAG)) { for (axis = 0; axis < 3; axis++) - EstM.A[axis] = (EstM.A[axis] * (float)mcfg.gyro_cmpfm_factor + magADC[axis]) * INV_GYR_CMPFM_FACTOR; + EstM.A[axis] = (EstM.A[axis] * (float)masterConfig.gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor; } f.SMALL_ANGLE = (EstG.A[Z] > smallAngle); @@ -354,7 +358,7 @@ static void getEstimatedAttitude(void) acc_calc(deltaT); // rotate acc vector into earth frame - if (cfg.throttle_correction_value) { + if (currentProfile.throttle_correction_value) { float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z); @@ -364,7 +368,7 @@ static void getEstimatedAttitude(void) int angle = lrintf(acosf(cosZ) * throttleAngleScale); if (angle > 900) angle = 900; - throttleAngleCorrection = lrintf(cfg.throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f))) ; + throttleAngleCorrection = lrintf(currentProfile.throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f))) ; } } @@ -410,7 +414,7 @@ int getEstimatedAltitude(void) // Integrator - Altitude in cm accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2) - accAlt = accAlt * cfg.baro_cf_alt + (float)BaroAlt * (1.0f - cfg.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc) + accAlt = accAlt * currentProfile.baro_cf_alt + (float)BaroAlt * (1.0f - currentProfile.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc) EstAlt = accAlt; vel += vel_acc; @@ -430,7 +434,7 @@ int getEstimatedAltitude(void) // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity). // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay - vel = vel * cfg.baro_cf_vel + baroVel * (1 - cfg.baro_cf_vel); + vel = vel * currentProfile.baro_cf_vel + baroVel * (1 - currentProfile.baro_cf_vel); vel_tmp = lrintf(vel); // set vario @@ -439,20 +443,20 @@ int getEstimatedAltitude(void) // Altitude P-Controller error = constrain(AltHold - EstAlt, -500, 500); error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position - setVel = constrain((cfg.P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s + setVel = constrain((currentProfile.P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s // Velocity PID-Controller // P error = setVel - vel_tmp; - BaroPID = constrain((cfg.P8[PIDVEL] * error / 32), -300, +300); + BaroPID = constrain((currentProfile.P8[PIDVEL] * error / 32), -300, +300); // I - errorAltitudeI += (cfg.I8[PIDVEL] * error) / 8; + errorAltitudeI += (currentProfile.I8[PIDVEL] * error) / 8; errorAltitudeI = constrain(errorAltitudeI, -(1024 * 200), (1024 * 200)); BaroPID += errorAltitudeI / 1024; // I in range +/-200 // D - BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150); + BaroPID -= constrain(currentProfile.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150); accZ_old = accZ_tmp; return 1; diff --git a/src/flight_mixer.c b/src/flight_mixer.c index 603959d20d..2482075bcb 100755 --- a/src/flight_mixer.c +++ b/src/flight_mixer.c @@ -159,14 +159,14 @@ const mixer_t mixers[] = { int16_t determineServoMiddleOrForwardFromChannel(int nr) { - uint8_t channelToForwardFrom = cfg.servoConf[nr].forwardFromChannel; + uint8_t channelToForwardFrom = currentProfile.servoConf[nr].forwardFromChannel; if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom << MAX_SUPPORTED_RC_CHANNEL_COUNT) { return rcData[channelToForwardFrom]; } if (nr < MAX_SERVOS) { - return cfg.servoConf[nr].middle; + return currentProfile.servoConf[nr].middle; } return DEFAULT_SERVO_MIDDLE; @@ -180,7 +180,7 @@ int servoDirection(int nr, int lr) // rate[1] = roll_direction // rate[0] = pitch_direction // servo.rate is also used as gimbal gain multiplier (yeah) - if (cfg.servoConf[nr].rate & lr) + if (currentProfile.servoConf[nr].rate & lr) return -1; else return 1; @@ -191,26 +191,26 @@ void mixerInit(void) int i; // enable servos for mixes that require them. note, this shifts motor counts. - useServo = mixers[mcfg.mixerConfiguration].useServo; + useServo = mixers[masterConfig.mixerConfiguration].useServo; // if we want camstab/trig, that also enables servos, even if mixer doesn't if (feature(FEATURE_SERVO_TILT)) useServo = 1; - if (mcfg.mixerConfiguration == MULTITYPE_CUSTOM) { + if (masterConfig.mixerConfiguration == MULTITYPE_CUSTOM) { // load custom mixer into currentMixer for (i = 0; i < MAX_MOTORS; i++) { // check if done - if (mcfg.customMixer[i].throttle == 0.0f) + if (masterConfig.customMixer[i].throttle == 0.0f) break; - currentMixer[i] = mcfg.customMixer[i]; + currentMixer[i] = masterConfig.customMixer[i]; numberMotor++; } } else { - numberMotor = mixers[mcfg.mixerConfiguration].numberMotor; + numberMotor = mixers[masterConfig.mixerConfiguration].numberMotor; // copy motor-based mixers - if (mixers[mcfg.mixerConfiguration].motor) { + if (mixers[masterConfig.mixerConfiguration].motor) { for (i = 0; i < numberMotor; i++) - currentMixer[i] = mixers[mcfg.mixerConfiguration].motor[i]; + currentMixer[i] = mixers[masterConfig.mixerConfiguration].motor[i]; } } @@ -226,8 +226,8 @@ void mixerInit(void) } // set flag that we're on something with wings - if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || - mcfg.mixerConfiguration == MULTITYPE_AIRPLANE) + if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || + masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE) f.FIXED_WING = 1; else f.FIXED_WING = 0; @@ -240,7 +240,7 @@ void mixerResetMotors(void) int i; // set disarmed motor values for (i = 0; i < MAX_MOTORS; i++) - motor_disarmed[i] = feature(FEATURE_3D) ? mcfg.neutral3d : mcfg.mincommand; + motor_disarmed[i] = feature(FEATURE_3D) ? masterConfig.neutral3d : masterConfig.mincommand; } void mixerLoadMix(int index) @@ -251,12 +251,12 @@ void mixerLoadMix(int index) index++; // clear existing for (i = 0; i < MAX_MOTORS; i++) - mcfg.customMixer[i].throttle = 0.0f; + masterConfig.customMixer[i].throttle = 0.0f; // do we have anything here to begin with? if (mixers[index].motor != NULL) { for (i = 0; i < mixers[index].numberMotor; i++) - mcfg.customMixer[i] = mixers[index].motor[i]; + masterConfig.customMixer[i] = mixers[index].motor[i]; } } @@ -271,14 +271,14 @@ void writeServos(void) if (!useServo) return; - switch (mcfg.mixerConfiguration) { + switch (masterConfig.mixerConfiguration) { case MULTITYPE_BI: pwmWriteServo(0, servo[4]); pwmWriteServo(1, servo[5]); break; case MULTITYPE_TRI: - if (cfg.tri_unarmed_servo) { + if (currentProfile.tri_unarmed_servo) { // if unarmed flag set, we always move servo pwmWriteServo(0, servo[5]); } else { @@ -345,33 +345,33 @@ static void airplaneMixer(void) int i; if (!f.ARMED) - servo[7] = mcfg.mincommand; // Kill throttle when disarmed + servo[7] = masterConfig.mincommand; // Kill throttle when disarmed else - servo[7] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle); + servo[7] = constrain(rcCommand[THROTTLE], masterConfig.minthrottle, masterConfig.maxthrottle); motor[0] = servo[7]; #if 0 - if (cfg.flaperons) { + if (currentProfile.flaperons) { } #endif - if (mcfg.flaps_speed) { + if (masterConfig.flaps_speed) { // configure SERVO3 middle point in GUI to using an AUX channel for FLAPS control // use servo min, servo max and servo rate for proper endpoints adjust static int16_t slow_LFlaps; int16_t lFlap = determineServoMiddleOrForwardFromChannel(2); - lFlap = constrain(lFlap, cfg.servoConf[2].min, cfg.servoConf[2].max); - lFlap = mcfg.rxConfig.midrc - lFlap; // shouldn't this be servoConf[2].middle? + lFlap = constrain(lFlap, currentProfile.servoConf[2].min, currentProfile.servoConf[2].max); + lFlap = masterConfig.rxConfig.midrc - lFlap; // shouldn't this be servoConf[2].middle? if (slow_LFlaps < lFlap) - slow_LFlaps += mcfg.flaps_speed; + slow_LFlaps += masterConfig.flaps_speed; else if (slow_LFlaps > lFlap) - slow_LFlaps -= mcfg.flaps_speed; + slow_LFlaps -= masterConfig.flaps_speed; - servo[2] = ((int32_t)cfg.servoConf[2].rate * slow_LFlaps) / 100L; - servo[2] += mcfg.rxConfig.midrc; + servo[2] = ((int32_t)currentProfile.servoConf[2].rate * slow_LFlaps) / 100L; + servo[2] += masterConfig.rxConfig.midrc; } if (f.PASSTHRU_MODE) { // Direct passthru from RX @@ -387,7 +387,7 @@ static void airplaneMixer(void) servo[6] = axisPID[PITCH]; // Elevator } for (i = 3; i < 7; i++) { - servo[i] = ((int32_t)cfg.servoConf[i].rate * servo[i]) / 100L; // servo rates + servo[i] = ((int32_t)currentProfile.servoConf[i].rate * servo[i]) / 100L; // servo rates servo[i] += determineServoMiddleOrForwardFromChannel(i); } } @@ -405,10 +405,10 @@ void mixTable(void) // motors for non-servo mixes if (numberMotor > 1) for (i = 0; i < numberMotor; i++) - motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw; + motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -currentProfile.yaw_direction * axisPID[YAW] * currentMixer[i].yaw; // airplane / servo mixes - switch (mcfg.mixerConfiguration) { + switch (masterConfig.mixerConfiguration) { case MULTITYPE_BI: servo[4] = (servoDirection(4, 2) * axisPID[YAW]) + (servoDirection(4, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(4); // LEFT servo[5] = (servoDirection(5, 2) * axisPID[YAW]) + (servoDirection(5, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(5); // RIGHT @@ -419,8 +419,8 @@ void mixTable(void) break; case MULTITYPE_GIMBAL: - servo[0] = (((int32_t)cfg.servoConf[0].rate * angle[PITCH]) / 50) + determineServoMiddleOrForwardFromChannel(0); - servo[1] = (((int32_t)cfg.servoConf[1].rate * angle[ROLL]) / 50) + determineServoMiddleOrForwardFromChannel(1); + servo[0] = (((int32_t)currentProfile.servoConf[0].rate * angle[PITCH]) / 50) + determineServoMiddleOrForwardFromChannel(0); + servo[1] = (((int32_t)currentProfile.servoConf[1].rate * angle[ROLL]) / 50) + determineServoMiddleOrForwardFromChannel(1); break; case MULTITYPE_AIRPLANE: @@ -429,9 +429,9 @@ void mixTable(void) case MULTITYPE_FLYING_WING: if (!f.ARMED) - servo[7] = mcfg.mincommand; + servo[7] = masterConfig.mincommand; else - servo[7] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle); + servo[7] = constrain(rcCommand[THROTTLE], masterConfig.minthrottle, masterConfig.maxthrottle); motor[0] = servo[7]; if (f.PASSTHRU_MODE) { // do not use sensors for correction, simple 2 channel mixing @@ -469,28 +469,28 @@ void mixTable(void) servo[1] = determineServoMiddleOrForwardFromChannel(1); if (rcOptions[BOXCAMSTAB]) { - if (cfg.gimbal_flags & GIMBAL_MIXTILT) { - servo[0] -= (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50; - servo[1] += (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 + (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50; + if (currentProfile.gimbal_flags & GIMBAL_MIXTILT) { + servo[0] -= (-(int32_t)currentProfile.servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)currentProfile.servoConf[1].rate * angle[ROLL] / 50; + servo[1] += (-(int32_t)currentProfile.servoConf[0].rate) * angle[PITCH] / 50 + (int32_t)currentProfile.servoConf[1].rate * angle[ROLL] / 50; } else { - servo[0] += (int32_t)cfg.servoConf[0].rate * angle[PITCH] / 50; - servo[1] += (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50; + servo[0] += (int32_t)currentProfile.servoConf[0].rate * angle[PITCH] / 50; + servo[1] += (int32_t)currentProfile.servoConf[1].rate * angle[ROLL] / 50; } } } // constrain servos for (i = 0; i < MAX_SERVOS; i++) - servo[i] = constrain(servo[i], cfg.servoConf[i].min, cfg.servoConf[i].max); // limit the values + servo[i] = constrain(servo[i], currentProfile.servoConf[i].min, currentProfile.servoConf[i].max); // limit the values // forward AUX1-4 to servo outputs (not constrained) - if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) { + if (currentProfile.gimbal_flags & GIMBAL_FORWARDAUX) { int offset = 0; // offset servos based off number already used in mixer types // airplane and servo_tilt together can't be used - if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING) + if (masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE || masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING) offset = 4; - else if (mixers[mcfg.mixerConfiguration].useServo) + else if (mixers[masterConfig.mixerConfiguration].useServo) offset = 2; for (i = 0; i < 4; i++) pwmWriteServo(i + offset, rcData[AUX1 + i]); @@ -501,21 +501,21 @@ void mixTable(void) if (motor[i] > maxMotor) maxMotor = motor[i]; for (i = 0; i < numberMotor; i++) { - if (maxMotor > mcfg.maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max. - motor[i] -= maxMotor - mcfg.maxthrottle; + if (maxMotor > masterConfig.maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max. + motor[i] -= maxMotor - masterConfig.maxthrottle; if (feature(FEATURE_3D)) { if ((rcData[THROTTLE]) > 1500) { - motor[i] = constrain(motor[i], mcfg.deadband3d_high, mcfg.maxthrottle); + motor[i] = constrain(motor[i], masterConfig.deadband3d_high, masterConfig.maxthrottle); } else { - motor[i] = constrain(motor[i], mcfg.mincommand, mcfg.deadband3d_low); + motor[i] = constrain(motor[i], masterConfig.mincommand, masterConfig.deadband3d_low); } } else { - motor[i] = constrain(motor[i], mcfg.minthrottle, mcfg.maxthrottle); - if ((rcData[THROTTLE]) < mcfg.rxConfig.mincheck) { + motor[i] = constrain(motor[i], masterConfig.minthrottle, masterConfig.maxthrottle); + if ((rcData[THROTTLE]) < masterConfig.rxConfig.mincheck) { if (!feature(FEATURE_MOTOR_STOP)) - motor[i] = mcfg.minthrottle; + motor[i] = masterConfig.minthrottle; else - motor[i] = mcfg.mincommand; + motor[i] = masterConfig.mincommand; } } if (!f.ARMED) { diff --git a/src/gps_common.c b/src/gps_common.c index 76cd7fe0b4..1559ec08c0 100644 --- a/src/gps_common.c +++ b/src/gps_common.c @@ -93,7 +93,7 @@ void gpsInit(uint8_t baudrateIndex) gpsData.lastMessage = millis(); gpsData.errors = 0; // only RX is needed for NMEA-style GPS - if (mcfg.gps_type == GPS_NMEA) + if (masterConfig.gps_type == GPS_NMEA) mode = MODE_RX; gpsSetPIDs(); @@ -105,7 +105,7 @@ void gpsInit(uint8_t baudrateIndex) void gpsInitHardware(void) { - switch (mcfg.gps_type) { + switch (masterConfig.gps_type) { case GPS_NMEA: // nothing to do, just set baud rate and try receiving some stuff and see if it parses serialSetBaudRate(serialPorts.gpsport, gpsInitData[gpsData.baudrateIndex].baudrate); @@ -203,7 +203,7 @@ void gpsThread(void) static bool gpsNewFrame(uint8_t c) { - switch (mcfg.gps_type) { + switch (masterConfig.gps_type) { case GPS_NMEA: // NMEA case GPS_MTK_NMEA: // MTK in NMEA mode return gpsNewFrameNMEA(c); @@ -312,11 +312,11 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param) pid->derivative = (input - pid->last_input) / *dt; // Low pass filter cut frequency for derivative calculation - // Set to "1 / ( 2 * PI * gps_lpf )" -#define PID_FILTER (1.0f / (2.0f * M_PI * (float)cfg.gps_lpf)) + // Set to "1 / ( 2 * PI * gps_lpf ) + float pidFilter = (1.0f / (2.0f * M_PI * (float)currentProfile.gps_lpf)); // discrete low pass filter, cuts out the // high frequency noise that can drive the controller crazy - pid->derivative = pid->last_derivative + (*dt / (PID_FILTER + *dt)) * (pid->derivative - pid->last_derivative); + pid->derivative = pid->last_derivative + (*dt / (pidFilter + *dt)) * (pid->derivative - pid->last_derivative); // update state pid->last_input = input; pid->last_derivative = pid->derivative; @@ -471,13 +471,13 @@ static void gpsNewData(uint16_t c) break; case NAV_MODE_WP: - speed = GPS_calc_desired_speed(cfg.nav_speed_max, NAV_SLOW_NAV); // slow navigation + speed = GPS_calc_desired_speed(currentProfile.nav_speed_max, NAV_SLOW_NAV); // slow navigation // use error as the desired rate towards the target // Desired output is in nav_lat and nav_lon where 1deg inclination is 100 GPS_calc_nav_rate(speed); // Tail control - if (cfg.nav_controls_heading) { + if (currentProfile.nav_controls_heading) { if (NAV_TAIL_FIRST) { magHold = wrap_18000(nav_bearing - 18000) / 100; } else { @@ -485,7 +485,7 @@ static void gpsNewData(uint16_t c) } } // Are we there yet ?(within x meters of the destination) - if ((wp_distance <= cfg.gps_wp_radius) || check_missed_wp()) { // if yes switch to poshold mode + if ((wp_distance <= currentProfile.gps_wp_radius) || check_missed_wp()) { // if yes switch to poshold mode nav_mode = NAV_MODE_POSHOLD; if (NAV_SET_TAKEOFF_HEADING) { magHold = nav_takeoff_bearing; @@ -528,18 +528,18 @@ void GPS_reset_nav(void) // Get the relevant P I D values and set the PID controllers void gpsSetPIDs(void) { - posholdPID_PARAM.kP = (float)cfg.P8[PIDPOS] / 100.0f; - posholdPID_PARAM.kI = (float)cfg.I8[PIDPOS] / 100.0f; + posholdPID_PARAM.kP = (float)currentProfile.P8[PIDPOS] / 100.0f; + posholdPID_PARAM.kI = (float)currentProfile.I8[PIDPOS] / 100.0f; posholdPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; - poshold_ratePID_PARAM.kP = (float)cfg.P8[PIDPOSR] / 10.0f; - poshold_ratePID_PARAM.kI = (float)cfg.I8[PIDPOSR] / 100.0f; - poshold_ratePID_PARAM.kD = (float)cfg.D8[PIDPOSR] / 1000.0f; + poshold_ratePID_PARAM.kP = (float)currentProfile.P8[PIDPOSR] / 10.0f; + poshold_ratePID_PARAM.kI = (float)currentProfile.I8[PIDPOSR] / 100.0f; + poshold_ratePID_PARAM.kD = (float)currentProfile.D8[PIDPOSR] / 1000.0f; poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; - navPID_PARAM.kP = (float)cfg.P8[PIDNAVR] / 10.0f; - navPID_PARAM.kI = (float)cfg.I8[PIDNAVR] / 100.0f; - navPID_PARAM.kD = (float)cfg.D8[PIDNAVR] / 1000.0f; + navPID_PARAM.kP = (float)currentProfile.P8[PIDNAVR] / 10.0f; + navPID_PARAM.kI = (float)currentProfile.I8[PIDNAVR] / 100.0f; + navPID_PARAM.kD = (float)currentProfile.D8[PIDNAVR] / 1000.0f; navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; } @@ -597,7 +597,7 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon) nav_bearing = target_bearing; GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]); original_target_bearing = target_bearing; - waypoint_speed_gov = cfg.nav_speed_min; + waypoint_speed_gov = currentProfile.nav_speed_min; } //////////////////////////////////////////////////////////////////////////////////// @@ -773,7 +773,7 @@ static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow) max_speed = min(max_speed, wp_distance / 2); } else { max_speed = min(max_speed, wp_distance); - max_speed = max(max_speed, cfg.nav_speed_min); // go at least 100cm/s + max_speed = max(max_speed, currentProfile.nav_speed_min); // go at least 100cm/s } // limit the ramp up of the speed @@ -1273,9 +1273,9 @@ void updateGpsState(void) { float sin_yaw_y = sinf(heading * 0.0174532925f); float cos_yaw_x = cosf(heading * 0.0174532925f); - if (cfg.nav_slew_rate) { - nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -cfg.nav_slew_rate, cfg.nav_slew_rate); // TODO check this on uint8 - nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -cfg.nav_slew_rate, cfg.nav_slew_rate); + if (currentProfile.nav_slew_rate) { + nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -currentProfile.nav_slew_rate, currentProfile.nav_slew_rate); // TODO check this on uint8 + nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -currentProfile.nav_slew_rate, currentProfile.nav_slew_rate); GPS_angle[ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10; GPS_angle[PITCH] = (nav_rated[LON] * sin_yaw_y + nav_rated[LAT] * cos_yaw_x) / 10; } else { diff --git a/src/main.c b/src/main.c index ea5ac1206e..4fd9d68453 100755 --- a/src/main.c +++ b/src/main.c @@ -13,7 +13,6 @@ #include "telemetry_common.h" #include "boardalignment.h" #include "config.h" -#include "config_storage.h" #include "build_config.h" @@ -37,29 +36,29 @@ int main(void) serialPort_t* loopbackPort1 = NULL; serialPort_t* loopbackPort2 = NULL; #endif - systemInit(mcfg.emfAvoidance); + systemInit(masterConfig.emfAvoidance); initPrintfSupport(); ensureEEPROMContainsValidData(); readEEPROM(); // configure power ADC - if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9)) - adc_params.powerAdcChannel = mcfg.power_adc_channel; + if (masterConfig.power_adc_channel > 0 && (masterConfig.power_adc_channel == 1 || masterConfig.power_adc_channel == 9)) + adc_params.powerAdcChannel = masterConfig.power_adc_channel; else { adc_params.powerAdcChannel = 0; - mcfg.power_adc_channel = 0; + masterConfig.power_adc_channel = 0; } adcInit(&adc_params); - initBoardAlignment(&mcfg.boardAlignment); + initBoardAlignment(&masterConfig.boardAlignment); // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform sensorsSet(SENSORS_SET); mixerInit(); // when using airplane/wing mixer, servo/motor outputs are remapped - if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING) + if (masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE || masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING) pwm_params.airplane = true; else pwm_params.airplane = false; @@ -68,18 +67,18 @@ int main(void) pwm_params.usePPM = feature(FEATURE_PPM); pwm_params.enableInput = !feature(FEATURE_SERIALRX); // disable inputs if using spektrum pwm_params.useServos = isMixerUsingServos(); - pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX; - pwm_params.motorPwmRate = mcfg.motor_pwm_rate; - pwm_params.servoPwmRate = mcfg.servo_pwm_rate; + pwm_params.extraServos = currentProfile.gimbal_flags & GIMBAL_FORWARDAUX; + pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; + pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below) if (feature(FEATURE_3D)) - pwm_params.idlePulse = mcfg.neutral3d; + pwm_params.idlePulse = masterConfig.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors - pwm_params.servoCenterPulse = mcfg.rxConfig.midrc; - pwm_params.failsafeThreshold = cfg.failsafeConfig.failsafe_detect_threshold; + pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc; + pwm_params.failsafeThreshold = currentProfile.failsafeConfig.failsafe_detect_threshold; - switch (mcfg.power_adc_channel) { + switch (masterConfig.power_adc_channel) { case 1: pwm_params.adcChannel = PWM2; break; @@ -91,14 +90,14 @@ int main(void) break; } - failsafe = failsafeInit(&mcfg.rxConfig); + failsafe = failsafeInit(&masterConfig.rxConfig); buzzerInit(failsafe); pwmInit(&pwm_params, failsafe); - rxInit(&mcfg.rxConfig, failsafe); + rxInit(&masterConfig.rxConfig, failsafe); if (feature(FEATURE_GPS) && !feature(FEATURE_SERIALRX)) { - gpsInit(mcfg.gps_baudrate); + gpsInit(masterConfig.gps_baudrate); } #ifdef SONAR @@ -128,16 +127,16 @@ int main(void) // Check battery type/voltage if (feature(FEATURE_VBAT)) - batteryInit(&mcfg.batteryConfig); + batteryInit(&masterConfig.batteryConfig); - serialInit(&mcfg.serialConfig); + serialInit(&masterConfig.serialConfig); #ifndef FY90Q if (feature(FEATURE_SOFTSERIAL)) { //mcfg.softserial_baudrate = 19200; // Uncomment to override config value - setupSoftSerialPrimary(mcfg.serialConfig.softserial_baudrate, mcfg.serialConfig.softserial_1_inverted); - setupSoftSerialSecondary(mcfg.serialConfig.softserial_2_inverted); + setupSoftSerialPrimary(masterConfig.serialConfig.softserial_baudrate, masterConfig.serialConfig.softserial_1_inverted); + setupSoftSerialSecondary(masterConfig.serialConfig.softserial_2_inverted); #ifdef SOFTSERIAL_LOOPBACK loopbackPort1 = (serialPort_t*)&(softSerialPorts[0]); @@ -154,7 +153,7 @@ int main(void) initTelemetry(&serialPorts); previousTime = micros(); - if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL) + if (masterConfig.mixerConfiguration == MULTITYPE_GIMBAL) calibratingA = CALIBRATING_ACC_CYCLES; calibratingG = CALIBRATING_GYRO_CYCLES; calibratingB = CALIBRATING_BARO_CYCLES; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles @@ -192,6 +191,6 @@ int main(void) void HardFault_Handler(void) { // fall out of the sky - writeAllMotors(mcfg.mincommand); + writeAllMotors(masterConfig.mincommand); while (1); } diff --git a/src/mw.c b/src/mw.c index 23b6f8f546..20b1718c1f 100755 --- a/src/mw.c +++ b/src/mw.c @@ -79,22 +79,22 @@ void annexCode(void) static uint8_t vbatTimer = 0; // PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value - if (rcData[THROTTLE] < cfg.tpaBreakPoint) { + if (rcData[THROTTLE] < currentProfile.tpaBreakPoint) { prop2 = 100; } else { if (rcData[THROTTLE] < 2000) { - prop2 = 100 - (uint16_t)cfg.dynThrPID * (rcData[THROTTLE] - cfg.tpaBreakPoint) / (2000 - cfg.tpaBreakPoint); + prop2 = 100 - (uint16_t)currentProfile.dynThrPID * (rcData[THROTTLE] - currentProfile.tpaBreakPoint) / (2000 - currentProfile.tpaBreakPoint); } else { - prop2 = 100 - cfg.dynThrPID; + prop2 = 100 - currentProfile.dynThrPID; } } for (axis = 0; axis < 3; axis++) { - tmp = min(abs(rcData[axis] - mcfg.rxConfig.midrc), 500); + tmp = min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500); if (axis != 2) { // ROLL & PITCH - if (cfg.deadband) { - if (tmp > cfg.deadband) { - tmp -= cfg.deadband; + if (currentProfile.deadband) { + if (tmp > currentProfile.deadband) { + tmp -= currentProfile.deadband; } else { tmp = 0; } @@ -102,28 +102,28 @@ void annexCode(void) tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; - prop1 = 100 - (uint16_t)cfg.controlRateConfig.rollPitchRate * tmp / 500; + prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.rollPitchRate * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; } else { // YAW - if (cfg.yawdeadband) { - if (tmp > cfg.yawdeadband) { - tmp -= cfg.yawdeadband; + if (currentProfile.yawdeadband) { + if (tmp > currentProfile.yawdeadband) { + tmp -= currentProfile.yawdeadband; } else { tmp = 0; } } - rcCommand[axis] = tmp * -mcfg.yaw_control_direction; - prop1 = 100 - (uint16_t)cfg.controlRateConfig.yawRate * abs(tmp) / 500; + rcCommand[axis] = tmp * -masterConfig.yaw_control_direction; + prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.yawRate * abs(tmp) / 500; } - dynP8[axis] = (uint16_t)cfg.P8[axis] * prop1 / 100; - dynI8[axis] = (uint16_t)cfg.I8[axis] * prop1 / 100; - dynD8[axis] = (uint16_t)cfg.D8[axis] * prop1 / 100; - if (rcData[axis] < mcfg.rxConfig.midrc) + dynP8[axis] = (uint16_t)currentProfile.P8[axis] * prop1 / 100; + dynI8[axis] = (uint16_t)currentProfile.I8[axis] * prop1 / 100; + dynD8[axis] = (uint16_t)currentProfile.D8[axis] * prop1 / 100; + if (rcData[axis] < masterConfig.rxConfig.midrc) rcCommand[axis] = -rcCommand[axis]; } - tmp = constrain(rcData[THROTTLE], mcfg.rxConfig.mincheck, PWM_RANGE_MAX); - tmp = (uint32_t)(tmp - mcfg.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - mcfg.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000] + tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX); + tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000] tmp2 = tmp / 100; rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] @@ -237,15 +237,15 @@ static void pidMultiWii(void) for (axis = 0; axis < 3; axis++) { if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC // 50 degrees max inclination - errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)mcfg.max_angle_inclination), +mcfg.max_angle_inclination) - angle[axis] + cfg.angleTrim[axis]; - PTermACC = errorAngle * cfg.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result - PTermACC = constrain(PTermACC, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5); + errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)masterConfig.max_angle_inclination), +masterConfig.max_angle_inclination) - angle[axis] + currentProfile.angleTrim[axis]; + PTermACC = errorAngle * currentProfile.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result + PTermACC = constrain(PTermACC, -currentProfile.D8[PIDLEVEL] * 5, +currentProfile.D8[PIDLEVEL] * 5); errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp - ITermACC = (errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12; + ITermACC = (errorAngleI[axis] * currentProfile.I8[PIDLEVEL]) >> 12; } if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2) { // MODE relying on GYRO or YAW axis - error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis]; + error = (int32_t)rcCommand[axis] * 10 * 8 / currentProfile.P8[axis]; error -= gyroData[axis]; PTermGYRO = rcCommand[axis]; @@ -253,7 +253,7 @@ static void pidMultiWii(void) errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp if (abs(gyroData[axis]) > 640) errorGyroI[axis] = 0; - ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6; + ITermGYRO = (errorGyroI[axis] / 125 * currentProfile.I8[axis]) >> 6; } if (f.HORIZON_MODE && axis < 2) { PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500; @@ -296,19 +296,19 @@ static void pidRewrite(void) // -----Get the desired angle rate depending on flight mode if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC // calculate error and limit the angle to max configured inclination - errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)mcfg.max_angle_inclination), +mcfg.max_angle_inclination) - angle[axis] + cfg.angleTrim[axis]; // 16 bits is ok here + errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)masterConfig.max_angle_inclination), +masterConfig.max_angle_inclination) - angle[axis] + currentProfile.angleTrim[axis]; // 16 bits is ok here } if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) - AngleRateTmp = (((int32_t)(cfg.controlRateConfig.yawRate + 27) * rcCommand[2]) >> 5); + AngleRateTmp = (((int32_t)(currentProfile.controlRateConfig.yawRate + 27) * rcCommand[2]) >> 5); } else { if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID - AngleRateTmp = ((int32_t) (cfg.controlRateConfig.rollPitchRate + 27) * rcCommand[axis]) >> 4; + AngleRateTmp = ((int32_t) (currentProfile.controlRateConfig.rollPitchRate + 27) * rcCommand[axis]) >> 4; if (f.HORIZON_MODE) { // mix up angle error to desired AngleRateTmp to add a little auto-level feel - AngleRateTmp += (errorAngle * cfg.I8[PIDLEVEL]) >> 8; + AngleRateTmp += (errorAngle * currentProfile.I8[PIDLEVEL]) >> 8; } } else { // it's the ANGLE mode - control is angle based, so control loop is needed - AngleRateTmp = (errorAngle * cfg.P8[PIDLEVEL]) >> 4; + AngleRateTmp = (errorAngle * currentProfile.P8[PIDLEVEL]) >> 4; } } @@ -319,13 +319,13 @@ static void pidRewrite(void) RateError = AngleRateTmp - gyroData[axis]; // -----calculate P component - PTerm = (RateError * cfg.P8[axis]) >> 7; + PTerm = (RateError * currentProfile.P8[axis]) >> 7; // -----calculate I component // there should be no division before accumulating the error to integrator, because the precision would be reduced. // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. // Time correction (to avoid different I scaling for different builds based on average cycle time) // is normalized to cycle time = 2048. - errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * cfg.I8[axis]; + errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * currentProfile.I8[axis]; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings @@ -343,7 +343,7 @@ static void pidRewrite(void) deltaSum = delta1[axis] + delta2[axis] + delta; delta2[axis] = delta1[axis]; delta1[axis] = delta; - DTerm = (deltaSum * cfg.D8[axis]) >> 8; + DTerm = (deltaSum * currentProfile.D8[axis]) >> 8; // -----calculate total PID output axisPID[axis] = PTerm + ITerm + DTerm; @@ -381,7 +381,7 @@ void loop(void) // calculate rc stuff from serial-based receivers (spek/sbus) if (feature(FEATURE_SERIALRX)) { - switch (mcfg.rxConfig.serialrx_type) { + switch (masterConfig.rxConfig.serialrx_type) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: rcReady = spektrumFrameComplete(); @@ -398,7 +398,7 @@ void loop(void) if (((int32_t)(currentTime - rcTime) >= 0) || rcReady) { // 50Hz or data driven rcReady = false; rcTime = currentTime + 20000; - computeRC(&mcfg.rxConfig, &rxRuntimeConfig); + computeRC(&masterConfig.rxConfig, &rxRuntimeConfig); // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { @@ -408,8 +408,8 @@ void loop(void) // Read value of AUX channel as rssi // 0 is disable, 1-4 is AUX{1..4} - if (mcfg.rssi_aux_channel > 0) { - const int16_t rssiChannelData = rcData[AUX1 + mcfg.rssi_aux_channel - 1]; + if (masterConfig.rssi_aux_channel > 0) { + const int16_t rssiChannelData = rcData[AUX1 + masterConfig.rssi_aux_channel - 1]; // Range of rssiChannelData is [1000;2000]. rssi should be in [0;1023]; rssi = (uint16_t)((constrain(rssiChannelData - 1000, 0, 1000) / 1000.0f) * 1023.0f); } @@ -422,9 +422,9 @@ void loop(void) // checking sticks positions for (i = 0; i < 4; i++) { stTmp >>= 2; - if (rcData[i] > mcfg.rxConfig.mincheck) + if (rcData[i] > masterConfig.rxConfig.mincheck) stTmp |= 0x80; // check for MIN - if (rcData[i] < mcfg.rxConfig.maxcheck) + if (rcData[i] < masterConfig.rxConfig.maxcheck) stTmp |= 0x40; // check for MAX } if (stTmp == rcSticks) { @@ -435,9 +435,9 @@ void loop(void) rcSticks = stTmp; // perform actions - if (feature(FEATURE_3D) && (rcData[THROTTLE] > (mcfg.rxConfig.midrc - mcfg.deadband3d_throttle) && rcData[THROTTLE] < (mcfg.rxConfig.midrc + mcfg.deadband3d_throttle))) + if (feature(FEATURE_3D) && (rcData[THROTTLE] > (masterConfig.rxConfig.midrc - masterConfig.deadband3d_throttle) && rcData[THROTTLE] < (masterConfig.rxConfig.midrc + masterConfig.deadband3d_throttle))) isThrottleLow = true; - else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < mcfg.rxConfig.mincheck)) + else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < masterConfig.rxConfig.mincheck)) isThrottleLow = true; if (isThrottleLow) { errorGyroI[ROLL] = 0; @@ -445,7 +445,7 @@ void loop(void) errorGyroI[YAW] = 0; errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; - if (cfg.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX + if (currentProfile.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX if (rcOptions[BOXARM] && f.OK_TO_ARM) mwArm(); else if (f.ARMED) @@ -456,10 +456,10 @@ void loop(void) if (rcDelayCommand == 20) { if (f.ARMED) { // actions during armed // Disarm on throttle down + yaw - if (cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE)) + if (currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE)) mwDisarm(); // Disarm on roll (only when retarded_arm is enabled) - if (mcfg.retarded_arm && cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO)) + if (masterConfig.retarded_arm && currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO)) mwDisarm(); } else { // actions during not armed i = 0; @@ -495,7 +495,7 @@ void loop(void) else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3 i = 3; if (i) { - mcfg.current_profile = i - 1; + masterConfig.current_profile = i - 1; writeEEPROM(); readEEPROM(); blinkLedAndSoundBeeper(2, 40, i); @@ -503,10 +503,10 @@ void loop(void) } // Arm via YAW - if (cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE)) + if (currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE)) mwArm(); // Arm via ROLL - else if (mcfg.retarded_arm && cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI)) + else if (masterConfig.retarded_arm && currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI)) mwArm(); // Calibrating Acc else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) @@ -517,20 +517,20 @@ void loop(void) i = 0; // Acc Trim if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) { - cfg.angleTrim[PITCH] += 2; + currentProfile.angleTrim[PITCH] += 2; i = 1; } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) { - cfg.angleTrim[PITCH] -= 2; + currentProfile.angleTrim[PITCH] -= 2; i = 1; } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) { - cfg.angleTrim[ROLL] += 2; + currentProfile.angleTrim[ROLL] += 2; i = 1; } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) { - cfg.angleTrim[ROLL] -= 2; + currentProfile.angleTrim[ROLL] -= 2; i = 1; } if (i) { - copyCurrentProfileToProfileSlot(mcfg.current_profile); + copyCurrentProfileToProfileSlot(masterConfig.current_profile); writeEEPROM(); readEEPROMAndNotify(); rcDelayCommand = 0; // allow autorepetition @@ -539,7 +539,7 @@ void loop(void) } if (feature(FEATURE_INFLIGHT_ACC_CAL)) { - if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > mcfg.rxConfig.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement + if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > masterConfig.rxConfig.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement InflightcalibratingA = 50; AccInflightCalibrationArmed = false; } @@ -557,7 +557,7 @@ void loop(void) for (i = 0; i < 4; i++) auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | (rcData[AUX1 + i] > 1700) << (3 * i + 2); for (i = 0; i < CHECKBOX_ITEM_COUNT; i++) - rcOptions[i] = (auxState & cfg.activate[i]) > 0; + rcOptions[i] = (auxState & currentProfile.activate[i]) > 0; if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode @@ -652,7 +652,7 @@ void loop(void) } } else { f.GPS_HOME_MODE = 0; - if (rcOptions[BOXGPSHOLD] && abs(rcCommand[ROLL]) < cfg.ap_mode && abs(rcCommand[PITCH]) < cfg.ap_mode) { + if (rcOptions[BOXGPSHOLD] && abs(rcCommand[ROLL]) < currentProfile.ap_mode && abs(rcCommand[PITCH]) < currentProfile.ap_mode) { if (!f.GPS_HOLD_MODE) { f.GPS_HOLD_MODE = 1; GPSNavReset = 0; @@ -683,7 +683,7 @@ void loop(void) f.PASSTHRU_MODE = 0; } - if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE) { + if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE) { f.HEADFREE_MODE = 0; } } else { // not in rc loop @@ -730,8 +730,8 @@ void loop(void) } currentTime = micros(); - if (mcfg.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) { - loopTime = currentTime + mcfg.looptime; + if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) { + loopTime = currentTime + masterConfig.looptime; computeIMU(); annexCode(); @@ -748,9 +748,9 @@ void loop(void) dif += 360; if (dif >= +180) dif -= 360; - dif *= -mcfg.yaw_control_direction; + dif *= -masterConfig.yaw_control_direction; if (f.SMALL_ANGLE) - rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; // 18 deg + rcCommand[YAW] -= dif * currentProfile.P8[PIDMAG] / 30; // 18 deg } else magHold = heading; } @@ -763,12 +763,12 @@ void loop(void) static int16_t AltHoldCorr = 0; if (!f.FIXED_WING) { // multirotor alt hold - if (cfg.alt_hold_fast_change) { + if (currentProfile.alt_hold_fast_change) { // rapid alt changes - if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) { + if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_throttle_neutral) { errorAltitudeI = 0; isAltHoldChanged = 1; - rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -cfg.alt_hold_throttle_neutral : cfg.alt_hold_throttle_neutral; + rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -currentProfile.alt_hold_throttle_neutral : currentProfile.alt_hold_throttle_neutral; } else { if (isAltHoldChanged) { AltHold = EstAlt; @@ -778,7 +778,7 @@ void loop(void) } } else { // slow alt changes for apfags - if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) { + if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_throttle_neutral) { // Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms) AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold; AltHold += AltHoldCorr / 2000; @@ -790,19 +790,19 @@ void loop(void) isAltHoldChanged = 0; } rcCommand[THROTTLE] = initialThrottleHold + BaroPID; - rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE], mcfg.minthrottle + 150, mcfg.maxthrottle); + rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE], masterConfig.minthrottle + 150, masterConfig.maxthrottle); } } else { // handle fixedwing-related althold. UNTESTED! and probably wrong // most likely need to check changes on pitch channel and 'reset' althold similar to // how throttle does it on multirotor - rcCommand[PITCH] += BaroPID * mcfg.fixedwing_althold_dir; + rcCommand[PITCH] += BaroPID * masterConfig.fixedwing_althold_dir; } } } #endif - if (cfg.throttle_correction_value && (f.ANGLE_MODE || f.HORIZON_MODE)) { + if (currentProfile.throttle_correction_value && (f.ANGLE_MODE || f.HORIZON_MODE)) { rcCommand[THROTTLE] += throttleAngleCorrection; } diff --git a/src/mw.h b/src/mw.h index daa8d6cfa3..1ec720eb69 100755 --- a/src/mw.h +++ b/src/mw.h @@ -35,7 +35,8 @@ enum { #include "serial_common.h" #include "rx_common.h" #include "config.h" -#include "config_storage.h" +#include "config_profile.h" +#include "config_master.h" extern int16_t axisPID[3]; extern int16_t rcCommand[4]; diff --git a/src/sensors_acceleration.c b/src/sensors_acceleration.c index 50d21051f0..84bbd3e7db 100644 --- a/src/sensors_acceleration.c +++ b/src/sensors_acceleration.c @@ -32,16 +32,16 @@ void ACC_Common(void) a[axis] += accADC[axis]; // Clear global variables for next reading accADC[axis] = 0; - mcfg.accZero[axis] = 0; + masterConfig.accZero[axis] = 0; } // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration if (calibratingA == 1) { - mcfg.accZero[GI_ROLL] = (a[GI_ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; - mcfg.accZero[GI_PITCH] = (a[GI_PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; - mcfg.accZero[GI_YAW] = (a[GI_YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G; - cfg.angleTrim[AI_ROLL] = 0; - cfg.angleTrim[AI_PITCH] = 0; - copyCurrentProfileToProfileSlot(mcfg.current_profile); + masterConfig.accZero[GI_ROLL] = (a[GI_ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; + masterConfig.accZero[GI_PITCH] = (a[GI_PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; + masterConfig.accZero[GI_YAW] = (a[GI_YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G; + currentProfile.angleTrim[AI_ROLL] = 0; + currentProfile.angleTrim[AI_PITCH] = 0; + copyCurrentProfileToProfileSlot(masterConfig.current_profile); writeEEPROM(); // write accZero in EEPROM readEEPROMAndNotify(); } @@ -54,11 +54,11 @@ void ACC_Common(void) static int16_t angleTrim_saved[2] = { 0, 0 }; // Saving old zeropoints before measurement if (InflightcalibratingA == 50) { - accZero_saved[GI_ROLL] = mcfg.accZero[GI_ROLL]; - accZero_saved[GI_PITCH] = mcfg.accZero[GI_PITCH]; - accZero_saved[GI_YAW] = mcfg.accZero[GI_YAW]; - angleTrim_saved[AI_ROLL] = cfg.angleTrim[AI_ROLL]; - angleTrim_saved[AI_PITCH] = cfg.angleTrim[AI_PITCH]; + accZero_saved[GI_ROLL] = masterConfig.accZero[GI_ROLL]; + accZero_saved[GI_PITCH] = masterConfig.accZero[GI_PITCH]; + accZero_saved[GI_YAW] = masterConfig.accZero[GI_YAW]; + angleTrim_saved[AI_ROLL] = currentProfile.angleTrim[AI_ROLL]; + angleTrim_saved[AI_PITCH] = currentProfile.angleTrim[AI_PITCH]; } if (InflightcalibratingA > 0) { for (axis = 0; axis < 3; axis++) { @@ -69,7 +69,7 @@ void ACC_Common(void) b[axis] += accADC[axis]; // Clear global variables for next reading accADC[axis] = 0; - mcfg.accZero[axis] = 0; + masterConfig.accZero[axis] = 0; } // all values are measured if (InflightcalibratingA == 1) { @@ -77,31 +77,31 @@ void ACC_Common(void) AccInflightCalibrationMeasurementDone = true; toggleBeep = 2; // buzzer for indicatiing the end of calibration // recover saved values to maintain current flight behavior until new values are transferred - mcfg.accZero[GI_ROLL] = accZero_saved[GI_ROLL]; - mcfg.accZero[GI_PITCH] = accZero_saved[GI_PITCH]; - mcfg.accZero[GI_YAW] = accZero_saved[GI_YAW]; - cfg.angleTrim[AI_ROLL] = angleTrim_saved[AI_ROLL]; - cfg.angleTrim[AI_PITCH] = angleTrim_saved[AI_PITCH]; + masterConfig.accZero[GI_ROLL] = accZero_saved[GI_ROLL]; + masterConfig.accZero[GI_PITCH] = accZero_saved[GI_PITCH]; + masterConfig.accZero[GI_YAW] = accZero_saved[GI_YAW]; + currentProfile.angleTrim[AI_ROLL] = angleTrim_saved[AI_ROLL]; + currentProfile.angleTrim[AI_PITCH] = angleTrim_saved[AI_PITCH]; } InflightcalibratingA--; } // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration if (AccInflightCalibrationSavetoEEProm) { // the copter is landed, disarmed and the combo has been done again AccInflightCalibrationSavetoEEProm = false; - mcfg.accZero[GI_ROLL] = b[GI_ROLL] / 50; - mcfg.accZero[GI_PITCH] = b[GI_PITCH] / 50; - mcfg.accZero[GI_YAW] = b[GI_YAW] / 50 - acc_1G; // for nunchuk 200=1G - cfg.angleTrim[AI_ROLL] = 0; - cfg.angleTrim[AI_PITCH] = 0; - copyCurrentProfileToProfileSlot(mcfg.current_profile); + masterConfig.accZero[GI_ROLL] = b[GI_ROLL] / 50; + masterConfig.accZero[GI_PITCH] = b[GI_PITCH] / 50; + masterConfig.accZero[GI_YAW] = b[GI_YAW] / 50 - acc_1G; // for nunchuk 200=1G + currentProfile.angleTrim[AI_ROLL] = 0; + currentProfile.angleTrim[AI_PITCH] = 0; + copyCurrentProfileToProfileSlot(masterConfig.current_profile); writeEEPROM(); // write accZero in EEPROM readEEPROMAndNotify(); } } - accADC[GI_ROLL] -= mcfg.accZero[GI_ROLL]; - accADC[GI_PITCH] -= mcfg.accZero[GI_PITCH]; - accADC[GI_YAW] -= mcfg.accZero[GI_YAW]; + accADC[GI_ROLL] -= masterConfig.accZero[GI_ROLL]; + accADC[GI_PITCH] -= masterConfig.accZero[GI_PITCH]; + accADC[GI_YAW] -= masterConfig.accZero[GI_YAW]; } void ACC_getADC(void) diff --git a/src/sensors_barometer.c b/src/sensors_barometer.c index 0799eea3d0..9002f3f425 100644 --- a/src/sensors_barometer.c +++ b/src/sensors_barometer.c @@ -16,7 +16,7 @@ void Baro_Common(void) int indexplus1; indexplus1 = (baroHistIdx + 1); - if (indexplus1 == cfg.baro_tab_size) + if (indexplus1 == currentProfile.baro_tab_size) indexplus1 = 0; baroHistTab[baroHistIdx] = baroPressure; baroPressureSum += baroHistTab[baroHistIdx]; @@ -58,9 +58,9 @@ int32_t Baro_calculateAltitude(void) // calculates height from ground via baro readings // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140 - BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / (cfg.baro_tab_size - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm + BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / (currentProfile.baro_tab_size - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm BaroAlt_tmp -= baroGroundAltitude; - BaroAlt = lrintf((float)BaroAlt * cfg.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf)); // additional LPF to reduce baro noise + BaroAlt = lrintf((float)BaroAlt * currentProfile.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - currentProfile.baro_noise_lpf)); // additional LPF to reduce baro noise return BaroAlt; } @@ -68,7 +68,7 @@ int32_t Baro_calculateAltitude(void) void performBaroCalibrationCycle(void) { baroGroundPressure -= baroGroundPressure / 8; - baroGroundPressure += baroPressureSum / (cfg.baro_tab_size - 1); + baroGroundPressure += baroPressureSum / (currentProfile.baro_tab_size - 1); baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f; calibratingB--; diff --git a/src/sensors_compass.c b/src/sensors_compass.c index b43b2d49bd..47af59c99d 100644 --- a/src/sensors_compass.c +++ b/src/sensors_compass.c @@ -35,7 +35,7 @@ int Mag_getADC(void) if (f.CALIBRATE_MAG) { tCal = t; for (axis = 0; axis < 3; axis++) { - mcfg.magZero[axis] = 0; + masterConfig.magZero[axis] = 0; magZeroTempMin[axis] = magADC[axis]; magZeroTempMax[axis] = magADC[axis]; } @@ -43,9 +43,9 @@ int Mag_getADC(void) } if (magInit) { // we apply offset only once mag calibration is done - magADC[X] -= mcfg.magZero[X]; - magADC[Y] -= mcfg.magZero[Y]; - magADC[Z] -= mcfg.magZero[Z]; + magADC[X] -= masterConfig.magZero[X]; + magADC[Y] -= masterConfig.magZero[Y]; + magADC[Z] -= masterConfig.magZero[Z]; } if (tCal != 0) { @@ -60,9 +60,9 @@ int Mag_getADC(void) } else { tCal = 0; for (axis = 0; axis < 3; axis++) { - mcfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets + masterConfig.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets } - copyCurrentProfileToProfileSlot(mcfg.current_profile); + copyCurrentProfileToProfileSlot(masterConfig.current_profile); writeEEPROM(); readEEPROMAndNotify(); } diff --git a/src/sensors_gyro.c b/src/sensors_gyro.c index d2c44aadf4..72ca464518 100644 --- a/src/sensors_gyro.c +++ b/src/sensors_gyro.c @@ -33,7 +33,7 @@ void GYRO_Common(void) if (calibratingG == 1) { float dev = devStandardDeviation(&var[axis]); // check deviation and startover if idiot was moving the model - if (mcfg.moron_threshold && dev > mcfg.moron_threshold) { + if (masterConfig.moron_threshold && dev > masterConfig.moron_threshold) { calibratingG = CALIBRATING_GYRO_CYCLES; devClear(&var[0]); devClear(&var[1]); diff --git a/src/sensors_initialisation.c b/src/sensors_initialisation.c index 266235949a..bc95a7b88c 100755 --- a/src/sensors_initialisation.c +++ b/src/sensors_initialisation.c @@ -38,12 +38,12 @@ void sensorsAutodetect(void) memset(&gyro, sizeof(gyro), 0); // Autodetect gyro hardware. We have MPU3050 or MPU6050. - if (mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf)) { + if (mpu6050Detect(&acc, &gyro, masterConfig.gyro_lpf)) { haveMpu6k = true; gyroAlign = CW0_DEG; // default NAZE alignment - } else if (l3g4200dDetect(&gyro, mcfg.gyro_lpf)) { + } else if (l3g4200dDetect(&gyro, masterConfig.gyro_lpf)) { gyroAlign = CW0_DEG; - } else if (mpu3050Detect(&gyro, mcfg.gyro_lpf)) { + } else if (mpu3050Detect(&gyro, masterConfig.gyro_lpf)) { gyroAlign = CW0_DEG; } else { // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error. @@ -52,7 +52,7 @@ void sensorsAutodetect(void) // Accelerometer. Fuck it. Let user break shit. retry: - switch (mcfg.acc_hardware) { + switch (masterConfig.acc_hardware) { case ACC_NONE: // disable ACC sensorsClear(SENSOR_ACC); break; @@ -64,15 +64,15 @@ retry: accHardware = ACC_ADXL345; accAlign = CW270_DEG; // default NAZE alignment } - if (mcfg.acc_hardware == ACC_ADXL345) + if (masterConfig.acc_hardware == ACC_ADXL345) break; ; // fallthrough case ACC_MPU6050: // MPU6050 if (haveMpu6k) { - mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf); // yes, i'm rerunning it again. re-fill acc struct + mpu6050Detect(&acc, &gyro, masterConfig.gyro_lpf); // yes, i'm rerunning it again. re-fill acc struct accHardware = ACC_MPU6050; accAlign = CW0_DEG; // default NAZE alignment - if (mcfg.acc_hardware == ACC_MPU6050) + if (masterConfig.acc_hardware == ACC_MPU6050) break; } ; // fallthrough @@ -81,7 +81,7 @@ retry: if (mma8452Detect(&acc)) { accHardware = ACC_MMA8452; accAlign = CW90_DEG; // default NAZE alignment - if (mcfg.acc_hardware == ACC_MMA8452) + if (masterConfig.acc_hardware == ACC_MMA8452) break; } ; // fallthrough @@ -89,7 +89,7 @@ retry: if (bma280Detect(&acc)) { accHardware = ACC_BMA280; accAlign = CW0_DEG; // - if (mcfg.acc_hardware == ACC_BMA280) + if (masterConfig.acc_hardware == ACC_BMA280) break; } #endif @@ -97,9 +97,9 @@ retry: // Found anything? Check if user fucked up or ACC is really missing. if (accHardware == ACC_DEFAULT) { - if (mcfg.acc_hardware > ACC_DEFAULT) { + if (masterConfig.acc_hardware > ACC_DEFAULT) { // Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present. - mcfg.acc_hardware = ACC_DEFAULT; + masterConfig.acc_hardware = ACC_DEFAULT; goto retry; } else { // We're really screwed @@ -118,14 +118,14 @@ retry: } #endif - if (mcfg.gyro_align != ALIGN_DEFAULT) { - gyroAlign = mcfg.gyro_align; + if (masterConfig.gyro_align != ALIGN_DEFAULT) { + gyroAlign = masterConfig.gyro_align; } - if (mcfg.acc_align != ALIGN_DEFAULT) { - accAlign = mcfg.acc_align; + if (masterConfig.acc_align != ALIGN_DEFAULT) { + accAlign = masterConfig.acc_align; } - if (mcfg.mag_align != ALIGN_DEFAULT) { - magAlign = mcfg.mag_align; + if (masterConfig.mag_align != ALIGN_DEFAULT) { + magAlign = masterConfig.mag_align; } @@ -145,8 +145,8 @@ retry: if (sensors(SENSOR_MAG)) { // calculate magnetic declination - deg = cfg.mag_declination / 100; - min = cfg.mag_declination % 100; + deg = currentProfile.mag_declination / 100; + min = currentProfile.mag_declination % 100; magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units } else { diff --git a/src/serial_cli.c b/src/serial_cli.c index 84cdff563a..6b8f6d7e43 100644 --- a/src/serial_cli.c +++ b/src/serial_cli.c @@ -122,117 +122,117 @@ typedef struct { } clivalue_t; const clivalue_t valueTable[] = { - { "looptime", VAR_UINT16, &mcfg.looptime, 0, 9000 }, - { "emf_avoidance", VAR_UINT8, &mcfg.emfAvoidance, 0, 1 }, - { "midrc", VAR_UINT16, &mcfg.rxConfig.midrc, 1200, 1700 }, - { "minthrottle", VAR_UINT16, &mcfg.minthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, - { "maxthrottle", VAR_UINT16, &mcfg.maxthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, - { "mincommand", VAR_UINT16, &mcfg.mincommand, PWM_RANGE_ZERO, PWM_RANGE_MAX }, - { "mincheck", VAR_UINT16, &mcfg.rxConfig.mincheck, PWM_RANGE_ZERO, PWM_RANGE_MAX }, - { "maxcheck", VAR_UINT16, &mcfg.rxConfig.maxcheck, PWM_RANGE_ZERO, PWM_RANGE_MAX }, - { "deadband3d_low", VAR_UINT16, &mcfg.deadband3d_low, PWM_RANGE_ZERO, PWM_RANGE_MAX }, - { "deadband3d_high", VAR_UINT16, &mcfg.deadband3d_high, PWM_RANGE_ZERO, PWM_RANGE_MAX }, - { "neutral3d", VAR_UINT16, &mcfg.neutral3d, PWM_RANGE_ZERO, PWM_RANGE_MAX }, - { "deadband3d_throttle", VAR_UINT16, &mcfg.deadband3d_throttle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, - { "motor_pwm_rate", VAR_UINT16, &mcfg.motor_pwm_rate, 50, 32000 }, - { "servo_pwm_rate", VAR_UINT16, &mcfg.servo_pwm_rate, 50, 498 }, - { "retarded_arm", VAR_UINT8, &mcfg.retarded_arm, 0, 1 }, - { "flaps_speed", VAR_UINT8, &mcfg.flaps_speed, 0, 100 }, - { "fixedwing_althold_dir", VAR_INT8, &mcfg.fixedwing_althold_dir, -1, 1 }, - { "reboot_character", VAR_UINT8, &mcfg.serialConfig.reboot_character, 48, 126 }, - { "serial_baudrate", VAR_UINT32, &mcfg.serialConfig.port1_baudrate, 1200, 115200 }, - { "softserial_baudrate", VAR_UINT32, &mcfg.serialConfig.softserial_baudrate, 1200, 19200 }, - { "softserial_1_inverted", VAR_UINT8, &mcfg.serialConfig.softserial_1_inverted, 0, 1 }, - { "softserial_2_inverted", VAR_UINT8, &mcfg.serialConfig.softserial_2_inverted, 0, 1 }, - { "gps_type", VAR_UINT8, &mcfg.gps_type, 0, GPS_HARDWARE_MAX }, - { "gps_baudrate", VAR_INT8, &mcfg.gps_baudrate, 0, GPS_BAUD_MAX }, - { "serialrx_type", VAR_UINT8, &mcfg.rxConfig.serialrx_type, 0, 3 }, - { "telemetry_provider", VAR_UINT8, &mcfg.telemetry_provider, 0, TELEMETRY_PROVIDER_MAX }, - { "telemetry_port", VAR_UINT8, &mcfg.telemetry_port, 0, TELEMETRY_PORT_MAX }, - { "telemetry_switch", VAR_UINT8, &mcfg.telemetry_switch, 0, 1 }, - { "vbatscale", VAR_UINT8, &mcfg.batteryConfig.vbatscale, 10, 200 }, - { "vbatmaxcellvoltage", VAR_UINT8, &mcfg.batteryConfig.vbatmaxcellvoltage, 10, 50 }, - { "vbatmincellvoltage", VAR_UINT8, &mcfg.batteryConfig.vbatmincellvoltage, 10, 50 }, - { "power_adc_channel", VAR_UINT8, &mcfg.power_adc_channel, 0, 9 }, - { "align_gyro", VAR_UINT8, &mcfg.gyro_align, 0, 8 }, - { "align_acc", VAR_UINT8, &mcfg.acc_align, 0, 8 }, - { "align_mag", VAR_UINT8, &mcfg.mag_align, 0, 8 }, - { "align_board_roll", VAR_INT16, &mcfg.boardAlignment.rollDegrees, -180, 360 }, - { "align_board_pitch", VAR_INT16, &mcfg.boardAlignment.pitchDegrees, -180, 360 }, - { "align_board_yaw", VAR_INT16, &mcfg.boardAlignment.yawDegrees, -180, 360 }, - { "yaw_control_direction", VAR_INT8, &mcfg.yaw_control_direction, -1, 1 }, - { "acc_hardware", VAR_UINT8, &mcfg.acc_hardware, 0, 5 }, - { "max_angle_inclination", VAR_UINT16, &mcfg.max_angle_inclination, 100, 900 }, - { "moron_threshold", VAR_UINT8, &mcfg.moron_threshold, 0, 128 }, - { "gyro_lpf", VAR_UINT16, &mcfg.gyro_lpf, 0, 256 }, - { "gyro_cmpf_factor", VAR_UINT16, &mcfg.gyro_cmpf_factor, 100, 1000 }, - { "gyro_cmpfm_factor", VAR_UINT16, &mcfg.gyro_cmpfm_factor, 100, 1000 }, - { "pid_controller", VAR_UINT8, &cfg.pidController, 0, 1 }, - { "deadband", VAR_UINT8, &cfg.deadband, 0, 32 }, - { "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 }, - { "alt_hold_throttle_neutral", VAR_UINT8, &cfg.alt_hold_throttle_neutral, 1, 250 }, - { "alt_hold_fast_change", VAR_UINT8, &cfg.alt_hold_fast_change, 0, 1 }, - { "throttle_correction_value", VAR_UINT8, &cfg.throttle_correction_value, 0, 150 }, - { "throttle_correction_angle", VAR_UINT16, &cfg.throttle_correction_angle, 1, 900 }, - { "rc_rate", VAR_UINT8, &cfg.controlRateConfig.rcRate8, 0, 250 }, - { "rc_expo", VAR_UINT8, &cfg.controlRateConfig.rcExpo8, 0, 100 }, - { "thr_mid", VAR_UINT8, &cfg.controlRateConfig.thrMid8, 0, 100 }, - { "thr_expo", VAR_UINT8, &cfg.controlRateConfig.thrExpo8, 0, 100 }, - { "roll_pitch_rate", VAR_UINT8, &cfg.controlRateConfig.rollPitchRate, 0, 100 }, - { "yawrate", VAR_UINT8, &cfg.controlRateConfig.yawRate, 0, 100 }, - { "tparate", VAR_UINT8, &cfg.dynThrPID, 0, 100}, - { "tpa_breakpoint", VAR_UINT16, &cfg.tpaBreakPoint, PWM_RANGE_MIN, PWM_RANGE_MAX}, - { "failsafe_delay", VAR_UINT8, &cfg.failsafeConfig.failsafe_delay, 0, 200 }, - { "failsafe_off_delay", VAR_UINT8, &cfg.failsafeConfig.failsafe_off_delay, 0, 200 }, - { "failsafe_throttle", VAR_UINT16, &cfg.failsafeConfig.failsafe_throttle, PWM_RANGE_MIN, PWM_RANGE_MAX }, - { "failsafe_detect_threshold", VAR_UINT16, &cfg.failsafeConfig.failsafe_detect_threshold, 100, PWM_RANGE_MAX }, - { "rssi_aux_channel", VAR_INT8, &mcfg.rssi_aux_channel, 0, 4 }, - { "yaw_direction", VAR_INT8, &cfg.yaw_direction, -1, 1 }, - { "tri_unarmed_servo", VAR_INT8, &cfg.tri_unarmed_servo, 0, 1 }, - { "gimbal_flags", VAR_UINT8, &cfg.gimbal_flags, 0, 255}, - { "acc_lpf_factor", VAR_UINT8, &cfg.acc_lpf_factor, 0, 250 }, - { "accxy_deadband", VAR_UINT8, &cfg.accxy_deadband, 0, 100 }, - { "accz_deadband", VAR_UINT8, &cfg.accz_deadband, 0, 100 }, - { "acc_unarmedcal", VAR_UINT8, &cfg.acc_unarmedcal, 0, 1 }, - { "acc_trim_pitch", VAR_INT16, &cfg.angleTrim[PITCH], -300, 300 }, - { "acc_trim_roll", VAR_INT16, &cfg.angleTrim[ROLL], -300, 300 }, - { "baro_tab_size", VAR_UINT8, &cfg.baro_tab_size, 0, BARO_TAB_SIZE_MAX }, - { "baro_noise_lpf", VAR_FLOAT, &cfg.baro_noise_lpf, 0, 1 }, - { "baro_cf_vel", VAR_FLOAT, &cfg.baro_cf_vel, 0, 1 }, - { "baro_cf_alt", VAR_FLOAT, &cfg.baro_cf_alt, 0, 1 }, - { "mag_declination", VAR_INT16, &cfg.mag_declination, -18000, 18000 }, - { "gps_pos_p", VAR_UINT8, &cfg.P8[PIDPOS], 0, 200 }, - { "gps_pos_i", VAR_UINT8, &cfg.I8[PIDPOS], 0, 200 }, - { "gps_pos_d", VAR_UINT8, &cfg.D8[PIDPOS], 0, 200 }, - { "gps_posr_p", VAR_UINT8, &cfg.P8[PIDPOSR], 0, 200 }, - { "gps_posr_i", VAR_UINT8, &cfg.I8[PIDPOSR], 0, 200 }, - { "gps_posr_d", VAR_UINT8, &cfg.D8[PIDPOSR], 0, 200 }, - { "gps_nav_p", VAR_UINT8, &cfg.P8[PIDNAVR], 0, 200 }, - { "gps_nav_i", VAR_UINT8, &cfg.I8[PIDNAVR], 0, 200 }, - { "gps_nav_d", VAR_UINT8, &cfg.D8[PIDNAVR], 0, 200 }, - { "gps_wp_radius", VAR_UINT16, &cfg.gps_wp_radius, 0, 2000 }, - { "nav_controls_heading", VAR_UINT8, &cfg.nav_controls_heading, 0, 1 }, - { "nav_speed_min", VAR_UINT16, &cfg.nav_speed_min, 10, 2000 }, - { "nav_speed_max", VAR_UINT16, &cfg.nav_speed_max, 10, 2000 }, - { "nav_slew_rate", VAR_UINT8, &cfg.nav_slew_rate, 0, 100 }, - { "p_pitch", VAR_UINT8, &cfg.P8[PITCH], 0, 200 }, - { "i_pitch", VAR_UINT8, &cfg.I8[PITCH], 0, 200 }, - { "d_pitch", VAR_UINT8, &cfg.D8[PITCH], 0, 200 }, - { "p_roll", VAR_UINT8, &cfg.P8[ROLL], 0, 200 }, - { "i_roll", VAR_UINT8, &cfg.I8[ROLL], 0, 200 }, - { "d_roll", VAR_UINT8, &cfg.D8[ROLL], 0, 200 }, - { "p_yaw", VAR_UINT8, &cfg.P8[YAW], 0, 200 }, - { "i_yaw", VAR_UINT8, &cfg.I8[YAW], 0, 200 }, - { "d_yaw", VAR_UINT8, &cfg.D8[YAW], 0, 200 }, - { "p_alt", VAR_UINT8, &cfg.P8[PIDALT], 0, 200 }, - { "i_alt", VAR_UINT8, &cfg.I8[PIDALT], 0, 200 }, - { "d_alt", VAR_UINT8, &cfg.D8[PIDALT], 0, 200 }, - { "p_level", VAR_UINT8, &cfg.P8[PIDLEVEL], 0, 200 }, - { "i_level", VAR_UINT8, &cfg.I8[PIDLEVEL], 0, 200 }, - { "d_level", VAR_UINT8, &cfg.D8[PIDLEVEL], 0, 200 }, - { "p_vel", VAR_UINT8, &cfg.P8[PIDVEL], 0, 200 }, - { "i_vel", VAR_UINT8, &cfg.I8[PIDVEL], 0, 200 }, - { "d_vel", VAR_UINT8, &cfg.D8[PIDVEL], 0, 200 }, + { "looptime", VAR_UINT16, &masterConfig.looptime, 0, 9000 }, + { "emf_avoidance", VAR_UINT8, &masterConfig.emfAvoidance, 0, 1 }, + { "midrc", VAR_UINT16, &masterConfig.rxConfig.midrc, 1200, 1700 }, + { "minthrottle", VAR_UINT16, &masterConfig.minthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, + { "maxthrottle", VAR_UINT16, &masterConfig.maxthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, + { "mincommand", VAR_UINT16, &masterConfig.mincommand, PWM_RANGE_ZERO, PWM_RANGE_MAX }, + { "mincheck", VAR_UINT16, &masterConfig.rxConfig.mincheck, PWM_RANGE_ZERO, PWM_RANGE_MAX }, + { "maxcheck", VAR_UINT16, &masterConfig.rxConfig.maxcheck, PWM_RANGE_ZERO, PWM_RANGE_MAX }, + { "deadband3d_low", VAR_UINT16, &masterConfig.deadband3d_low, PWM_RANGE_ZERO, PWM_RANGE_MAX }, + { "deadband3d_high", VAR_UINT16, &masterConfig.deadband3d_high, PWM_RANGE_ZERO, PWM_RANGE_MAX }, + { "neutral3d", VAR_UINT16, &masterConfig.neutral3d, PWM_RANGE_ZERO, PWM_RANGE_MAX }, + { "deadband3d_throttle", VAR_UINT16, &masterConfig.deadband3d_throttle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, + { "motor_pwm_rate", VAR_UINT16, &masterConfig.motor_pwm_rate, 50, 32000 }, + { "servo_pwm_rate", VAR_UINT16, &masterConfig.servo_pwm_rate, 50, 498 }, + { "retarded_arm", VAR_UINT8, &masterConfig.retarded_arm, 0, 1 }, + { "flaps_speed", VAR_UINT8, &masterConfig.flaps_speed, 0, 100 }, + { "fixedwing_althold_dir", VAR_INT8, &masterConfig.fixedwing_althold_dir, -1, 1 }, + { "reboot_character", VAR_UINT8, &masterConfig.serialConfig.reboot_character, 48, 126 }, + { "serial_baudrate", VAR_UINT32, &masterConfig.serialConfig.port1_baudrate, 1200, 115200 }, + { "softserial_baudrate", VAR_UINT32, &masterConfig.serialConfig.softserial_baudrate, 1200, 19200 }, + { "softserial_1_inverted", VAR_UINT8, &masterConfig.serialConfig.softserial_1_inverted, 0, 1 }, + { "softserial_2_inverted", VAR_UINT8, &masterConfig.serialConfig.softserial_2_inverted, 0, 1 }, + { "gps_type", VAR_UINT8, &masterConfig.gps_type, 0, GPS_HARDWARE_MAX }, + { "gps_baudrate", VAR_INT8, &masterConfig.gps_baudrate, 0, GPS_BAUD_MAX }, + { "serialrx_type", VAR_UINT8, &masterConfig.rxConfig.serialrx_type, 0, 3 }, + { "telemetry_provider", VAR_UINT8, &masterConfig.telemetry_provider, 0, TELEMETRY_PROVIDER_MAX }, + { "telemetry_port", VAR_UINT8, &masterConfig.telemetry_port, 0, TELEMETRY_PORT_MAX }, + { "telemetry_switch", VAR_UINT8, &masterConfig.telemetry_switch, 0, 1 }, + { "vbatscale", VAR_UINT8, &masterConfig.batteryConfig.vbatscale, 10, 200 }, + { "vbatmaxcellvoltage", VAR_UINT8, &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50 }, + { "vbatmincellvoltage", VAR_UINT8, &masterConfig.batteryConfig.vbatmincellvoltage, 10, 50 }, + { "power_adc_channel", VAR_UINT8, &masterConfig.power_adc_channel, 0, 9 }, + { "align_gyro", VAR_UINT8, &masterConfig.gyro_align, 0, 8 }, + { "align_acc", VAR_UINT8, &masterConfig.acc_align, 0, 8 }, + { "align_mag", VAR_UINT8, &masterConfig.mag_align, 0, 8 }, + { "align_board_roll", VAR_INT16, &masterConfig.boardAlignment.rollDegrees, -180, 360 }, + { "align_board_pitch", VAR_INT16, &masterConfig.boardAlignment.pitchDegrees, -180, 360 }, + { "align_board_yaw", VAR_INT16, &masterConfig.boardAlignment.yawDegrees, -180, 360 }, + { "yaw_control_direction", VAR_INT8, &masterConfig.yaw_control_direction, -1, 1 }, + { "acc_hardware", VAR_UINT8, &masterConfig.acc_hardware, 0, 5 }, + { "max_angle_inclination", VAR_UINT16, &masterConfig.max_angle_inclination, 100, 900 }, + { "moron_threshold", VAR_UINT8, &masterConfig.moron_threshold, 0, 128 }, + { "gyro_lpf", VAR_UINT16, &masterConfig.gyro_lpf, 0, 256 }, + { "gyro_cmpf_factor", VAR_UINT16, &masterConfig.gyro_cmpf_factor, 100, 1000 }, + { "gyro_cmpfm_factor", VAR_UINT16, &masterConfig.gyro_cmpfm_factor, 100, 1000 }, + { "pid_controller", VAR_UINT8, ¤tProfile.pidController, 0, 1 }, + { "deadband", VAR_UINT8, ¤tProfile.deadband, 0, 32 }, + { "yawdeadband", VAR_UINT8, ¤tProfile.yawdeadband, 0, 100 }, + { "alt_hold_throttle_neutral", VAR_UINT8, ¤tProfile.alt_hold_throttle_neutral, 1, 250 }, + { "alt_hold_fast_change", VAR_UINT8, ¤tProfile.alt_hold_fast_change, 0, 1 }, + { "throttle_correction_value", VAR_UINT8, ¤tProfile.throttle_correction_value, 0, 150 }, + { "throttle_correction_angle", VAR_UINT16, ¤tProfile.throttle_correction_angle, 1, 900 }, + { "rc_rate", VAR_UINT8, ¤tProfile.controlRateConfig.rcRate8, 0, 250 }, + { "rc_expo", VAR_UINT8, ¤tProfile.controlRateConfig.rcExpo8, 0, 100 }, + { "thr_mid", VAR_UINT8, ¤tProfile.controlRateConfig.thrMid8, 0, 100 }, + { "thr_expo", VAR_UINT8, ¤tProfile.controlRateConfig.thrExpo8, 0, 100 }, + { "roll_pitch_rate", VAR_UINT8, ¤tProfile.controlRateConfig.rollPitchRate, 0, 100 }, + { "yawrate", VAR_UINT8, ¤tProfile.controlRateConfig.yawRate, 0, 100 }, + { "tparate", VAR_UINT8, ¤tProfile.dynThrPID, 0, 100}, + { "tpa_breakpoint", VAR_UINT16, ¤tProfile.tpaBreakPoint, PWM_RANGE_MIN, PWM_RANGE_MAX}, + { "failsafe_delay", VAR_UINT8, ¤tProfile.failsafeConfig.failsafe_delay, 0, 200 }, + { "failsafe_off_delay", VAR_UINT8, ¤tProfile.failsafeConfig.failsafe_off_delay, 0, 200 }, + { "failsafe_throttle", VAR_UINT16, ¤tProfile.failsafeConfig.failsafe_throttle, PWM_RANGE_MIN, PWM_RANGE_MAX }, + { "failsafe_detect_threshold", VAR_UINT16, ¤tProfile.failsafeConfig.failsafe_detect_threshold, 100, PWM_RANGE_MAX }, + { "rssi_aux_channel", VAR_INT8, &masterConfig.rssi_aux_channel, 0, 4 }, + { "yaw_direction", VAR_INT8, ¤tProfile.yaw_direction, -1, 1 }, + { "tri_unarmed_servo", VAR_INT8, ¤tProfile.tri_unarmed_servo, 0, 1 }, + { "gimbal_flags", VAR_UINT8, ¤tProfile.gimbal_flags, 0, 255}, + { "acc_lpf_factor", VAR_UINT8, ¤tProfile.acc_lpf_factor, 0, 250 }, + { "accxy_deadband", VAR_UINT8, ¤tProfile.accxy_deadband, 0, 100 }, + { "accz_deadband", VAR_UINT8, ¤tProfile.accz_deadband, 0, 100 }, + { "acc_unarmedcal", VAR_UINT8, ¤tProfile.acc_unarmedcal, 0, 1 }, + { "acc_trim_pitch", VAR_INT16, ¤tProfile.angleTrim[PITCH], -300, 300 }, + { "acc_trim_roll", VAR_INT16, ¤tProfile.angleTrim[ROLL], -300, 300 }, + { "baro_tab_size", VAR_UINT8, ¤tProfile.baro_tab_size, 0, BARO_TAB_SIZE_MAX }, + { "baro_noise_lpf", VAR_FLOAT, ¤tProfile.baro_noise_lpf, 0, 1 }, + { "baro_cf_vel", VAR_FLOAT, ¤tProfile.baro_cf_vel, 0, 1 }, + { "baro_cf_alt", VAR_FLOAT, ¤tProfile.baro_cf_alt, 0, 1 }, + { "mag_declination", VAR_INT16, ¤tProfile.mag_declination, -18000, 18000 }, + { "gps_pos_p", VAR_UINT8, ¤tProfile.P8[PIDPOS], 0, 200 }, + { "gps_pos_i", VAR_UINT8, ¤tProfile.I8[PIDPOS], 0, 200 }, + { "gps_pos_d", VAR_UINT8, ¤tProfile.D8[PIDPOS], 0, 200 }, + { "gps_posr_p", VAR_UINT8, ¤tProfile.P8[PIDPOSR], 0, 200 }, + { "gps_posr_i", VAR_UINT8, ¤tProfile.I8[PIDPOSR], 0, 200 }, + { "gps_posr_d", VAR_UINT8, ¤tProfile.D8[PIDPOSR], 0, 200 }, + { "gps_nav_p", VAR_UINT8, ¤tProfile.P8[PIDNAVR], 0, 200 }, + { "gps_nav_i", VAR_UINT8, ¤tProfile.I8[PIDNAVR], 0, 200 }, + { "gps_nav_d", VAR_UINT8, ¤tProfile.D8[PIDNAVR], 0, 200 }, + { "gps_wp_radius", VAR_UINT16, ¤tProfile.gps_wp_radius, 0, 2000 }, + { "nav_controls_heading", VAR_UINT8, ¤tProfile.nav_controls_heading, 0, 1 }, + { "nav_speed_min", VAR_UINT16, ¤tProfile.nav_speed_min, 10, 2000 }, + { "nav_speed_max", VAR_UINT16, ¤tProfile.nav_speed_max, 10, 2000 }, + { "nav_slew_rate", VAR_UINT8, ¤tProfile.nav_slew_rate, 0, 100 }, + { "p_pitch", VAR_UINT8, ¤tProfile.P8[PITCH], 0, 200 }, + { "i_pitch", VAR_UINT8, ¤tProfile.I8[PITCH], 0, 200 }, + { "d_pitch", VAR_UINT8, ¤tProfile.D8[PITCH], 0, 200 }, + { "p_roll", VAR_UINT8, ¤tProfile.P8[ROLL], 0, 200 }, + { "i_roll", VAR_UINT8, ¤tProfile.I8[ROLL], 0, 200 }, + { "d_roll", VAR_UINT8, ¤tProfile.D8[ROLL], 0, 200 }, + { "p_yaw", VAR_UINT8, ¤tProfile.P8[YAW], 0, 200 }, + { "i_yaw", VAR_UINT8, ¤tProfile.I8[YAW], 0, 200 }, + { "d_yaw", VAR_UINT8, ¤tProfile.D8[YAW], 0, 200 }, + { "p_alt", VAR_UINT8, ¤tProfile.P8[PIDALT], 0, 200 }, + { "i_alt", VAR_UINT8, ¤tProfile.I8[PIDALT], 0, 200 }, + { "d_alt", VAR_UINT8, ¤tProfile.D8[PIDALT], 0, 200 }, + { "p_level", VAR_UINT8, ¤tProfile.P8[PIDLEVEL], 0, 200 }, + { "i_level", VAR_UINT8, ¤tProfile.I8[PIDLEVEL], 0, 200 }, + { "d_level", VAR_UINT8, ¤tProfile.D8[PIDLEVEL], 0, 200 }, + { "p_vel", VAR_UINT8, ¤tProfile.P8[PIDVEL], 0, 200 }, + { "i_vel", VAR_UINT8, ¤tProfile.I8[PIDVEL], 0, 200 }, + { "d_vel", VAR_UINT8, ¤tProfile.D8[PIDVEL], 0, 200 }, }; #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t)) @@ -447,14 +447,14 @@ static void cliAux(char *cmdline) if (len == 0) { // print out aux channel settings for (i = 0; i < CHECKBOX_ITEM_COUNT; i++) - printf("aux %u %u\r\n", i, cfg.activate[i]); + printf("aux %u %u\r\n", i, currentProfile.activate[i]); } else { ptr = cmdline; i = atoi(ptr); if (i < CHECKBOX_ITEM_COUNT) { ptr = strchr(cmdline, ' '); val = atoi(ptr); - cfg.activate[i] = val; + currentProfile.activate[i] = val; } else { printf("Invalid Feature index: must be < %u\r\n", CHECKBOX_ITEM_COUNT); } @@ -475,20 +475,20 @@ static void cliCMix(char *cmdline) if (len == 0) { cliPrint("Custom mixer: \r\nMotor\tThr\tRoll\tPitch\tYaw\r\n"); for (i = 0; i < MAX_MOTORS; i++) { - if (mcfg.customMixer[i].throttle == 0.0f) + if (masterConfig.customMixer[i].throttle == 0.0f) break; num_motors++; printf("#%d:\t", i + 1); - printf("%s\t", ftoa(mcfg.customMixer[i].throttle, buf)); - printf("%s\t", ftoa(mcfg.customMixer[i].roll, buf)); - printf("%s\t", ftoa(mcfg.customMixer[i].pitch, buf)); - printf("%s\r\n", ftoa(mcfg.customMixer[i].yaw, buf)); + printf("%s\t", ftoa(masterConfig.customMixer[i].throttle, buf)); + printf("%s\t", ftoa(masterConfig.customMixer[i].roll, buf)); + printf("%s\t", ftoa(masterConfig.customMixer[i].pitch, buf)); + printf("%s\r\n", ftoa(masterConfig.customMixer[i].yaw, buf)); } mixsum[0] = mixsum[1] = mixsum[2] = 0.0f; for (i = 0; i < num_motors; i++) { - mixsum[0] += mcfg.customMixer[i].roll; - mixsum[1] += mcfg.customMixer[i].pitch; - mixsum[2] += mcfg.customMixer[i].yaw; + mixsum[0] += masterConfig.customMixer[i].roll; + mixsum[1] += masterConfig.customMixer[i].pitch; + mixsum[2] += masterConfig.customMixer[i].yaw; } cliPrint("Sanity check:\t"); for (i = 0; i < 3; i++) @@ -498,7 +498,7 @@ static void cliCMix(char *cmdline) } else if (strncasecmp(cmdline, "reset", 5) == 0) { // erase custom mixer for (i = 0; i < MAX_MOTORS; i++) - mcfg.customMixer[i].throttle = 0.0f; + masterConfig.customMixer[i].throttle = 0.0f; } else if (strncasecmp(cmdline, "load", 4) == 0) { ptr = strchr(cmdline, ' '); if (ptr) { @@ -522,22 +522,22 @@ static void cliCMix(char *cmdline) if (--i < MAX_MOTORS) { ptr = strchr(ptr, ' '); if (ptr) { - mcfg.customMixer[i].throttle = _atof(++ptr); + masterConfig.customMixer[i].throttle = _atof(++ptr); check++; } ptr = strchr(ptr, ' '); if (ptr) { - mcfg.customMixer[i].roll = _atof(++ptr); + masterConfig.customMixer[i].roll = _atof(++ptr); check++; } ptr = strchr(ptr, ' '); if (ptr) { - mcfg.customMixer[i].pitch = _atof(++ptr); + masterConfig.customMixer[i].pitch = _atof(++ptr); check++; } ptr = strchr(ptr, ' '); if (ptr) { - mcfg.customMixer[i].yaw = _atof(++ptr); + masterConfig.customMixer[i].yaw = _atof(++ptr); check++; } if (check != 4) { @@ -565,17 +565,17 @@ static void cliDump(char *cmdline) cliAux(""); // print out current motor mix - printf("mixer %s\r\n", mixerNames[mcfg.mixerConfiguration - 1]); + printf("mixer %s\r\n", mixerNames[masterConfig.mixerConfiguration - 1]); // print custom mix if exists - if (mcfg.customMixer[0].throttle != 0.0f) { + if (masterConfig.customMixer[0].throttle != 0.0f) { for (i = 0; i < MAX_MOTORS; i++) { - if (mcfg.customMixer[i].throttle == 0.0f) + if (masterConfig.customMixer[i].throttle == 0.0f) break; - thr = mcfg.customMixer[i].throttle; - roll = mcfg.customMixer[i].roll; - pitch = mcfg.customMixer[i].pitch; - yaw = mcfg.customMixer[i].yaw; + thr = masterConfig.customMixer[i].throttle; + roll = masterConfig.customMixer[i].roll; + pitch = masterConfig.customMixer[i].pitch; + yaw = masterConfig.customMixer[i].yaw; printf("cmix %d", i + 1); if (thr < 0) printf(" "); @@ -609,7 +609,7 @@ static void cliDump(char *cmdline) // print RC MAPPING for (i = 0; i < 8; i++) - buf[mcfg.rxConfig.rcmap[i]] = rcChannelLetters[i]; + buf[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i]; buf[i] = '\0'; printf("map %s\r\n", buf); @@ -725,11 +725,11 @@ static void cliMap(char *cmdline) cliPrint("Must be any order of AETR1234\r\n"); return; } - parseRcChannels(cmdline, &mcfg.rxConfig); + parseRcChannels(cmdline, &masterConfig.rxConfig); } cliPrint("Current assignment: "); for (i = 0; i < 8; i++) - out[mcfg.rxConfig.rcmap[i]] = rcChannelLetters[i]; + out[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i]; out[i] = '\0'; printf("%s\r\n", out); } @@ -742,7 +742,7 @@ static void cliMixer(char *cmdline) len = strlen(cmdline); if (len == 0) { - printf("Current mixer: %s\r\n", mixerNames[mcfg.mixerConfiguration - 1]); + printf("Current mixer: %s\r\n", mixerNames[masterConfig.mixerConfiguration - 1]); return; } else if (strncasecmp(cmdline, "list", len) == 0) { cliPrint("Available mixers: "); @@ -761,7 +761,7 @@ static void cliMixer(char *cmdline) break; } if (strncasecmp(cmdline, mixerNames[i], len) == 0) { - mcfg.mixerConfiguration = i + 1; + masterConfig.mixerConfiguration = i + 1; printf("Mixer set to %s\r\n", mixerNames[i]); break; } @@ -821,12 +821,12 @@ static void cliProfile(char *cmdline) len = strlen(cmdline); if (len == 0) { - printf("Current profile: %d\r\n", mcfg.current_profile); + printf("Current profile: %d\r\n", masterConfig.current_profile); return; } else { i = atoi(cmdline); if (i >= 0 && i <= 2) { - mcfg.current_profile = i; + masterConfig.current_profile = i; writeEEPROM(); readEEPROM(); cliProfile(""); @@ -843,7 +843,7 @@ static void cliReboot(void) { static void cliSave(char *cmdline) { cliPrint("Saving..."); - copyCurrentProfileToProfileSlot(mcfg.current_profile); + copyCurrentProfileToProfileSlot(masterConfig.current_profile); writeEEPROM(); cliReboot(); } diff --git a/src/serial_msp.c b/src/serial_msp.c index b85bbcf8d9..c59e10c5e5 100755 --- a/src/serial_msp.c +++ b/src/serial_msp.c @@ -270,13 +270,13 @@ void mspInit(void) availableBoxes[idx++] = BOXGPSHOME; availableBoxes[idx++] = BOXGPSHOLD; } - if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE) + if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE) availableBoxes[idx++] = BOXPASSTHRU; availableBoxes[idx++] = BOXBEEPERON; if (feature(FEATURE_INFLIGHT_ACC_CAL)) availableBoxes[idx++] = BOXCALIB; availableBoxes[idx++] = BOXOSD; - if (feature(FEATURE_TELEMETRY && mcfg.telemetry_switch)) + if (feature(FEATURE_TELEMETRY && masterConfig.telemetry_switch)) availableBoxes[idx++] = BOXTELEMETRY; numberBoxItems = idx; } @@ -294,8 +294,8 @@ static void evaluateCommand(void) headSerialReply(0); break; case MSP_SET_ACC_TRIM: - cfg.angleTrim[PITCH] = read16(); - cfg.angleTrim[ROLL] = read16(); + currentProfile.angleTrim[PITCH] = read16(); + currentProfile.angleTrim[ROLL] = read16(); headSerialReply(0); break; case MSP_SET_RAW_GPS: @@ -310,39 +310,39 @@ static void evaluateCommand(void) break; case MSP_SET_PID: for (i = 0; i < PID_ITEM_COUNT; i++) { - cfg.P8[i] = read8(); - cfg.I8[i] = read8(); - cfg.D8[i] = read8(); + currentProfile.P8[i] = read8(); + currentProfile.I8[i] = read8(); + currentProfile.D8[i] = read8(); } headSerialReply(0); break; case MSP_SET_BOX: for (i = 0; i < numberBoxItems; i++) - cfg.activate[availableBoxes[i]] = read16(); + currentProfile.activate[availableBoxes[i]] = read16(); headSerialReply(0); break; case MSP_SET_RC_TUNING: - cfg.controlRateConfig.rcRate8 = read8(); - cfg.controlRateConfig.rcExpo8 = read8(); - cfg.controlRateConfig.rollPitchRate = read8(); - cfg.controlRateConfig.yawRate = read8(); - cfg.dynThrPID = read8(); - cfg.controlRateConfig.thrMid8 = read8(); - cfg.controlRateConfig.thrExpo8 = read8(); + currentProfile.controlRateConfig.rcRate8 = read8(); + currentProfile.controlRateConfig.rcExpo8 = read8(); + currentProfile.controlRateConfig.rollPitchRate = read8(); + currentProfile.controlRateConfig.yawRate = read8(); + currentProfile.dynThrPID = read8(); + currentProfile.controlRateConfig.thrMid8 = read8(); + currentProfile.controlRateConfig.thrExpo8 = read8(); headSerialReply(0); break; case MSP_SET_MISC: read16(); // powerfailmeter - mcfg.minthrottle = read16(); - mcfg.maxthrottle = read16(); - mcfg.mincommand = read16(); - cfg.failsafeConfig.failsafe_throttle = read16(); + masterConfig.minthrottle = read16(); + masterConfig.maxthrottle = read16(); + masterConfig.mincommand = read16(); + currentProfile.failsafeConfig.failsafe_throttle = read16(); read16(); read32(); - cfg.mag_declination = read16() * 10; - mcfg.batteryConfig.vbatscale = read8(); // actual vbatscale as intended - mcfg.batteryConfig.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI - mcfg.batteryConfig.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI + currentProfile.mag_declination = read16() * 10; + masterConfig.batteryConfig.vbatscale = read8(); // actual vbatscale as intended + masterConfig.batteryConfig.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI + masterConfig.batteryConfig.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI read8(); // vbatlevel_crit (unused) headSerialReply(0); break; @@ -353,9 +353,9 @@ static void evaluateCommand(void) break; case MSP_SELECT_SETTING: if (!f.ARMED) { - mcfg.current_profile = read8(); - if (mcfg.current_profile > 2) { - mcfg.current_profile = 0; + masterConfig.current_profile = read8(); + if (masterConfig.current_profile > 2) { + masterConfig.current_profile = 0; } writeEEPROM(); readEEPROM(); @@ -369,9 +369,9 @@ static void evaluateCommand(void) case MSP_IDENT: headSerialReply(7); serialize8(VERSION); // multiwii version - serialize8(mcfg.mixerConfiguration); // type of multicopter + serialize8(masterConfig.mixerConfiguration); // type of multicopter serialize8(MSP_VERSION); // MultiWii Serial Protocol Version - serialize32(CAP_PLATFORM_32BIT | CAP_DYNBALANCE | (mcfg.flaps_speed ? CAP_FLAPS : 0) | CAP_CHANNEL_FORWARDING); // "capability" + serialize32(CAP_PLATFORM_32BIT | CAP_DYNBALANCE | (masterConfig.flaps_speed ? CAP_FLAPS : 0) | CAP_CHANNEL_FORWARDING); // "capability" break; case MSP_STATUS: headSerialReply(11); @@ -402,7 +402,7 @@ static void evaluateCommand(void) junk |= 1 << i; } serialize32(junk); - serialize8(mcfg.current_profile); + serialize8(masterConfig.current_profile); break; case MSP_RAW_IMU: headSerialReply(18); @@ -425,38 +425,38 @@ static void evaluateCommand(void) case MSP_SERVO_CONF: headSerialReply(56); for (i = 0; i < MAX_SERVOS; i++) { - serialize16(cfg.servoConf[i].min); - serialize16(cfg.servoConf[i].max); - serialize16(cfg.servoConf[i].middle); - serialize8(cfg.servoConf[i].rate); + serialize16(currentProfile.servoConf[i].min); + serialize16(currentProfile.servoConf[i].max); + serialize16(currentProfile.servoConf[i].middle); + serialize8(currentProfile.servoConf[i].rate); } break; case MSP_SET_SERVO_CONF: headSerialReply(0); for (i = 0; i < MAX_SERVOS; i++) { - cfg.servoConf[i].min = read16(); - cfg.servoConf[i].max = read16(); + currentProfile.servoConf[i].min = read16(); + currentProfile.servoConf[i].max = read16(); // provide temporary support for old clients that try and send a channel index instead of a servo middle uint16_t potentialServoMiddleOrChannelToForward = read16(); if (potentialServoMiddleOrChannelToForward < MAX_SERVOS) { - cfg.servoConf[i].forwardFromChannel = potentialServoMiddleOrChannelToForward; + currentProfile.servoConf[i].forwardFromChannel = potentialServoMiddleOrChannelToForward; } if (potentialServoMiddleOrChannelToForward >= PWM_RANGE_MIN && potentialServoMiddleOrChannelToForward <= PWM_RANGE_MAX) { - cfg.servoConf[i].middle = potentialServoMiddleOrChannelToForward; + currentProfile.servoConf[i].middle = potentialServoMiddleOrChannelToForward; } - cfg.servoConf[i].rate = read8(); + currentProfile.servoConf[i].rate = read8(); } break; case MSP_CHANNEL_FORWARDING: headSerialReply(8); for (i = 0; i < MAX_SERVOS; i++) { - serialize8(cfg.servoConf[i].forwardFromChannel); + serialize8(currentProfile.servoConf[i].forwardFromChannel); } break; case MSP_SET_CHANNEL_FORWARDING: headSerialReply(0); for (i = 0; i < MAX_SERVOS; i++) { - cfg.servoConf[i].forwardFromChannel = read8(); + currentProfile.servoConf[i].forwardFromChannel = read8(); } break; case MSP_MOTOR: @@ -503,20 +503,20 @@ static void evaluateCommand(void) break; case MSP_RC_TUNING: headSerialReply(7); - serialize8(cfg.controlRateConfig.rcRate8); - serialize8(cfg.controlRateConfig.rcExpo8); - serialize8(cfg.controlRateConfig.rollPitchRate); - serialize8(cfg.controlRateConfig.yawRate); - serialize8(cfg.dynThrPID); - serialize8(cfg.controlRateConfig.thrMid8); - serialize8(cfg.controlRateConfig.thrExpo8); + serialize8(currentProfile.controlRateConfig.rcRate8); + serialize8(currentProfile.controlRateConfig.rcExpo8); + serialize8(currentProfile.controlRateConfig.rollPitchRate); + serialize8(currentProfile.controlRateConfig.yawRate); + serialize8(currentProfile.dynThrPID); + serialize8(currentProfile.controlRateConfig.thrMid8); + serialize8(currentProfile.controlRateConfig.thrExpo8); break; case MSP_PID: headSerialReply(3 * PID_ITEM_COUNT); for (i = 0; i < PID_ITEM_COUNT; i++) { - serialize8(cfg.P8[i]); - serialize8(cfg.I8[i]); - serialize8(cfg.D8[i]); + serialize8(currentProfile.P8[i]); + serialize8(currentProfile.I8[i]); + serialize8(currentProfile.D8[i]); } break; case MSP_PIDNAMES: @@ -526,7 +526,7 @@ static void evaluateCommand(void) case MSP_BOX: headSerialReply(2 * numberBoxItems); for (i = 0; i < numberBoxItems; i++) - serialize16(cfg.activate[availableBoxes[i]]); + serialize16(currentProfile.activate[availableBoxes[i]]); break; case MSP_BOXNAMES: // headSerialReply(sizeof(boxnames) - 1); @@ -540,16 +540,16 @@ static void evaluateCommand(void) case MSP_MISC: headSerialReply(2 * 6 + 4 + 2 + 4); serialize16(0); // intPowerTrigger1 (aka useless trash) - serialize16(mcfg.minthrottle); - serialize16(mcfg.maxthrottle); - serialize16(mcfg.mincommand); - serialize16(cfg.failsafeConfig.failsafe_throttle); + serialize16(masterConfig.minthrottle); + serialize16(masterConfig.maxthrottle); + serialize16(masterConfig.mincommand); + serialize16(currentProfile.failsafeConfig.failsafe_throttle); serialize16(0); // plog useless shit serialize32(0); // plog useless shit - serialize16(cfg.mag_declination / 10); // TODO check this shit - serialize8(mcfg.batteryConfig.vbatscale); - serialize8(mcfg.batteryConfig.vbatmincellvoltage); - serialize8(mcfg.batteryConfig.vbatmaxcellvoltage); + serialize16(currentProfile.mag_declination / 10); // TODO check this shit + serialize8(masterConfig.batteryConfig.vbatscale); + serialize8(masterConfig.batteryConfig.vbatmincellvoltage); + serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage); serialize8(0); break; case MSP_MOTOR_PINS: @@ -621,7 +621,7 @@ static void evaluateCommand(void) if (f.ARMED) { headSerialError(0); } else { - copyCurrentProfileToProfileSlot(mcfg.current_profile); + copyCurrentProfileToProfileSlot(masterConfig.current_profile); writeEEPROM(); readEEPROM(); headSerialReply(0); @@ -638,8 +638,8 @@ static void evaluateCommand(void) // Additional commands that are not compatible with MultiWii case MSP_ACC_TRIM: headSerialReply(4); - serialize16(cfg.angleTrim[PITCH]); - serialize16(cfg.angleTrim[ROLL]); + serialize16(currentProfile.angleTrim[PITCH]); + serialize16(currentProfile.angleTrim[ROLL]); break; case MSP_UID: headSerialReply(12); diff --git a/src/telemetry_common.c b/src/telemetry_common.c index ff667d20d9..c3f8e40713 100644 --- a/src/telemetry_common.c +++ b/src/telemetry_common.c @@ -13,12 +13,12 @@ static bool isTelemetryConfigurationValid = false; // flag used to avoid repeate bool isTelemetryProviderFrSky(void) { - return mcfg.telemetry_provider == TELEMETRY_PROVIDER_FRSKY; + return masterConfig.telemetry_provider == TELEMETRY_PROVIDER_FRSKY; } bool isTelemetryProviderHoTT(void) { - return mcfg.telemetry_provider == TELEMETRY_PROVIDER_HOTT; + return masterConfig.telemetry_provider == TELEMETRY_PROVIDER_HOTT; } bool canUseTelemetryWithCurrentConfiguration(void) @@ -28,14 +28,14 @@ bool canUseTelemetryWithCurrentConfiguration(void) } if (!feature(FEATURE_SOFTSERIAL)) { - if (mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1 || mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2) { + if (masterConfig.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1 || masterConfig.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2) { // softserial feature must be enabled to use telemetry on softserial ports return false; } } if (isTelemetryProviderHoTT()) { - if (mcfg.telemetry_port == TELEMETRY_PORT_UART) { + if (masterConfig.telemetry_port == TELEMETRY_PORT_UART) { // HoTT requires a serial port that supports RX/TX mode swapping return false; } @@ -48,20 +48,20 @@ void initTelemetry(serialPorts_t *serialPorts) { // Force telemetry to uart when softserial disabled if (!feature(FEATURE_SOFTSERIAL)) - mcfg.telemetry_port = TELEMETRY_PORT_UART; + masterConfig.telemetry_port = TELEMETRY_PORT_UART; #ifdef FY90Q // FY90Q does not support softserial - mcfg.telemetry_port = TELEMETRY_PORT_UART; + masterConfig.telemetry_port = TELEMETRY_PORT_UART; serialPorts->telemport = serialPorts->mainport; #endif isTelemetryConfigurationValid = canUseTelemetryWithCurrentConfiguration(); #ifndef FY90Q - if (mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1) + if (masterConfig.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1) serialPorts->telemport = &(softSerialPorts[0].port); - else if (mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2) + else if (masterConfig.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2) serialPorts->telemport = &(softSerialPorts[1].port); else serialPorts->telemport = serialPorts->mainport; @@ -76,8 +76,8 @@ bool determineNewTelemetryEnabledState(void) { bool enabled = true; - if (mcfg.telemetry_port == TELEMETRY_PORT_UART) { - if (!mcfg.telemetry_switch) + if (masterConfig.telemetry_port == TELEMETRY_PORT_UART) { + if (!masterConfig.telemetry_switch) enabled = f.ARMED; else enabled = rcOptions[BOXTELEMETRY]; diff --git a/src/telemetry_frsky.c b/src/telemetry_frsky.c index 7f4a34bdbd..f2654013c6 100644 --- a/src/telemetry_frsky.c +++ b/src/telemetry_frsky.c @@ -224,14 +224,14 @@ static void sendHeading(void) void freeFrSkyTelemetryPort(void) { - if (mcfg.telemetry_port == TELEMETRY_PORT_UART) { + if (masterConfig.telemetry_port == TELEMETRY_PORT_UART) { resetMainSerialPort(); } } void configureFrSkyTelemetryPort(void) { - if (mcfg.telemetry_port == TELEMETRY_PORT_UART) { + if (masterConfig.telemetry_port == TELEMETRY_PORT_UART) { openMainSerialPort(9600); } }