diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index b330730ce7..77ea5c2722 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -35,7 +35,6 @@ #include "common/maths.h" #include "common/utils.h" -#include "config/config_master.h" #include "config/config_profile.h" #include "config/feature.h" #include "config/parameter_group.h" @@ -47,13 +46,18 @@ #include "drivers/pwm_output.h" #include "fc/config.h" +#include "fc/controlrate_profile.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" #include "flight/failsafe.h" +#include "flight/mixer.h" +#include "flight/navigation.h" #include "flight/pid.h" +#include "flight/servos.h" #include "io/beeper.h" +#include "io/gps.h" #include "io/serial.h" #include "rx/rx.h" @@ -371,7 +375,7 @@ static blackboxMainState_t* blackboxHistory[3]; static bool blackboxModeActivationConditionPresent = false; /** - * Return true if it is safe to edit the Blackbox configuration in the emasterConfig. + * Return true if it is safe to edit the Blackbox configuration. */ bool blackboxMayEditConfig() { @@ -1195,7 +1199,6 @@ static bool blackboxWriteSysinfo() return false; } - const profile_t *currentProfile = &masterConfig.profile[systemConfig()->current_profile_index]; const controlRateConfig_t *currentControlRateProfile = controlRateProfiles(systemConfig()->activeRateProfile); switch (xmitState.headerIndex) { BLACKBOX_PRINT_HEADER_LINE("Firmware type:%s", "Cleanflight"); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 49cee1d845..93aa7c57a7 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -91,7 +91,7 @@ #define throttleCorrectionConfig(x) (&masterConfig.throttleCorrectionConfig) #define batteryConfig(x) (&masterConfig.batteryConfig) #define rcControlsConfig(x) (&masterConfig.rcControlsConfig) -#define gpsProfile(x) (&masterConfig.gpsProfile) +#define navigationConfig(x) (&masterConfig.navigationConfig) #define gpsConfig(x) (&masterConfig.gpsConfig) #define rxConfig(x) (&masterConfig.rxConfig) #define armingConfig(x) (&masterConfig.armingConfig) @@ -109,7 +109,7 @@ #define sonarConfig(x) (&masterConfig.sonarConfig) #define ledStripConfig(x) (&masterConfig.ledStripConfig) #define statusLedConfig(x) (&masterConfig.statusLedConfig) -#define osdConfig(x) (&masterConfig.osdProfile) +#define osdConfig(x) (&masterConfig.osdConfig) #define vcdProfile(x) (&masterConfig.vcdProfile) #define sdcardConfig(x) (&masterConfig.sdcardConfig) #define blackboxConfig(x) (&masterConfig.blackboxConfig) @@ -143,7 +143,7 @@ #define throttleCorrectionConfigMutable(x) (&masterConfig.throttleCorrectionConfig) #define batteryConfigMutable(x) (&masterConfig.batteryConfig) #define rcControlsConfigMutable(x) (&masterConfig.rcControlsConfig) -#define gpsProfileMutable(x) (&masterConfig.gpsProfile) +#define navigationConfigMutable(x) (&masterConfig.navigationConfig) #define gpsConfigMutable(x) (&masterConfig.gpsConfig) #define rxConfigMutable(x) (&masterConfig.rxConfig) #define armingConfigMutable(x) (&masterConfig.armingConfig) @@ -160,7 +160,7 @@ #define sonarConfigMutable(x) (&masterConfig.sonarConfig) #define ledStripConfigMutable(x) (&masterConfig.ledStripConfig) #define statusLedConfigMutable(x) (&masterConfig.statusLedConfig) -#define osdProfileMutable(x) (&masterConfig.osdProfile) +#define osdConfigMutable(x) (&masterConfig.osdConfig) #define vcdProfileMutable(x) (&masterConfig.vcdProfile) #define sdcardConfigMutable(x) (&masterConfig.sdcardConfig) #define blackboxConfigMutable(x) (&masterConfig.blackboxConfig) @@ -178,7 +178,6 @@ #define beeperConfigMutable(x) (&masterConfig.beeperConfig) #define transponderConfigMutable(x) (&masterConfig.transponderConfig) -#define osdConfig(x) (&masterConfig.osdProfile) #define servoParams(i) (&servoProfile()->servoConf[i]) #define adjustmentRanges(i) (&adjustmentProfile()->adjustmentRanges[i]) #define rxFailsafeChannelConfigs(i) (&masterConfig.rxConfig.failsafe_channel_configurations[i]) @@ -186,7 +185,6 @@ #define controlRateProfiles(i) (&masterConfig.controlRateProfile[i]) #define pidProfiles(i) (&masterConfig.profile[i].pidProfile) -#define osdConfigMutable(x) (&masterConfig.osdProfile) #define servoParamsMutable(i) (&servoProfile()->servoConf[i]) #define adjustmentRangesMutable(i) (&masterConfig.adjustmentProfile.adjustmentRanges[i]) #define rxFailsafeChannelConfigsMutable(i) (&masterConfig.rxConfig.>failsafe_channel_configurations[i]) @@ -242,7 +240,7 @@ typedef struct master_s { rcControlsConfig_t rcControlsConfig; #ifdef GPS - gpsProfile_t gpsProfile; + navigationConfig_t navigationConfig; gpsConfig_t gpsConfig; #endif @@ -290,7 +288,7 @@ typedef struct master_s { #endif #ifdef OSD - osd_profile_t osdProfile; + osdConfig_t osdConfig; #endif #ifdef USE_MAX7456 diff --git a/src/main/config/feature.c b/src/main/config/feature.c index 09410192e7..bb5e5b9a75 100644 --- a/src/main/config/feature.c +++ b/src/main/config/feature.c @@ -21,7 +21,6 @@ #include "platform.h" -#include "config/config_master.h" #include "config/feature.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" @@ -78,5 +77,3 @@ uint32_t featureMask(void) { return featureConfig()->enabledFeatures; } - - diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index d1f45b9c96..ea2e99484b 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -94,7 +94,9 @@ #define PG_ADC_CONFIG 510 #define PG_SDCARD_CONFIG 511 #define PG_DISPLAY_PORT_MSP_CONFIG 512 -#define PG_BETAFLIGHT_END 512 +#define PG_DISPLAY_PORT_MAX7456_CONFIG 513 +#define PG_VCD_CONFIG 514 +#define PG_BETAFLIGHT_END 514 // OSD configuration (subject to change) diff --git a/src/main/drivers/accgyro_spi_bmi160.c b/src/main/drivers/accgyro_spi_bmi160.c index e18e92ef76..40738e4b76 100644 --- a/src/main/drivers/accgyro_spi_bmi160.c +++ b/src/main/drivers/accgyro_spi_bmi160.c @@ -54,7 +54,6 @@ #include "config/config_eeprom.h" #include "config/config_profile.h" -#include "config/config_master.h" #include "fc/runtime_config.h" #ifdef USE_ACCGYRO_BMI160 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 197c646cc1..48c4e0fe53 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -73,6 +73,7 @@ uint8_t cliMode = 0; #include "fc/cli.h" #include "fc/config.h" +#include "fc/controlrate_profile.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -621,11 +622,11 @@ static const clivalue_t valueTable[] = { { "gps_nav_p", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->P8[PIDNAVR], .config.minmax = { 0, 200 } }, { "gps_nav_i", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->I8[PIDNAVR], .config.minmax = { 0, 200 } }, { "gps_nav_d", VAR_UINT8 | PROFILE_VALUE, &pidProfiles(0)->D8[PIDNAVR], .config.minmax = { 0, 200 } }, - { "gps_wp_radius", VAR_UINT16 | MASTER_VALUE, &gpsProfile()->gps_wp_radius, .config.minmax = { 0, 2000 } }, - { "nav_controls_heading", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gpsProfile()->nav_controls_heading, .config.lookup = { TABLE_OFF_ON } }, - { "nav_speed_min", VAR_UINT16 | MASTER_VALUE, &gpsProfile()->nav_speed_min, .config.minmax = { 10, 2000 } }, - { "nav_speed_max", VAR_UINT16 | MASTER_VALUE, &gpsProfile()->nav_speed_max, .config.minmax = { 10, 2000 } }, - { "nav_slew_rate", VAR_UINT8 | MASTER_VALUE, &gpsProfile()->nav_slew_rate, .config.minmax = { 0, 100 } }, + { "gps_wp_radius", VAR_UINT16 | MASTER_VALUE, &navigationConfig()->gps_wp_radius, .config.minmax = { 0, 2000 } }, + { "nav_controls_heading", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &navigationConfig()->nav_controls_heading, .config.lookup = { TABLE_OFF_ON } }, + { "nav_speed_min", VAR_UINT16 | MASTER_VALUE, &navigationConfig()->nav_speed_min, .config.minmax = { 10, 2000 } }, + { "nav_speed_max", VAR_UINT16 | MASTER_VALUE, &navigationConfig()->nav_speed_max, .config.minmax = { 10, 2000 } }, + { "nav_slew_rate", VAR_UINT8 | MASTER_VALUE, &navigationConfig()->nav_slew_rate, .config.minmax = { 0, 100 } }, #endif #ifdef BEEPER @@ -927,7 +928,7 @@ static gpsConfig_t gpsConfigCopy; #endif #ifdef NAV static positionEstimationConfig_t positionEstimationConfigCopy; -static navConfig_t navConfigCopy; +static navigationConfig_t navigationConfigCopy; #endif #ifdef TELEMETRY static telemetryConfig_t telemetryConfigCopy; @@ -938,7 +939,7 @@ static adjustmentRange_t adjustmentRangesCopy[MAX_ADJUSTMENT_RANGE_COUNT]; static ledStripConfig_t ledStripConfigCopy; #endif #ifdef OSD -static osd_profile_t osdConfigCopy; +static osdConfig_t osdConfigCopy; #endif static systemConfig_t systemConfigCopy; #ifdef BEEPER @@ -1221,8 +1222,8 @@ static const cliCurrentAndDefaultConfig_t *getCurrentAndDefaultConfigs(pgn_t pgn ret.defaultConfig = positionEstimationConfig(); break; case PG_NAV_CONFIG: - ret.currentConfig = &navConfigCopy; - ret.defaultConfig = navConfig(); + ret.currentConfig = &navigationConfigCopy; + ret.defaultConfig = navigationConfig(); break; #endif #ifdef TELEMETRY @@ -1375,7 +1376,7 @@ void *getValuePointer(const clivalue_t *value) return ptr; } -#endif +#endif // USE_PARAMETER_GROUPS static void *getDefaultPointer(void *valuePointer, const master_t *defaultConfig) { @@ -3148,7 +3149,7 @@ static void cliMap(char *cmdline) cliShowParseError(); return; } - parseRcChannels(cmdline, &masterConfig.rxConfig); + parseRcChannels(cmdline, rxConfigMutable()); } cliPrint("Map: "); uint32_t i; diff --git a/src/main/fc/config.c b/src/main/fc/config.c index e01e315fdc..147163a11b 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -156,6 +156,9 @@ PG_RESET_TEMPLATE(sdcardConfig_t, sdcardConfig, ); #endif +// no template required since defaults are zero +PG_REGISTER(vcdProfile_t, vcdProfile, PG_VCD_CONFIG, 0); + #ifndef USE_PARAMETER_GROUPS static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -253,15 +256,15 @@ void resetProfile(profile_t *profile) } #ifdef GPS -void resetGpsProfile(gpsProfile_t *gpsProfile) +void resetNavigationConfig(navigationConfig_t *navigationConfig) { - gpsProfile->gps_wp_radius = 200; - gpsProfile->gps_lpf = 20; - gpsProfile->nav_slew_rate = 30; - gpsProfile->nav_controls_heading = 1; - gpsProfile->nav_speed_min = 100; - gpsProfile->nav_speed_max = 300; - gpsProfile->ap_mode = 40; + navigationConfig->gps_wp_radius = 200; + navigationConfig->gps_lpf = 20; + navigationConfig->nav_slew_rate = 30; + navigationConfig->nav_controls_heading = 1; + navigationConfig->nav_speed_min = 100; + navigationConfig->nav_speed_max = 300; + navigationConfig->ap_mode = 40; } #endif @@ -741,6 +744,7 @@ void resetMixerConfig(mixerConfig_t *mixerConfig) } #endif +#ifndef USE_PARAMETER_GROUPS #ifdef USE_MAX7456 void resetMax7456Config(vcdProfile_t *pVcdProfile) { @@ -750,7 +754,6 @@ void resetMax7456Config(vcdProfile_t *pVcdProfile) } #endif -#ifndef USE_PARAMETER_GROUPS void resetDisplayPortProfile(displayPortProfile_t *pDisplayPortProfile) { pDisplayPortProfile->colAdjust = 0; @@ -842,6 +845,7 @@ void createDefaultConfig(master_t *config) intFeatureSet(DEFAULT_FEATURES, featuresPtr); #endif +#ifndef USE_PARAMETER_GROUPS #ifdef USE_MSP_DISPLAYPORT resetDisplayPortProfile(&config->displayPortProfileMsp); #endif @@ -854,8 +858,9 @@ void createDefaultConfig(master_t *config) #endif #ifdef OSD - osdResetConfig(&config->osdProfile); + osdResetConfig(&config->osdConfig); #endif +#endif // USE_PARAMETER_GROUPS #ifdef BOARD_HAS_VOLTAGE_DIVIDER // only enable the VBAT feature by default if the board has a voltage divider otherwise @@ -1089,7 +1094,7 @@ void createDefaultConfig(master_t *config) #ifndef USE_PARAMETER_GROUPS #ifdef GPS - resetGpsProfile(&config->gpsProfile); + resetNavigationConfig(&config->navigationConfig); #endif #endif diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index a05ee1843f..6748d79c7a 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -464,7 +464,7 @@ void init(void) #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit(); - navigationInit(¤tProfile->pidProfile); + navigationInit(); } #endif diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index d7e50f6276..eda9e16ffa 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -28,22 +28,26 @@ #include "common/utils.h" #include "common/filter.h" +#include "config/config_profile.h" +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "fc/config.h" +#include "fc/controlrate_profile.h" +#include "fc/fc_core.h" +#include "fc/fc_rc.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "fc/fc_rc.h" -#include "fc/fc_core.h" #include "rx/rx.h" #include "scheduler/scheduler.h" +#include "flight/failsafe.h" +#include "flight/imu.h" #include "flight/pid.h" -#include "config/config_profile.h" -#include "config/config_master.h" -#include "config/feature.h" - static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; static float throttlePIDAttenuation; diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 633e1163a8..f553c11a80 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -36,17 +36,17 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" #include "config/feature.h" -#include "config/config_master.h" #include "flight/pid.h" #include "io/beeper.h" #include "io/motors.h" +#include "fc/config.h" +#include "fc/controlrate_profile.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "fc/fc_rc.h" -#include "fc/config.h" #include "rx/rx.h" diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index de7eeb990f..03d42395d6 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -31,6 +31,7 @@ #include "common/maths.h" #include "common/time.h" +#include "config/config_profile.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" @@ -56,9 +57,9 @@ #include "sensors/sensors.h" -PG_REGISTER_WITH_RESET_TEMPLATE(gpsProfile_t, gpsProfile, PG_NAVIGATION_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(navigationConfig_t, navigationConfig, PG_NAVIGATION_CONFIG, 0); -PG_RESET_TEMPLATE(gpsProfile_t, gpsProfile, +PG_RESET_TEMPLATE(navigationConfig_t, navigationConfig, .gps_wp_radius = 200, .gps_lpf = 20, .nav_slew_rate = 30, @@ -85,9 +86,9 @@ static int16_t nav_rated[2]; // Adding a rate controller to the na navigationMode_e nav_mode = NAV_MODE_NONE; // Navigation mode // When using PWM input GPS usage reduces number of available channels by 2 - see pwm_common.c/pwmInit() -void navigationInit(pidProfile_t *pidProfile) +void navigationInit(void) { - gpsUsePIDs(pidProfile); + gpsUsePIDs(¤tProfile->pidProfile); } @@ -167,7 +168,7 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param) // Low pass filter cut frequency for derivative calculation // Set to "1 / ( 2 * PI * gps_lpf ) - float pidFilter = (1.0f / (2.0f * M_PIf * (float)gpsProfile()->gps_lpf)); + float pidFilter = (1.0f / (2.0f * M_PIf * (float)navigationConfig()->gps_lpf)); // discrete low pass filter, cuts out the // high frequency noise that can drive the controller crazy pid->derivative = pid->last_derivative + (*dt / (pidFilter + *dt)) * (pid->derivative - pid->last_derivative); @@ -331,13 +332,13 @@ void onGpsNewData(void) break; case NAV_MODE_WP: - speed = GPS_calc_desired_speed(gpsProfile()->nav_speed_max, NAV_SLOW_NAV); // slow navigation + speed = GPS_calc_desired_speed(navigationConfig()->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 (gpsProfile()->nav_controls_heading) { + if (navigationConfig()->nav_controls_heading) { if (NAV_TAIL_FIRST) { magHold = wrap_18000(nav_bearing - 18000) / 100; } else { @@ -345,7 +346,7 @@ void onGpsNewData(void) } } // Are we there yet ?(within x meters of the destination) - if ((wp_distance <= gpsProfile()->gps_wp_radius) || check_missed_wp()) { // if yes switch to poshold mode + if ((wp_distance <= navigationConfig()->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; @@ -435,7 +436,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 = gpsProfile()->nav_speed_min; + waypoint_speed_gov = navigationConfig()->nav_speed_min; } //////////////////////////////////////////////////////////////////////////////////// @@ -614,7 +615,7 @@ static uint16_t GPS_calc_desired_speed(uint16_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, gpsProfile()->nav_speed_min); // go at least 100cm/s + max_speed = MAX(max_speed, navigationConfig()->nav_speed_min); // go at least 100cm/s } // limit the ramp up of the speed @@ -651,9 +652,9 @@ void updateGpsStateForHomeAndHoldMode(void) { float sin_yaw_y = sin_approx(DECIDEGREES_TO_DEGREES(attitude.values.yaw) * 0.0174532925f); float cos_yaw_x = cos_approx(DECIDEGREES_TO_DEGREES(attitude.values.yaw) * 0.0174532925f); - if (gpsProfile()->nav_slew_rate) { - nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -gpsProfile()->nav_slew_rate, gpsProfile()->nav_slew_rate); // TODO check this on uint8 - nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -gpsProfile()->nav_slew_rate, gpsProfile()->nav_slew_rate); + if (navigationConfig()->nav_slew_rate) { + nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -navigationConfig()->nav_slew_rate, navigationConfig()->nav_slew_rate); // TODO check this on uint8 + nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -navigationConfig()->nav_slew_rate, navigationConfig()->nav_slew_rate); GPS_angle[AI_ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10; GPS_angle[AI_PITCH] = (nav_rated[LON] * sin_yaw_y + nav_rated[LAT] * cos_yaw_x) / 10; } else { @@ -697,7 +698,7 @@ void updateGpsWaypointsAndMode(void) // process HOLD mode // - if (IS_RC_MODE_ACTIVE(BOXGPSHOLD) && areSticksInApModePosition(gpsProfile()->ap_mode)) { + if (IS_RC_MODE_ACTIVE(BOXGPSHOLD) && areSticksInApModePosition(navigationConfig()->ap_mode)) { if (!FLIGHT_MODE(GPS_HOLD_MODE)) { // Transition to HOLD mode diff --git a/src/main/flight/navigation.h b/src/main/flight/navigation.h index af49cfd263..c7375dd82d 100644 --- a/src/main/flight/navigation.h +++ b/src/main/flight/navigation.h @@ -26,7 +26,7 @@ typedef enum { // FIXME ap_mode is badly named, it's a value that is compared to rcCommand, not a flag at it's name implies. -typedef struct gpsProfile_s { +typedef struct navigationConfig_s { 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 @@ -34,9 +34,9 @@ typedef struct gpsProfile_s { 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 -} gpsProfile_t; +} navigationConfig_t; -PG_DECLARE(gpsProfile_t, gpsProfile); +PG_DECLARE(navigationConfig_t, navigationConfig); extern int16_t GPS_angle[ANGLE_INDEX_COUNT]; // it's the angles that must be applied for GPS correction @@ -48,8 +48,7 @@ extern int16_t GPS_directionToHome; // direction to home or hol point in extern navigationMode_e nav_mode; // Navigation mode -struct pidProfile_s; -void navigationInit(struct pidProfile_s *pidProfile); +void navigationInit(void); void GPS_reset_home_position(void); void GPS_reset_nav(void); void GPS_set_next_wp(int32_t* lat, int32_t* lon); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 6845c1fcd3..c656f6fb2d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -29,9 +29,10 @@ #include "common/maths.h" #include "common/filter.h" +#include "config/config_reset.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" -#include "config/config_reset.h" +#include "config/config_profile.h" #include "fc/fc_core.h" #include "fc/fc_rc.h" @@ -75,61 +76,72 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 0); +void resetPidProfile(pidProfile_t *pidProfile) +{ + RESET_CONFIG(const pidProfile_t, pidProfile, + .P8[ROLL] = 44, + .I8[ROLL] = 40, + .D8[ROLL] = 20, + .P8[PITCH] = 58, + .I8[PITCH] = 50, + .D8[PITCH] = 22, + .P8[YAW] = 70, + .I8[YAW] = 45, + .D8[YAW] = 20, + .P8[PIDALT] = 50, + .I8[PIDALT] = 0, + .D8[PIDALT] = 0, + .P8[PIDPOS] = 15, // POSHOLD_P * 100, + .I8[PIDPOS] = 0, // POSHOLD_I * 100, + .D8[PIDPOS] = 0, + .P8[PIDPOSR] = 34, // POSHOLD_RATE_P * 10, + .I8[PIDPOSR] = 14, // POSHOLD_RATE_I * 100, + .D8[PIDPOSR] = 53, // POSHOLD_RATE_D * 1000, + .P8[PIDNAVR] = 25, // NAV_P * 10, + .I8[PIDNAVR] = 33, // NAV_I * 100, + .D8[PIDNAVR] = 83, // NAV_D * 1000, + .P8[PIDLEVEL] = 50, + .I8[PIDLEVEL] = 50, + .D8[PIDLEVEL] = 100, + .P8[PIDMAG] = 40, + .P8[PIDVEL] = 55, + .I8[PIDVEL] = 55, + .D8[PIDVEL] = 75, + + .yaw_p_limit = YAW_P_LIMIT_MAX, + .pidSumLimit = PIDSUM_LIMIT, + .yaw_lpf_hz = 0, + .itermWindupPointPercent = 50, + .dterm_filter_type = FILTER_BIQUAD, + .dterm_lpf_hz = 100, // filtering ON by default + .dterm_notch_hz = 260, + .dterm_notch_cutoff = 160, + .vbatPidCompensation = 0, + .pidAtMinThrottle = PID_STABILISATION_ON, + .levelAngleLimit = 55, + .levelSensitivity = 55, + .setpointRelaxRatio = 20, + .dtermSetpointWeight = 100, + .yawRateAccelLimit = 10.0f, + .rateAccelLimit = 0.0f, + .itermThrottleThreshold = 350, + .itermAcceleratorGain = 1.0f + ); +} void pgResetFn_pidProfiles(pidProfile_t *pidProfiles) { for (int i = 0; i < MAX_PROFILE_COUNT; i++) { - RESET_CONFIG(const pidProfile_t, &pidProfiles[i], - .P8[ROLL] = 44, - .I8[ROLL] = 40, - .D8[ROLL] = 20, - .P8[PITCH] = 58, - .I8[PITCH] = 50, - .D8[PITCH] = 22, - .P8[YAW] = 70, - .I8[YAW] = 45, - .D8[YAW] = 20, - .P8[PIDALT] = 50, - .I8[PIDALT] = 0, - .D8[PIDALT] = 0, - .P8[PIDPOS] = 15, // POSHOLD_P * 100, - .I8[PIDPOS] = 0, // POSHOLD_I * 100, - .D8[PIDPOS] = 0, - .P8[PIDPOSR] = 34, // POSHOLD_RATE_P * 10, - .I8[PIDPOSR] = 14, // POSHOLD_RATE_I * 100, - .D8[PIDPOSR] = 53, // POSHOLD_RATE_D * 1000, - .P8[PIDNAVR] = 25, // NAV_P * 10, - .I8[PIDNAVR] = 33, // NAV_I * 100, - .D8[PIDNAVR] = 83, // NAV_D * 1000, - .P8[PIDLEVEL] = 50, - .I8[PIDLEVEL] = 50, - .D8[PIDLEVEL] = 100, - .P8[PIDMAG] = 40, - .P8[PIDVEL] = 55, - .I8[PIDVEL] = 55, - .D8[PIDVEL] = 75, - - .yaw_p_limit = YAW_P_LIMIT_MAX, - .pidSumLimit = PIDSUM_LIMIT, - .yaw_lpf_hz = 0, - .itermWindupPointPercent = 50, - .dterm_filter_type = FILTER_BIQUAD, - .dterm_lpf_hz = 100, // filtering ON by default - .dterm_notch_hz = 260, - .dterm_notch_cutoff = 160, - .vbatPidCompensation = 0, - .pidAtMinThrottle = PID_STABILISATION_ON, - .levelAngleLimit = 55, - .levelSensitivity = 55, - .setpointRelaxRatio = 20, - .dtermSetpointWeight = 100, - .yawRateAccelLimit = 10.0f, - .rateAccelLimit = 0.0f, - .itermThrottleThreshold = 350, - .itermAcceleratorGain = 1.0f - ); + resetPidProfile(&pidProfiles[i]); } } +#ifdef USE_PARAMETER_GROUPS +void resetProfile(profile_t *profile) +{ + resetPidProfile(&profile->pidProfile); +} +#endif + void pidSetTargetLooptime(uint32_t pidLooptime) { targetPidLooptime = pidLooptime; diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c index 23504a5929..e1a83fd67e 100644 --- a/src/main/io/displayport_max7456.c +++ b/src/main/io/displayport_max7456.c @@ -34,9 +34,10 @@ #include "io/displayport_max7456.h" #include "io/osd.h" -displayPort_t max7456DisplayPort; // Referenced from osd.c +displayPort_t max7456DisplayPort; -extern uint16_t refreshTimeout; +// no template required since defaults are zero +PG_REGISTER(displayPortProfile_t, displayPortProfileMax7456, PG_DISPLAY_PORT_MAX7456_CONFIG, 0); static int grab(displayPort_t *displayPort) { diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 033ede387b..38db0ba257 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -44,7 +44,6 @@ #include "common/typeconversion.h" #include "common/utils.h" -#include "config/config_master.h" #include "config/config_profile.h" #include "config/feature.h" #include "config/parameter_group.h" @@ -135,6 +134,8 @@ static displayPort_t *osdDisplayPort; #define AH_SIDEBAR_WIDTH_POS 7 #define AH_SIDEBAR_HEIGHT_POS 3 +PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 0); + /** * Gets the correct altitude symbol for the current unit system */ @@ -471,7 +472,11 @@ void osdDrawElements(void) #endif // GPS } -void osdResetConfig(osd_profile_t *osdProfile) +#ifdef USE_PARAMETER_GROUPS +void pgResetFn_osdConfig(osdConfig_t *osdProfile) +#else +void osdResetConfig(osdConfig_t *osdProfile) +#endif { osdProfile->item_pos[OSD_RSSI_VALUE] = OSD_POS(22, 0); osdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | VISIBLE_FLAG; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 00b6abc163..7381a97f34 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -56,7 +56,7 @@ typedef enum { OSD_UNIT_METRIC } osd_unit_e; -typedef struct osd_profile_s { +typedef struct osdConfig_s { uint16_t item_pos[OSD_ITEM_COUNT]; // Alarms @@ -66,13 +66,14 @@ typedef struct osd_profile_s { uint16_t alt_alarm; osd_unit_e units; -} osd_profile_t; +} osdConfig_t; -// !!TODO change to osdConfig_t -PG_DECLARE(osd_profile_t, osdConfig); +extern uint16_t refreshTimeout; + +PG_DECLARE(osdConfig_t, osdConfig); struct displayPort_s; void osdInit(struct displayPort_s *osdDisplayPort); -void osdResetConfig(osd_profile_t *osdProfile); +void osdResetConfig(osdConfig_t *osdProfile); void osdResetAlarms(void); void osdUpdate(timeUs_t currentTimeUs);