mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-25 09:16:01 +03:00
Merge pull request #1131 from martinbudden/inav_pg_completion
Completion of conversion to parameter groups
This commit is contained in:
commit
ba3fb787a4
19 changed files with 137 additions and 182 deletions
|
@ -76,7 +76,6 @@
|
|||
#include "flight/navigation_rewrite.h"
|
||||
|
||||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
#include "config/feature.h"
|
||||
|
||||
#include "blackbox.h"
|
||||
|
@ -487,7 +486,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
|
|||
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
|
||||
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
|
||||
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2:
|
||||
return currentProfile->pidProfile.D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0;
|
||||
return pidProfile()->D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0;
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_MAG:
|
||||
#ifdef MAG
|
||||
|
@ -1376,42 +1375,42 @@ static bool blackboxWriteSysinfo()
|
|||
BLACKBOX_PRINT_HEADER_LINE("rates:%d,%d,%d", currentControlRateProfile->rates[ROLL],
|
||||
currentControlRateProfile->rates[PITCH],
|
||||
currentControlRateProfile->rates[YAW]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rollPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDROLL],
|
||||
currentProfile->pidProfile.I8[PIDROLL],
|
||||
currentProfile->pidProfile.D8[PIDROLL]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("pitchPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDPITCH],
|
||||
currentProfile->pidProfile.I8[PIDPITCH],
|
||||
currentProfile->pidProfile.D8[PIDPITCH]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("yawPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDYAW],
|
||||
currentProfile->pidProfile.I8[PIDYAW],
|
||||
currentProfile->pidProfile.D8[PIDYAW]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("altPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDALT],
|
||||
currentProfile->pidProfile.I8[PIDALT],
|
||||
currentProfile->pidProfile.D8[PIDALT]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("posPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDPOS],
|
||||
currentProfile->pidProfile.I8[PIDPOS],
|
||||
currentProfile->pidProfile.D8[PIDPOS]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("posrPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDPOSR],
|
||||
currentProfile->pidProfile.I8[PIDPOSR],
|
||||
currentProfile->pidProfile.D8[PIDPOSR]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("navrPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDNAVR],
|
||||
currentProfile->pidProfile.I8[PIDNAVR],
|
||||
currentProfile->pidProfile.D8[PIDNAVR]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("levelPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDLEVEL],
|
||||
currentProfile->pidProfile.I8[PIDLEVEL],
|
||||
currentProfile->pidProfile.D8[PIDLEVEL]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("magPID:%d", currentProfile->pidProfile.P8[PIDMAG]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("velPID:%d,%d,%d", currentProfile->pidProfile.P8[PIDVEL],
|
||||
currentProfile->pidProfile.I8[PIDVEL],
|
||||
currentProfile->pidProfile.D8[PIDVEL]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", currentProfile->pidProfile.yaw_p_limit);
|
||||
BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", (int)(currentProfile->pidProfile.yaw_lpf_hz * 100.0f));
|
||||
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", (int)(currentProfile->pidProfile.dterm_lpf_hz * 100.0f));
|
||||
BLACKBOX_PRINT_HEADER_LINE("rollPID:%d,%d,%d", pidProfile()->P8[PIDROLL],
|
||||
pidProfile()->I8[PIDROLL],
|
||||
pidProfile()->D8[PIDROLL]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("pitchPID:%d,%d,%d", pidProfile()->P8[PIDPITCH],
|
||||
pidProfile()->I8[PIDPITCH],
|
||||
pidProfile()->D8[PIDPITCH]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("yawPID:%d,%d,%d", pidProfile()->P8[PIDYAW],
|
||||
pidProfile()->I8[PIDYAW],
|
||||
pidProfile()->D8[PIDYAW]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("altPID:%d,%d,%d", pidProfile()->P8[PIDALT],
|
||||
pidProfile()->I8[PIDALT],
|
||||
pidProfile()->D8[PIDALT]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("posPID:%d,%d,%d", pidProfile()->P8[PIDPOS],
|
||||
pidProfile()->I8[PIDPOS],
|
||||
pidProfile()->D8[PIDPOS]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("posrPID:%d,%d,%d", pidProfile()->P8[PIDPOSR],
|
||||
pidProfile()->I8[PIDPOSR],
|
||||
pidProfile()->D8[PIDPOSR]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("navrPID:%d,%d,%d", pidProfile()->P8[PIDNAVR],
|
||||
pidProfile()->I8[PIDNAVR],
|
||||
pidProfile()->D8[PIDNAVR]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("levelPID:%d,%d,%d", pidProfile()->P8[PIDLEVEL],
|
||||
pidProfile()->I8[PIDLEVEL],
|
||||
pidProfile()->D8[PIDLEVEL]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("magPID:%d", pidProfile()->P8[PIDMAG]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("velPID:%d,%d,%d", pidProfile()->P8[PIDVEL],
|
||||
pidProfile()->I8[PIDVEL],
|
||||
pidProfile()->D8[PIDVEL]);
|
||||
BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", pidProfile()->yaw_p_limit);
|
||||
BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", (int)(pidProfile()->yaw_lpf_hz * 100.0f));
|
||||
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", (int)(pidProfile()->dterm_lpf_hz * 100.0f));
|
||||
BLACKBOX_PRINT_HEADER_LINE("deadband:%d", rcControlsConfig()->deadband);
|
||||
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", rcControlsConfig()->yaw_deadband);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", gyroConfig()->gyro_lpf);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", gyroConfig()->gyro_soft_lpf_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(currentProfile->pidProfile.acc_soft_lpf_hz * 100.0f));
|
||||
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(pidProfile()->acc_soft_lpf_hz * 100.0f));
|
||||
BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", accelerometerConfig()->acc_hardware);
|
||||
BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", barometerConfig()->baro_hardware);
|
||||
BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", compassConfig()->mag_hardware);
|
||||
|
|
|
@ -16,6 +16,7 @@
|
|||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
#include <ctype.h>
|
||||
|
||||
|
@ -37,7 +38,6 @@
|
|||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "config/config_master.h"
|
||||
#include "config/config_profile.h"
|
||||
#include "config/feature.h"
|
||||
|
||||
|
|
|
@ -28,7 +28,6 @@
|
|||
|
||||
#include "config/config_eeprom.h"
|
||||
#include "config/config_streamer.h"
|
||||
#include "config/config_master.h"
|
||||
#include "config/parameter_group.h"
|
||||
|
||||
#include "drivers/system.h"
|
||||
|
@ -113,9 +112,6 @@ static bool scanEEPROM(void)
|
|||
}
|
||||
chk = updateChecksum(chk, header, sizeof(*header));
|
||||
p += sizeof(*header);
|
||||
// include the transitional masterConfig record
|
||||
chk = updateChecksum(chk, p, sizeof(masterConfig));
|
||||
p += sizeof(masterConfig);
|
||||
|
||||
for (;;) {
|
||||
const configRecord_t *record = (const configRecord_t *)p;
|
||||
|
@ -149,7 +145,6 @@ static const configRecord_t *findEEPROM(const pgRegistry_t *reg, configRecordFla
|
|||
{
|
||||
const uint8_t *p = &__config_start;
|
||||
p += sizeof(configHeader_t); // skip header
|
||||
p += sizeof(master_t); // skip the transitional master_t record
|
||||
while (true) {
|
||||
const configRecord_t *record = (const configRecord_t *)p;
|
||||
if (record->size == 0
|
||||
|
@ -170,11 +165,6 @@ static const configRecord_t *findEEPROM(const pgRegistry_t *reg, configRecordFla
|
|||
// but each PG is loaded/initialized exactly once and in defined order.
|
||||
bool loadEEPROM(void)
|
||||
{
|
||||
// read in the transitional masterConfig record
|
||||
const uint8_t *p = &__config_start;
|
||||
p += sizeof(configHeader_t); // skip header
|
||||
masterConfig = *(master_t*)p;
|
||||
|
||||
PG_FOREACH(reg) {
|
||||
configRecordFlags_e cls_start, cls_end;
|
||||
if (pgIsSystem(reg)) {
|
||||
|
@ -217,9 +207,6 @@ static bool writeSettingsToEEPROM(void)
|
|||
|
||||
config_streamer_write(&streamer, (uint8_t *)&header, sizeof(header));
|
||||
chk = updateChecksum(chk, (uint8_t *)&header, sizeof(header));
|
||||
// write the transitional masterConfig record
|
||||
config_streamer_write(&streamer, (uint8_t *)&masterConfig, sizeof(masterConfig));
|
||||
chk = updateChecksum(chk, (uint8_t *)&masterConfig, sizeof(masterConfig));
|
||||
PG_FOREACH(reg) {
|
||||
const uint16_t regSize = pgSize(reg);
|
||||
configRecord_t record = {
|
||||
|
|
|
@ -38,9 +38,6 @@
|
|||
#include "io/osd.h"
|
||||
#include "io/ledstrip.h"
|
||||
|
||||
#define pwmRxConfig(x) (&masterConfig.pwmRxConfig)
|
||||
#define pwmRxConfigMutable(x) (&masterConfig.pwmRxConfig)
|
||||
|
||||
|
||||
// System-wide
|
||||
typedef struct master_s {
|
||||
|
@ -48,10 +45,6 @@ typedef struct master_s {
|
|||
uint16_t size;
|
||||
uint8_t magic_be; // magic number, should be 0xBE
|
||||
|
||||
pwmRxConfig_t pwmRxConfig;
|
||||
|
||||
profile_t profile[MAX_PROFILE_COUNT];
|
||||
|
||||
uint8_t magic_ef; // magic number, should be 0xEF
|
||||
uint8_t chk; // XOR checksum
|
||||
/*
|
||||
|
@ -60,7 +53,5 @@ typedef struct master_s {
|
|||
*/
|
||||
} master_t;
|
||||
|
||||
extern master_t masterConfig;
|
||||
extern const profile_t *currentProfile;
|
||||
|
||||
void createDefaultConfig(master_t *config);
|
||||
//extern master_t masterConfig;
|
||||
//extern const profile_t *currentProfile;
|
||||
|
|
|
@ -127,9 +127,9 @@ void resetPPMDataReceivedState(void)
|
|||
|
||||
#define MIN_CHANNELS_BEFORE_PPM_FRAME_CONSIDERED_VALID 4
|
||||
|
||||
void pwmRxInit(const pwmRxConfig_t *pwmRxConfig)
|
||||
void pwmRxInit(inputFilteringMode_e inputFilteringModeToUse)
|
||||
{
|
||||
inputFilteringMode = pwmRxConfig->inputFilteringMode;
|
||||
inputFilteringMode = inputFilteringModeToUse;
|
||||
}
|
||||
|
||||
#ifdef DEBUG_PPM_ISR
|
||||
|
|
|
@ -35,10 +35,6 @@ uint16_t ppmRead(uint8_t channel);
|
|||
bool isPPMDataBeingReceived(void);
|
||||
void resetPPMDataReceivedState(void);
|
||||
|
||||
typedef struct pwmRxConfig_s {
|
||||
inputFilteringMode_e inputFilteringMode;
|
||||
} pwmRxConfig_t;
|
||||
|
||||
void pwmRxInit(const pwmRxConfig_t *pwmRxConfig);
|
||||
void pwmRxInit(inputFilteringMode_e inputFilteringMode);
|
||||
|
||||
bool isPWMDataBeingReceived(void);
|
||||
|
|
|
@ -32,7 +32,6 @@
|
|||
|
||||
#include "config/config_eeprom.h"
|
||||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
#include "config/feature.h"
|
||||
#include "config/parameter_group.h"
|
||||
#include "config/parameter_group_ids.h"
|
||||
|
@ -84,9 +83,6 @@
|
|||
#define BRUSHED_MOTORS_PWM_RATE 16000
|
||||
#define BRUSHLESS_MOTORS_PWM_RATE 400
|
||||
|
||||
master_t masterConfig; // master config struct with data independent from profiles
|
||||
const profile_t *currentProfile;
|
||||
|
||||
PG_REGISTER(profileSelection_t, profileSelection, PG_PROFILE_SELECTION, 0);
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
|
||||
|
@ -97,7 +93,8 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
|
|||
.accTaskFrequency = ACC_TASK_FREQUENCY_DEFAULT,
|
||||
.attitudeTaskFrequency = ATTITUDE_TASK_FREQUENCY_DEFAULT,
|
||||
.asyncMode = ASYNC_MODE_NONE,
|
||||
.throttle_tilt_compensation_strength = 0 // 0-100, 0 - disabled
|
||||
.throttle_tilt_compensation_strength = 0, // 0-100, 0 - disabled
|
||||
.pwmRxInputFilteringMode = INPUT_FILTERING_DISABLED
|
||||
);
|
||||
|
||||
PG_REGISTER(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 0);
|
||||
|
@ -159,16 +156,9 @@ uint16_t getCurrentMinthrottle(void)
|
|||
}
|
||||
|
||||
// Default settings
|
||||
void createDefaultConfig(master_t *config)
|
||||
void createDefaultConfig(void)
|
||||
{
|
||||
// Clear all configuration
|
||||
memset(config, 0, sizeof(master_t));
|
||||
|
||||
config->version = EEPROM_CONF_VERSION;
|
||||
|
||||
|
||||
config->pwmRxConfig.inputFilteringMode = INPUT_FILTERING_DISABLED;
|
||||
|
||||
// Radio
|
||||
#ifdef RX_CHANNELS_TAER
|
||||
parseRcChannels("TAER1234");
|
||||
|
@ -185,15 +175,6 @@ void createDefaultConfig(master_t *config)
|
|||
#if defined(TARGET_CONFIG)
|
||||
targetConfiguration();
|
||||
#endif
|
||||
|
||||
// copy first profile into remaining profile
|
||||
for (int i = 1; i < MAX_PROFILE_COUNT; i++) {
|
||||
memcpy(&config->profile[i], &config->profile[0], sizeof(profile_t));
|
||||
}
|
||||
|
||||
for (int i = 1; i < MAX_PROFILE_COUNT; i++) {
|
||||
config->profile[i].defaultRateProfileIndex = i % MAX_CONTROL_RATE_PROFILE_COUNT;
|
||||
}
|
||||
}
|
||||
|
||||
void resetConfigs(void)
|
||||
|
@ -201,7 +182,7 @@ void resetConfigs(void)
|
|||
pgResetAll(MAX_PROFILE_COUNT);
|
||||
pgActivateProfile(0);
|
||||
|
||||
createDefaultConfig(&masterConfig);
|
||||
createDefaultConfig();
|
||||
|
||||
setProfile(getCurrentProfileIndex());
|
||||
setControlRateProfile(getCurrentProfileIndex());
|
||||
|
@ -248,7 +229,7 @@ void validateAndFixConfig(void)
|
|||
}
|
||||
#endif
|
||||
#ifdef USE_DTERM_NOTCH
|
||||
if (pidProfile()->dterm_soft_notch_cutoff >= currentProfile->pidProfile.dterm_soft_notch_hz) {
|
||||
if (pidProfile()->dterm_soft_notch_cutoff >= pidProfile()->dterm_soft_notch_hz) {
|
||||
pidProfileMutable()->dterm_soft_notch_hz = 0;
|
||||
}
|
||||
#endif
|
||||
|
@ -523,7 +504,7 @@ void setProfile(uint8_t profileIndex)
|
|||
profileIndex = 0;
|
||||
}
|
||||
profileSelectionMutable()->current_profile_index = profileIndex;
|
||||
currentProfile = &masterConfig.profile[profileIndex];
|
||||
//!!! currentProfile = &masterConfig.profile[profileIndex];
|
||||
}
|
||||
|
||||
void changeProfile(uint8_t profileIndex)
|
||||
|
|
|
@ -20,8 +20,8 @@
|
|||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "common/time.h"
|
||||
|
||||
#include "config/parameter_group.h"
|
||||
#include "drivers/pwm_rx.h"
|
||||
|
||||
#define MAX_PROFILE_COUNT 3
|
||||
#define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500
|
||||
|
@ -86,6 +86,7 @@ typedef struct systemConfig_s {
|
|||
uint8_t debug_mode;
|
||||
uint8_t i2c_overclock; // Overclock i2c Bus for faster IMU readings
|
||||
uint8_t throttle_tilt_compensation_strength; // the correction that will be applied at throttle_correction_angle.
|
||||
inputFilteringMode_e pwmRxInputFilteringMode;
|
||||
char name[MAX_NAME_LENGTH + 1];
|
||||
} systemConfig_t;
|
||||
|
||||
|
@ -127,6 +128,7 @@ void applyAndSaveBoardAlignmentDelta(int16_t roll, int16_t pitch);
|
|||
|
||||
uint16_t getCurrentMinthrottle(void);
|
||||
|
||||
void createDefaultConfig(void);
|
||||
void resetConfigs(void);
|
||||
void targetConfiguration(void);
|
||||
|
||||
|
|
|
@ -114,7 +114,6 @@
|
|||
|
||||
#include "config/config_eeprom.h"
|
||||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
#include "config/feature.h"
|
||||
#include "config/parameter_group.h"
|
||||
#include "config/parameter_group_ids.h"
|
||||
|
@ -314,7 +313,7 @@ void init(void)
|
|||
pwm_params.enablePWMOutput = feature(FEATURE_PWM_OUTPUT_ENABLE);
|
||||
|
||||
#if defined(USE_RX_PWM) || defined(USE_RX_PPM)
|
||||
pwmRxInit(pwmRxConfig());
|
||||
pwmRxInit(systemConfig()->pwmRxInputFilteringMode);
|
||||
#endif
|
||||
|
||||
#ifdef USE_PMW_SERVO_DRIVER
|
||||
|
@ -499,7 +498,7 @@ void init(void)
|
|||
flashLedsAndBeep();
|
||||
|
||||
#ifdef USE_DTERM_NOTCH
|
||||
pidInitFilters(¤tProfile->pidProfile);
|
||||
pidInitFilters(pidProfile());
|
||||
#endif
|
||||
|
||||
imuInit();
|
||||
|
|
|
@ -58,7 +58,6 @@
|
|||
|
||||
#include "config/config_eeprom.h"
|
||||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
#include "config/feature.h"
|
||||
|
||||
|
||||
|
@ -67,6 +66,7 @@
|
|||
#include "io/gps.h"
|
||||
#include "io/gimbal.h"
|
||||
#include "io/ledstrip.h"
|
||||
#include "io/osd.h"
|
||||
#include "io/serial.h"
|
||||
#include "io/serial_4way.h"
|
||||
|
||||
|
@ -1539,7 +1539,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
#ifdef USE_DTERM_NOTCH
|
||||
pidProfileMutable()->dterm_soft_notch_hz = constrain(sbufReadU16(src), 0, 500);
|
||||
pidProfileMutable()->dterm_soft_notch_cutoff = constrain(sbufReadU16(src), 1, 500);
|
||||
pidInitFilters(¤tProfile->pidProfile);
|
||||
pidInitFilters(pidProfile());
|
||||
#endif
|
||||
#ifdef USE_GYRO_NOTCH_2
|
||||
gyroConfigMutable()->gyro_soft_notch_hz_2 = constrain(sbufReadU16(src), 0, 500);
|
||||
|
|
|
@ -77,7 +77,6 @@
|
|||
#include "flight/navigation_rewrite.h"
|
||||
|
||||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
#include "config/feature.h"
|
||||
|
||||
// June 2013 V2.2-dev
|
||||
|
@ -670,10 +669,10 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
// Update PID coefficients
|
||||
updatePIDCoefficients(¤tProfile->pidProfile, currentControlRateProfile, motorConfig());
|
||||
updatePIDCoefficients(pidProfile(), currentControlRateProfile, motorConfig());
|
||||
|
||||
// Calculate stabilisation
|
||||
pidController(¤tProfile->pidProfile, currentControlRateProfile, rxConfig());
|
||||
pidController(pidProfile(), currentControlRateProfile, rxConfig());
|
||||
|
||||
#ifdef HIL
|
||||
if (hilActive) {
|
||||
|
|
|
@ -45,7 +45,6 @@ uint8_t cliMode = 0;
|
|||
|
||||
#include "config/config_eeprom.h"
|
||||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
#include "config/feature.h"
|
||||
#include "config/parameter_group.h"
|
||||
#include "config/parameter_group_ids.h"
|
||||
|
@ -488,7 +487,6 @@ typedef union {
|
|||
cliMinMaxConfig_t minmax;
|
||||
} cliValueConfig_t;
|
||||
|
||||
#ifdef USE_PARAMETER_GROUPS
|
||||
typedef struct {
|
||||
const char *name;
|
||||
const uint8_t type; // see cliValueFlag_e
|
||||
|
@ -865,22 +863,9 @@ static const clivalue_t valueTable[] = {
|
|||
#endif
|
||||
{ "throttle_tilt_comp_str", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, throttle_tilt_compensation_strength) },
|
||||
{ "mode_range_logic_operator", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_AUX_OPERATOR }, PG_MODE_ACTIVATION_OPERATOR_CONFIG, offsetof(modeActivationOperatorConfig_t, modeActivationOperator) },
|
||||
{ "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, pwmRxInputFilteringMode) },
|
||||
};
|
||||
|
||||
#else
|
||||
|
||||
typedef struct {
|
||||
const char *name;
|
||||
const uint8_t type; // see cliValueFlag_e
|
||||
void *ptr;
|
||||
const cliValueConfig_t config;
|
||||
} clivalue_t;
|
||||
|
||||
const clivalue_t valueTable[] = {
|
||||
{ "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &pwmRxConfig()->inputFilteringMode, .config.lookup = { TABLE_OFF_ON } },
|
||||
};
|
||||
#endif
|
||||
|
||||
static void cliPrint(const char *str)
|
||||
{
|
||||
while (*str) {
|
||||
|
@ -1308,31 +1293,8 @@ static void restoreConfigs(void)
|
|||
#endif
|
||||
}
|
||||
|
||||
static void *getDefaultPointer(const void *valuePointer, const master_t *defaultConfig)
|
||||
static void dumpValues(uint16_t valueSection, uint8_t dumpMask)
|
||||
{
|
||||
return ((uint8_t *)valuePointer) - (uint32_t)&masterConfig + (uint32_t)defaultConfig;
|
||||
}
|
||||
|
||||
static bool valueEqualsDefault(const clivalue_t *value, const master_t *defaultConfig)
|
||||
{
|
||||
const void *ptr = getValuePointer(value);
|
||||
const void *ptrDefault = getDefaultPointer(ptr, defaultConfig);
|
||||
return valuePtrEqualsDefault(value->type, ptr, ptrDefault);
|
||||
}
|
||||
|
||||
static void cliPrintVarDefault(const clivalue_t *var, uint32_t full, const master_t *defaultConfig)
|
||||
{
|
||||
void *ptr = getValuePointer(var);
|
||||
|
||||
void *defaultPtr = getDefaultPointer(ptr, defaultConfig);
|
||||
|
||||
printValuePointer(var, defaultPtr, full);
|
||||
}
|
||||
|
||||
static void dumpValues(uint16_t valueSection, uint8_t dumpMask, const master_t *defaultConfig)
|
||||
{
|
||||
const char *format = "set %s = ";
|
||||
#ifdef USE_PARAMETER_GROUPS
|
||||
if (valueSection == MASTER_VALUE) {
|
||||
// gyroConfig() has been set to default, gyroConfigCopy contains current value
|
||||
dumpPgValues(MASTER_VALUE, dumpMask, PG_GYRO_CONFIG, &gyroConfigCopy, gyroConfig());
|
||||
|
@ -1378,21 +1340,6 @@ static void dumpValues(uint16_t valueSection, uint8_t dumpMask, const master_t *
|
|||
#endif
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) {
|
||||
const clivalue_t *value = &valueTable[i];
|
||||
if ((value->type & VALUE_SECTION_MASK) == valueSection) {
|
||||
const bool equalsDefault = valueEqualsDefault(value, defaultConfig);
|
||||
if (cliDefaultPrintf(dumpMask, equalsDefault, format, valueTable[i].name)) {
|
||||
cliPrintVarDefault(value, 0, defaultConfig);
|
||||
cliPrint("\r\n");
|
||||
}
|
||||
if (cliDumpPrintf(dumpMask, equalsDefault, format, valueTable[i].name)) {
|
||||
cliPrintVar(value, 0);
|
||||
cliPrint("\r\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
cliPrint("\r\n");
|
||||
}
|
||||
|
||||
|
@ -3167,7 +3114,7 @@ static void cliRateProfile(char *cmdline)
|
|||
}
|
||||
}
|
||||
|
||||
static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, const master_t *defaultConfig)
|
||||
static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask)
|
||||
{
|
||||
if (profileIndex >= MAX_PROFILE_COUNT) {
|
||||
// Faulty values
|
||||
|
@ -3177,10 +3124,10 @@ static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, const master_
|
|||
cliPrintHashLine("profile");
|
||||
cliProfile("");
|
||||
cliPrint("\r\n");
|
||||
dumpValues(PROFILE_VALUE, dumpMask, defaultConfig);
|
||||
dumpValues(PROFILE_VALUE, dumpMask);
|
||||
}
|
||||
|
||||
static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, const master_t *defaultConfig)
|
||||
static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask)
|
||||
{
|
||||
if (rateProfileIndex >= MAX_CONTROL_RATE_PROFILE_COUNT) {
|
||||
// Faulty values
|
||||
|
@ -3190,7 +3137,7 @@ static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, const
|
|||
cliPrintHashLine("rateprofile");
|
||||
cliRateProfile("");
|
||||
cliPrint("\r\n");
|
||||
dumpValues(CONTROL_RATE_VALUE, dumpMask, defaultConfig);
|
||||
dumpValues(CONTROL_RATE_VALUE, dumpMask);
|
||||
}
|
||||
|
||||
static void cliSave(char *cmdline)
|
||||
|
@ -3384,7 +3331,7 @@ static void cliStatus(char *cmdline)
|
|||
#endif
|
||||
cliPrintf("Stack size: %d, Stack address: 0x%x\r\n", stackTotalSize(), stackHighMem());
|
||||
|
||||
cliPrintf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", (uint16_t)cycleTime, i2cErrorCounter, sizeof(master_t));
|
||||
cliPrintf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", (uint16_t)cycleTime, i2cErrorCounter, PG_REGISTRY_SIZE);
|
||||
|
||||
#ifdef USE_SDCARD
|
||||
cliSdInfo(NULL);
|
||||
|
@ -3477,8 +3424,7 @@ static void printConfig(char *cmdline, bool doDiff)
|
|||
dumpMask = dumpMask | DO_DIFF;
|
||||
}
|
||||
|
||||
static master_t defaultConfig;
|
||||
createDefaultConfig(&defaultConfig);
|
||||
createDefaultConfig();
|
||||
|
||||
backupConfigs();
|
||||
#ifdef USE_PARAMETER_GROUPS
|
||||
|
@ -3565,12 +3511,12 @@ static void printConfig(char *cmdline, bool doDiff)
|
|||
printRxFail(dumpMask, rxFailsafeChannelConfigsCopy, rxFailsafeChannelConfigs(0));
|
||||
|
||||
cliPrintHashLine("master");
|
||||
dumpValues(MASTER_VALUE, dumpMask, &defaultConfig);
|
||||
dumpValues(MASTER_VALUE, dumpMask);
|
||||
|
||||
if (dumpMask & DUMP_ALL) {
|
||||
const uint8_t activeProfile = getCurrentProfileIndex();
|
||||
for (uint32_t profileCount=0; profileCount<MAX_PROFILE_COUNT;profileCount++) {
|
||||
cliDumpProfile(profileCount, dumpMask, &defaultConfig);
|
||||
cliDumpProfile(profileCount, dumpMask);
|
||||
}
|
||||
|
||||
changeProfile(activeProfile);
|
||||
|
@ -3579,24 +3525,24 @@ static void printConfig(char *cmdline, bool doDiff)
|
|||
|
||||
uint8_t currentRateIndex = getCurrentControlRateProfile();
|
||||
for (uint32_t rateCount = 0; rateCount<MAX_CONTROL_RATE_PROFILE_COUNT; rateCount++) {
|
||||
cliDumpRateProfile(rateCount, dumpMask, &defaultConfig);
|
||||
cliDumpRateProfile(rateCount, dumpMask);
|
||||
}
|
||||
changeControlRateProfile(currentRateIndex);
|
||||
cliPrintHashLine("restore original rateprofile selection");
|
||||
cliRateProfile("");
|
||||
cliPrintHashLine("save configuration\r\nsave");
|
||||
} else {
|
||||
cliDumpProfile(getCurrentProfileIndex(), dumpMask, &defaultConfig);
|
||||
cliDumpRateProfile(getCurrentControlRateProfile(), dumpMask, &defaultConfig);
|
||||
cliDumpProfile(getCurrentProfileIndex(), dumpMask);
|
||||
cliDumpRateProfile(getCurrentControlRateProfile(), dumpMask);
|
||||
}
|
||||
}
|
||||
|
||||
if (dumpMask & DUMP_PROFILE) {
|
||||
cliDumpProfile(getCurrentProfileIndex(), dumpMask, &defaultConfig);
|
||||
cliDumpProfile(getCurrentProfileIndex(), dumpMask);
|
||||
}
|
||||
|
||||
if (dumpMask & DUMP_RATES) {
|
||||
cliDumpRateProfile(getCurrentControlRateProfile(), dumpMask, &defaultConfig);
|
||||
cliDumpRateProfile(getCurrentControlRateProfile(), dumpMask);
|
||||
}
|
||||
// restore configs from copies
|
||||
restoreConfigs();
|
||||
|
|
|
@ -60,7 +60,6 @@
|
|||
|
||||
#include "config/config_profile.h"
|
||||
#include "config/feature.h"
|
||||
#include "config/config_master.h"
|
||||
|
||||
|
||||
extern const mixer_t mixers[];
|
||||
|
|
|
@ -24,8 +24,6 @@
|
|||
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "config/config_master.h"
|
||||
|
||||
#include "drivers/display.h"
|
||||
#include "drivers/max7456.h"
|
||||
|
||||
|
|
|
@ -73,7 +73,6 @@
|
|||
#include "flight/imu.h"
|
||||
#include "flight/navigation_rewrite.h"
|
||||
|
||||
#include "config/config_master.h"
|
||||
#include "config/config_profile.h"
|
||||
#include "config/feature.h"
|
||||
|
||||
|
|
|
@ -48,7 +48,6 @@
|
|||
#include "common/utils.h"
|
||||
|
||||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
#include "config/feature.h"
|
||||
#include "config/parameter_group.h"
|
||||
#include "config/parameter_group_ids.h"
|
||||
|
@ -361,22 +360,19 @@ static void osdDrawSingleElement(uint8_t item)
|
|||
|
||||
case OSD_ROLL_PIDS:
|
||||
{
|
||||
const pidProfile_t *pidProfile = ¤tProfile->pidProfile;
|
||||
sprintf(buff, "ROL %3d %3d %3d", pidProfile->P8[PIDROLL], pidProfile->I8[PIDROLL], pidProfile->D8[PIDROLL]);
|
||||
sprintf(buff, "ROL %3d %3d %3d", pidProfile()->P8[PIDROLL], pidProfile()->I8[PIDROLL], pidProfile()->D8[PIDROLL]);
|
||||
break;
|
||||
}
|
||||
|
||||
case OSD_PITCH_PIDS:
|
||||
{
|
||||
const pidProfile_t *pidProfile = ¤tProfile->pidProfile;
|
||||
sprintf(buff, "PIT %3d %3d %3d", pidProfile->P8[PIDPITCH], pidProfile->I8[PIDPITCH], pidProfile->D8[PIDPITCH]);
|
||||
sprintf(buff, "PIT %3d %3d %3d", pidProfile()->P8[PIDPITCH], pidProfile()->I8[PIDPITCH], pidProfile()->D8[PIDPITCH]);
|
||||
break;
|
||||
}
|
||||
|
||||
case OSD_YAW_PIDS:
|
||||
{
|
||||
const pidProfile_t *pidProfile = ¤tProfile->pidProfile;
|
||||
sprintf(buff, "YAW %3d %3d %3d", pidProfile->P8[PIDYAW], pidProfile->I8[PIDYAW], pidProfile->D8[PIDYAW]);
|
||||
sprintf(buff, "YAW %3d %3d %3d", pidProfile()->P8[PIDYAW], pidProfile()->I8[PIDYAW], pidProfile()->D8[PIDYAW]);
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
64
src/main/target/ALIENFLIGHTF3/config.c
Normal file
64
src/main/target/ALIENFLIGHTF3/config.c
Normal file
|
@ -0,0 +1,64 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <platform.h>
|
||||
|
||||
#include "common/axis.h"
|
||||
|
||||
#include "fc/controlrate_profile.h"
|
||||
#include "fc/rc_controls.h"
|
||||
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/pid.h"
|
||||
|
||||
#include "io/serial.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
|
||||
// alternative defaults settings for BlueJayF4 targets
|
||||
void targetConfiguration(void)
|
||||
{
|
||||
// alternative defaults settings for ALIENFLIGHTF1 and ALIENFLIGHTF3 targets
|
||||
serialConfigMutable()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
|
||||
batteryConfigMutable()->vbatscale = 20;
|
||||
rxConfigMutable()->spektrum_sat_bind = 5;
|
||||
motorConfigMutable()->minthrottle = 1000;
|
||||
motorConfigMutable()->maxthrottle = 2000;
|
||||
motorConfigMutable()->motorPwmRate = 32000;
|
||||
pidProfileMutable()->P8[ROLL] = 36;
|
||||
pidProfileMutable()->P8[PITCH] = 36;
|
||||
failsafeConfigMutable()->failsafe_delay = 2;
|
||||
failsafeConfigMutable()->failsafe_off_delay = 0;
|
||||
controlRateProfilesMutable(0)->rates[FD_PITCH] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT;
|
||||
controlRateProfilesMutable(0)->rates[FD_ROLL] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT;
|
||||
controlRateProfilesMutable(0)->rates[FD_YAW] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT;
|
||||
parseRcChannels("TAER1234");
|
||||
|
||||
*customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R
|
||||
*customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R
|
||||
*customMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L
|
||||
*customMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L
|
||||
*customMotorMixerMutable(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R
|
||||
*customMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L
|
||||
*customMotorMixerMutable(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R
|
||||
*customMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L
|
||||
}
|
|
@ -40,6 +40,7 @@
|
|||
#include "io/gps.h"
|
||||
#include "io/serial.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
|
@ -53,7 +54,6 @@
|
|||
|
||||
#include "config/feature.h"
|
||||
//#include "config/config.h"
|
||||
#include "config/config_master.h"
|
||||
|
||||
#define CRSF_CYCLETIME_US 100000 // 100ms, 10 Hz
|
||||
|
||||
|
|
|
@ -69,7 +69,6 @@
|
|||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
#include "config/feature.h"
|
||||
|
||||
#include "fc/mw.h"
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue