diff --git a/src/main/config/config.c b/src/main/config/config.c index 6ee0d97dc0..a02556cd71 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -492,27 +492,24 @@ static void resetConf(void) resetControlRateConfig(&masterConfig.controlRateProfiles[0]); - // for (i = 0; i < CHECKBOXITEMS; i++) - // cfg.activate[i] = 0; + resetRollAndPitchTrims(&masterConfig.accelerometerTrims); - resetRollAndPitchTrims(¤tProfile->accelerometerTrims); + masterConfig.mag_declination = 0; + masterConfig.acc_lpf_hz = 20; + masterConfig.accz_lpf_cutoff = 5.0f; + masterConfig.accDeadband.xy = 40; + masterConfig.accDeadband.z = 40; + masterConfig.acc_unarmedcal = 1; - currentProfile->mag_declination = 0; - currentProfile->acc_lpf_hz = 20; - currentProfile->accz_lpf_cutoff = 5.0f; - currentProfile->accDeadband.xy = 40; - currentProfile->accDeadband.z = 40; - currentProfile->acc_unarmedcal = 1; - - resetBarometerConfig(¤tProfile->barometerConfig); + resetBarometerConfig(&masterConfig.barometerConfig); // Radio parseRcChannels("AETR1234", &masterConfig.rxConfig); - resetRcControlsConfig(¤tProfile->rcControlsConfig); + resetRcControlsConfig(&masterConfig.rcControlsConfig); - 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 + masterConfig.throttle_correction_value = 0; // could 10 with althold or 40 for fpv + masterConfig.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv // Failsafe Variables masterConfig.failsafeConfig.failsafe_delay = 10; // 1sec @@ -524,21 +521,21 @@ static void resetConf(void) #ifdef USE_SERVOS // servos for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - 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 = 100; - currentProfile->servoConf[i].angleAtMin = DEFAULT_SERVO_MIN_ANGLE; - currentProfile->servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE; - currentProfile->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; + masterConfig.servoConf[i].min = DEFAULT_SERVO_MIN; + masterConfig.servoConf[i].max = DEFAULT_SERVO_MAX; + masterConfig.servoConf[i].middle = DEFAULT_SERVO_MIDDLE; + masterConfig.servoConf[i].rate = 100; + masterConfig.servoConf[i].angleAtMin = DEFAULT_SERVO_MIN_ANGLE; + masterConfig.servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE; + masterConfig.servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; } // gimbal - currentProfile->gimbalConfig.mode = GIMBAL_MODE_NORMAL; + masterConfig.gimbalConfig.mode = GIMBAL_MODE_NORMAL; #endif #ifdef GPS - resetGpsProfile(¤tProfile->gpsProfile); + resetGpsProfile(&masterConfig.gpsProfile); #endif // custom mixer. clear by defaults. @@ -779,7 +776,7 @@ void activateConfig(void) pidSetController(currentProfile->pidProfile.pidController); #ifdef GPS - gpsUseProfile(¤tProfile->gpsProfile); + gpsUseProfile(&masterConfig.gpsProfile); gpsUsePIDs(¤tProfile->pidProfile); #endif @@ -788,8 +785,8 @@ void activateConfig(void) mixerUseConfigs( #ifdef USE_SERVOS - currentProfile->servoConf, - ¤tProfile->gimbalConfig, + masterConfig.servoConf, + &masterConfig.gimbalConfig, #endif &masterConfig.flight3DConfig, &masterConfig.escAndServoConfig, @@ -800,27 +797,27 @@ void activateConfig(void) imuRuntimeConfig.dcm_kp = masterConfig.dcm_kp / 10000.0f; imuRuntimeConfig.dcm_ki = masterConfig.dcm_ki / 10000.0f; - imuRuntimeConfig.acc_cut_hz = currentProfile->acc_lpf_hz; - imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal; + imuRuntimeConfig.acc_cut_hz = masterConfig.acc_lpf_hz; + imuRuntimeConfig.acc_unarmedcal = masterConfig.acc_unarmedcal; imuRuntimeConfig.small_angle = masterConfig.small_angle; imuConfigure( &imuRuntimeConfig, ¤tProfile->pidProfile, - ¤tProfile->accDeadband, - currentProfile->accz_lpf_cutoff, - currentProfile->throttle_correction_angle + &masterConfig.accDeadband, + masterConfig.accz_lpf_cutoff, + masterConfig.throttle_correction_angle ); configureAltitudeHold( ¤tProfile->pidProfile, - ¤tProfile->barometerConfig, - ¤tProfile->rcControlsConfig, + &masterConfig.barometerConfig, + &masterConfig.rcControlsConfig, &masterConfig.escAndServoConfig ); #ifdef BARO - useBarometerConfig(¤tProfile->barometerConfig); + useBarometerConfig(&masterConfig.barometerConfig); #endif } diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 96e5830c36..f5ce7113c5 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -60,6 +60,40 @@ typedef struct master_t { uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device uint8_t baro_hardware; // Barometer hardware to use + int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/ + // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. + + rollAndPitchTrims_t accelerometerTrims; // accelerometer trim + + // sensor-related stuff + uint8_t acc_lpf_hz; // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter + float accz_lpf_cutoff; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz + accDeadband_t accDeadband; + + barometerConfig_t barometerConfig; + + uint8_t acc_unarmedcal; // turn automatic acc compensation on/off + + 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. + + // Radio/ESC-related configuration + rcControlsConfig_t rcControlsConfig; + + + +#ifdef USE_SERVOS + // Servo-related stuff + servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration + // gimbal-related configuration + gimbalConfig_t gimbalConfig; +#endif + +#ifdef GPS + gpsProfile_t gpsProfile; +#endif + + uint16_t max_angle_inclination; // max inclination allowed in angle (level) mode. default 500 (50 degrees). flightDynamicsTrims_t accZero; flightDynamicsTrims_t magZero; diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index 09bf545b53..2560744738 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -19,42 +19,5 @@ typedef struct profile_s { pidProfile_t pidProfile; - uint8_t defaultRateProfileIndex; - - int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/ - // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. - - rollAndPitchTrims_t accelerometerTrims; // accelerometer trim - - // sensor-related stuff - uint8_t acc_lpf_hz; // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter - float accz_lpf_cutoff; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz - accDeadband_t accDeadband; - - barometerConfig_t barometerConfig; - - uint8_t acc_unarmedcal; // turn automatic acc compensation on/off - - - - - - // Radio/ESC-related configuration - - rcControlsConfig_t rcControlsConfig; - - 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. - -#ifdef USE_SERVOS - // Servo-related stuff - servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration - // gimbal-related configuration - gimbalConfig_t gimbalConfig; -#endif - -#ifdef GPS - gpsProfile_t gpsProfile; -#endif } profile_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 24ba0412e1..d676e2b100 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -550,11 +550,11 @@ const clivalue_t valueTable[] = { { "gps_nav_p", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDNAVR], .config.minmax = { 0, 200 } }, { "gps_nav_i", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDNAVR], .config.minmax = { 0, 200 } }, { "gps_nav_d", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDNAVR], .config.minmax = { 0, 200 } }, - { "gps_wp_radius", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].gpsProfile.gps_wp_radius, .config.minmax = { 0, 2000 } }, - { "nav_controls_heading", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].gpsProfile.nav_controls_heading, .config.lookup = { TABLE_OFF_ON } }, - { "nav_speed_min", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].gpsProfile.nav_speed_min, .config.minmax = { 10, 2000 } }, - { "nav_speed_max", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].gpsProfile.nav_speed_max, .config.minmax = { 10, 2000 } }, - { "nav_slew_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].gpsProfile.nav_slew_rate, .config.minmax = { 0, 100 } }, + { "gps_wp_radius", VAR_UINT16 | PROFILE_VALUE, &masterConfig.gpsProfile.gps_wp_radius, .config.minmax = { 0, 2000 } }, + { "nav_controls_heading", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.gpsProfile.nav_controls_heading, .config.lookup = { TABLE_OFF_ON } }, + { "nav_speed_min", VAR_UINT16 | PROFILE_VALUE, &masterConfig.gpsProfile.nav_speed_min, .config.minmax = { 10, 2000 } }, + { "nav_speed_max", VAR_UINT16 | PROFILE_VALUE, &masterConfig.gpsProfile.nav_speed_max, .config.minmax = { 10, 2000 } }, + { "nav_slew_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.gpsProfile.nav_slew_rate, .config.minmax = { 0, 100 } }, #endif #ifdef GTUNE { "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], .config.minmax = { 10, 200 } }, @@ -606,13 +606,13 @@ const clivalue_t valueTable[] = { { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } }, - { "alt_hold_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].rcControlsConfig.alt_hold_deadband, .config.minmax = { 1, 250 } }, - { "alt_hold_fast_change", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].rcControlsConfig.alt_hold_fast_change, .config.lookup = { TABLE_OFF_ON } }, - { "deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].rcControlsConfig.deadband, .config.minmax = { 0, 32 } }, - { "yaw_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].rcControlsConfig.yaw_deadband, .config.minmax = { 0, 100 } }, + { "alt_hold_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.rcControlsConfig.alt_hold_deadband, .config.minmax = { 1, 250 } }, + { "alt_hold_fast_change", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.rcControlsConfig.alt_hold_fast_change, .config.lookup = { TABLE_OFF_ON } }, + { "deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.rcControlsConfig.deadband, .config.minmax = { 0, 32 } }, + { "yaw_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.rcControlsConfig.yaw_deadband, .config.minmax = { 0, 100 } }, - { "throttle_correction_value", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].throttle_correction_value, .config.minmax = { 0, 150 } }, - { "throttle_correction_angle", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].throttle_correction_angle, .config.minmax = { 1, 900 } }, + { "throttle_correction_value", VAR_UINT8 | PROFILE_VALUE, &masterConfig.throttle_correction_value, .config.minmax = { 0, 150 } }, + { "throttle_correction_angle", VAR_UINT16 | PROFILE_VALUE, &masterConfig.throttle_correction_angle, .config.minmax = { 1, 900 } }, { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, .config.minmax = { -1, 1 } }, @@ -646,26 +646,26 @@ const clivalue_t valueTable[] = { { "rx_max_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_max_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } }, #ifdef USE_SERVOS - { "gimbal_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].gimbalConfig.mode, .config.lookup = { TABLE_GIMBAL_MODE } }, + { "gimbal_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.gimbalConfig.mode, .config.lookup = { TABLE_GIMBAL_MODE } }, #endif { "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } }, - { "acc_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].acc_lpf_hz, .config.minmax = { 0, 200 } }, - { "accxy_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.xy, .config.minmax = { 0, 100 } }, - { "accz_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.z, .config.minmax = { 0, 100 } }, - { "accz_lpf_cutoff", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].accz_lpf_cutoff, .config.minmax = { 1, 20 } }, - { "acc_unarmedcal", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].acc_unarmedcal, .config.lookup = { TABLE_OFF_ON } }, - { "acc_trim_pitch", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].accelerometerTrims.values.pitch, .config.minmax = { -300, 300 } }, - { "acc_trim_roll", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].accelerometerTrims.values.roll, .config.minmax = { -300, 300 } }, + { "acc_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.acc_lpf_hz, .config.minmax = { 0, 200 } }, + { "accxy_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.accDeadband.xy, .config.minmax = { 0, 100 } }, + { "accz_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.accDeadband.z, .config.minmax = { 0, 100 } }, + { "accz_lpf_cutoff", VAR_FLOAT | PROFILE_VALUE, &masterConfig.accz_lpf_cutoff, .config.minmax = { 1, 20 } }, + { "acc_unarmedcal", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.acc_unarmedcal, .config.lookup = { TABLE_OFF_ON } }, + { "acc_trim_pitch", VAR_INT16 | PROFILE_VALUE, &masterConfig.accelerometerTrims.values.pitch, .config.minmax = { -300, 300 } }, + { "acc_trim_roll", VAR_INT16 | PROFILE_VALUE, &masterConfig.accelerometerTrims.values.roll, .config.minmax = { -300, 300 } }, - { "baro_tab_size", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_sample_count, .config.minmax = { 0, BARO_SAMPLE_COUNT_MAX } }, - { "baro_noise_lpf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_noise_lpf, .config.minmax = { 0 , 1 } }, - { "baro_cf_vel", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_cf_vel, .config.minmax = { 0 , 1 } }, - { "baro_cf_alt", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_cf_alt, .config.minmax = { 0 , 1 } }, + { "baro_tab_size", VAR_UINT8 | PROFILE_VALUE, &masterConfig.barometerConfig.baro_sample_count, .config.minmax = { 0, BARO_SAMPLE_COUNT_MAX } }, + { "baro_noise_lpf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.barometerConfig.baro_noise_lpf, .config.minmax = { 0 , 1 } }, + { "baro_cf_vel", VAR_FLOAT | PROFILE_VALUE, &masterConfig.barometerConfig.baro_cf_vel, .config.minmax = { 0 , 1 } }, + { "baro_cf_alt", VAR_FLOAT | PROFILE_VALUE, &masterConfig.barometerConfig.baro_cf_alt, .config.minmax = { 0 , 1 } }, { "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.baro_hardware, .config.lookup = { TABLE_BARO_HARDWARE } }, { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, - { "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, .config.minmax = { -18000, 18000 } }, + { "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, { "delta_from_gyro", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaFromGyro, .config.lookup = { TABLE_OFF_ON } }, { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } }, @@ -1280,7 +1280,7 @@ static void cliServo(char *cmdline) if (isEmpty(cmdline)) { // print out servo settings for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servo = ¤tProfile->servoConf[i]; + servo = &masterConfig.servoConf[i]; cliPrintf("servo %u %d %d %d %d %d %d %d\r\n", i, @@ -1331,7 +1331,7 @@ static void cliServo(char *cmdline) return; } - servo = ¤tProfile->servoConf[i]; + servo = &masterConfig.servoConf[i]; if ( arguments[MIN] < PWM_PULSE_MIN || arguments[MIN] > PWM_PULSE_MAX || @@ -1392,7 +1392,7 @@ static void cliServoMix(char *cmdline) // erase custom mixer memset(masterConfig.customServoMixer, 0, sizeof(masterConfig.customServoMixer)); for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - currentProfile->servoConf[i].reversedSources = 0; + masterConfig.servoConf[i].reversedSources = 0; } } else if (strncasecmp(cmdline, "load", 4) == 0) { ptr = strchr(cmdline, ' '); @@ -1426,7 +1426,7 @@ static void cliServoMix(char *cmdline) for (servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) { cliPrintf("%d", servoIndex); for (inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++) - cliPrintf("\t%s ", (currentProfile->servoConf[servoIndex].reversedSources & (1 << inputSource)) ? "r" : "n"); + cliPrintf("\t%s ", (masterConfig.servoConf[servoIndex].reversedSources & (1 << inputSource)) ? "r" : "n"); cliPrintf("\r\n"); } return; @@ -1447,9 +1447,9 @@ static void cliServoMix(char *cmdline) && args[INPUT] >= 0 && args[INPUT] < INPUT_SOURCE_COUNT && (*ptr == 'r' || *ptr == 'n')) { if (*ptr == 'r') - currentProfile->servoConf[args[SERVO]].reversedSources |= 1 << args[INPUT]; + masterConfig.servoConf[args[SERVO]].reversedSources |= 1 << args[INPUT]; else - currentProfile->servoConf[args[SERVO]].reversedSources &= ~(1 << args[INPUT]); + masterConfig.servoConf[args[SERVO]].reversedSources &= ~(1 << args[INPUT]); } else cliShowParseError(); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index a113898636..2b3ce178e7 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -713,14 +713,14 @@ static bool processOutCommand(uint8_t cmdMSP) case MSP_SERVO_CONFIGURATIONS: headSerialReply(MAX_SUPPORTED_SERVOS * sizeof(servoParam_t)); for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - serialize16(currentProfile->servoConf[i].min); - serialize16(currentProfile->servoConf[i].max); - serialize16(currentProfile->servoConf[i].middle); - serialize8(currentProfile->servoConf[i].rate); - serialize8(currentProfile->servoConf[i].angleAtMin); - serialize8(currentProfile->servoConf[i].angleAtMax); - serialize8(currentProfile->servoConf[i].forwardFromChannel); - serialize32(currentProfile->servoConf[i].reversedSources); + serialize16(masterConfig.servoConf[i].min); + serialize16(masterConfig.servoConf[i].max); + serialize16(masterConfig.servoConf[i].middle); + serialize8(masterConfig.servoConf[i].rate); + serialize8(masterConfig.servoConf[i].angleAtMin); + serialize8(masterConfig.servoConf[i].angleAtMax); + serialize8(masterConfig.servoConf[i].forwardFromChannel); + serialize32(masterConfig.servoConf[i].reversedSources); } break; case MSP_SERVO_MIX_RULES: @@ -900,7 +900,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(masterConfig.rxConfig.rssi_channel); serialize8(0); - serialize16(currentProfile->mag_declination / 10); + serialize16(masterConfig.mag_declination / 10); serialize8(masterConfig.batteryConfig.vbatscale); serialize8(masterConfig.batteryConfig.vbatmincellvoltage); @@ -973,8 +973,8 @@ static bool processOutCommand(uint8_t cmdMSP) // Additional commands that are not compatible with MultiWii case MSP_ACC_TRIM: headSerialReply(4); - serialize16(currentProfile->accelerometerTrims.values.pitch); - serialize16(currentProfile->accelerometerTrims.values.roll); + serialize16(masterConfig.accelerometerTrims.values.pitch); + serialize16(masterConfig.accelerometerTrims.values.roll); break; case MSP_UID: @@ -1240,8 +1240,8 @@ static bool processInCommand(void) } break; case MSP_SET_ACC_TRIM: - currentProfile->accelerometerTrims.values.pitch = read16(); - currentProfile->accelerometerTrims.values.roll = read16(); + masterConfig.accelerometerTrims.values.pitch = read16(); + masterConfig.accelerometerTrims.values.roll = read16(); break; case MSP_SET_ARMING_CONFIG: masterConfig.auto_disarm_delay = read8(); @@ -1391,7 +1391,7 @@ static bool processInCommand(void) masterConfig.rxConfig.rssi_channel = read8(); read8(); - currentProfile->mag_declination = read16() * 10; + masterConfig.mag_declination = read16() * 10; masterConfig.batteryConfig.vbatscale = read8(); // actual vbatscale as intended masterConfig.batteryConfig.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI @@ -1412,14 +1412,14 @@ static bool processInCommand(void) if (i >= MAX_SUPPORTED_SERVOS) { headSerialError(0); } else { - currentProfile->servoConf[i].min = read16(); - currentProfile->servoConf[i].max = read16(); - currentProfile->servoConf[i].middle = read16(); - currentProfile->servoConf[i].rate = read8(); - currentProfile->servoConf[i].angleAtMin = read8(); - currentProfile->servoConf[i].angleAtMax = read8(); - currentProfile->servoConf[i].forwardFromChannel = read8(); - currentProfile->servoConf[i].reversedSources = read32(); + masterConfig.servoConf[i].min = read16(); + masterConfig.servoConf[i].max = read16(); + masterConfig.servoConf[i].middle = read16(); + masterConfig.servoConf[i].rate = read8(); + masterConfig.servoConf[i].angleAtMin = read8(); + masterConfig.servoConf[i].angleAtMax = read8(); + masterConfig.servoConf[i].forwardFromChannel = read8(); + masterConfig.servoConf[i].reversedSources = read32(); } #endif break; diff --git a/src/main/main.c b/src/main/main.c index b094fb40f8..b9a482e3e1 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -455,7 +455,7 @@ void init(void) } #endif - if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination, masterConfig.gyro_lpf)) { + if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, masterConfig.mag_declination, masterConfig.gyro_lpf)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } @@ -499,7 +499,7 @@ void init(void) &masterConfig.gpsConfig ); navigationInit( - ¤tProfile->gpsProfile, + &masterConfig.gpsProfile, ¤tProfile->pidProfile ); } diff --git a/src/main/mw.c b/src/main/mw.c index 0cfbd0fc66..f8cace7430 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -127,8 +127,8 @@ extern pidControllerFuncPtr pid_controller; void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta) { - currentProfile->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll; - currentProfile->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch; + masterConfig.accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll; + masterConfig.accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch; saveConfigAndNotify(); } @@ -240,9 +240,9 @@ void annexCode(void) for (axis = 0; axis < 3; axis++) { tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500); if (axis == ROLL || axis == PITCH) { - if (currentProfile->rcControlsConfig.deadband) { - if (tmp > currentProfile->rcControlsConfig.deadband) { - tmp -= currentProfile->rcControlsConfig.deadband; + if (masterConfig.rcControlsConfig.deadband) { + if (tmp > masterConfig.rcControlsConfig.deadband) { + tmp -= masterConfig.rcControlsConfig.deadband; } else { tmp = 0; } @@ -253,9 +253,9 @@ void annexCode(void) prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; } else if (axis == YAW) { - if (currentProfile->rcControlsConfig.yaw_deadband) { - if (tmp > currentProfile->rcControlsConfig.yaw_deadband) { - tmp -= currentProfile->rcControlsConfig.yaw_deadband; + if (masterConfig.rcControlsConfig.yaw_deadband) { + if (tmp > masterConfig.rcControlsConfig.yaw_deadband) { + tmp -= masterConfig.rcControlsConfig.yaw_deadband; } else { tmp = 0; } @@ -674,8 +674,8 @@ void taskMainPidLoop(void) rcCommand[YAW] = 0; } - if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { - rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value); + if (masterConfig.throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { + rcCommand[THROTTLE] += calculateThrottleAngleCorrection(masterConfig.throttle_correction_value); } #ifdef GPS @@ -691,7 +691,7 @@ void taskMainPidLoop(void) ¤tProfile->pidProfile, currentControlRateProfile, masterConfig.max_angle_inclination, - ¤tProfile->accelerometerTrims, + &masterConfig.accelerometerTrims, &masterConfig.rxConfig ); @@ -738,7 +738,7 @@ void taskMainPidLoopCheck(void) { void taskUpdateAccelerometer(void) { - imuUpdateAccelerometer(¤tProfile->accelerometerTrims); + imuUpdateAccelerometer(&masterConfig.accelerometerTrims); } void taskHandleSerial(void)