Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 17:55:28 +03:00
Code Issues Wiki Activity

Added imuConfig parameter group

Browse source
This commit is contained in:
Martin Budden 2017-01-11 17:50:38 +00:00
parent 2464b8602e
commit 10a0c8655f
6 changed files with 44 additions and 35 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/config/config_master.h
View file

@ -50,8 +50,6 @@
#define servoMixerConfig(x) (&masterConfig.servoMixerConfig)
#define servoMixerConfigMutable(x) (&masterConfig.servoMixerConfig)
#define imuConfig(x) (&masterConfig.imuConfig)
#define imuConfigMutable(x) (&masterConfig.imuConfig)
#define rcControlsConfig(x) (&masterConfig.rcControlsConfig)
#define rcControlsConfigMutable(x) (&masterConfig.rcControlsConfig)
#define gpsConfig(x) (&masterConfig.gpsConfig)
@ -113,8 +111,6 @@ typedef struct master_s {
#endif
imuConfig_t imuConfig;
#ifdef GPS
gpsConfig_t gpsConfig;
#endif

2
src/main/config/parameter_group_ids.h
View file

@ -37,7 +37,7 @@
//#define PG_FEATURE_CONFIG 19
#define PG_MIXER_CONFIG 20
//#define PG_SERVO_MIXER 21
//#define PG_IMU_CONFIG 22
#define PG_IMU_CONFIG 22
//#define PG_PROFILE_SELECTION 23
#define PG_RX_CONFIG 24
//#define PG_RC_CONTROLS_CONFIG 25

9
src/main/fc/config.c
View file

@ -342,13 +342,6 @@ void createDefaultConfig(master_t *config)
// profile
config->current_profile_index = 0;
// IMU
config->imuConfig.dcm_kp_acc = 2500; // 0.25 * 10000
config->imuConfig.dcm_ki_acc = 50; // 0.005 * 10000
config->imuConfig.dcm_kp_mag = 10000; // 1.00 * 10000
config->imuConfig.dcm_ki_mag = 0; // 0.00 * 10000
config->imuConfig.small_angle = 25;
#ifdef TELEMETRY
resetTelemetryConfig(&config->telemetryConfig);
#endif
@ -552,7 +545,7 @@ static void activateConfig(void)
servosUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoConf);
#endif
imuConfigure(&masterConfig.imuConfig, &currentProfile->pidProfile);
imuConfigure(&currentProfile->pidProfile);
pidInit();

16
src/main/fc/serial_cli.c
View file

@ -642,6 +642,13 @@ static const clivalue_t valueTable[] = {
// PG_SERIAL_CONFIG
{ "reboot_character", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 48, 126 }, PG_SERIAL_CONFIG, offsetof(serialConfig_t, reboot_character) },
// PG_IMU_CONFIG
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, UINT16_MAX }, PG_IMU_CONFIG, offsetof(imuConfig_t, dcm_kp_acc) },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, UINT16_MAX }, PG_IMU_CONFIG, offsetof(imuConfig_t, dcm_ki_acc) },
{ "imu_dcm_kp_mag", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, UINT16_MAX }, PG_IMU_CONFIG, offsetof(imuConfig_t, dcm_kp_mag) },
{ "imu_dcm_ki_mag", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, UINT16_MAX }, PG_IMU_CONFIG, offsetof(imuConfig_t, dcm_ki_mag) },
{ "small_angle", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 180 }, PG_IMU_CONFIG, offsetof(imuConfig_t, small_angle) },
};
#else
@ -667,7 +674,6 @@ const clivalue_t valueTable[] = {
{ "fixed_wing_auto_arm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &armingConfig()->fixed_wing_auto_arm, .config.lookup = { TABLE_OFF_ON } },
{ "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &armingConfig()->disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } },
{ "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, &armingConfig()->auto_disarm_delay, .config.minmax = { 0, 60 } },
{ "small_angle", VAR_UINT8 | MASTER_VALUE, &imuConfig()->small_angle, .config.minmax = { 0, 180 } },
#ifdef GPS
@ -777,11 +783,6 @@ const clivalue_t valueTable[] = {
#endif
#endif
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &imuConfig()->dcm_kp_acc, .config.minmax = { 0, 65535 } },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &imuConfig()->dcm_ki_acc, .config.minmax = { 0, 65535 } },
{ "imu_dcm_kp_mag", VAR_UINT16 | MASTER_VALUE, &imuConfig()->dcm_kp_mag, .config.minmax = { 0, 65535 } },
{ "imu_dcm_ki_mag", VAR_UINT16 | MASTER_VALUE, &imuConfig()->dcm_ki_mag, .config.minmax = { 0, 65535 } },
{ "deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.rcControlsConfig.deadband, .config.minmax = { 0, 32 }, },
{ "yaw_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.rcControlsConfig.yaw_deadband, .config.minmax = { 0, 100 }, },
{ "pos_hold_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.rcControlsConfig.pos_hold_deadband, .config.minmax = { 10, 250 }, },
@ -1110,6 +1111,7 @@ static motorMixer_t customMotorMixerCopy[MAX_SUPPORTED_MOTORS];
static mixerConfig_t mixerConfigCopy;
static flight3DConfig_t flight3DConfigCopy;
static serialConfig_t serialConfigCopy;
static imuConfig_t imuConfigCopy;
static void backupConfigs(void)
{
@ -1144,6 +1146,7 @@ static void backupConfigs(void)
mixerConfigCopy = *mixerConfig();
flight3DConfigCopy = *flight3DConfig();
serialConfigCopy = *serialConfig();
imuConfigCopy = *imuConfig();
}
static void restoreConfigs(void)
@ -1178,6 +1181,7 @@ static void restoreConfigs(void)
*mixerConfigMutable() = mixerConfigCopy;
*flight3DConfigMutable() = flight3DConfigCopy;
*serialConfigMutable() = serialConfigCopy;
*imuConfigMutable() = imuConfigCopy;
}
static void *getDefaultPointer(const void *valuePointer, const master_t *defaultConfig)

43
src/main/flight/imu.c
View file

@ -23,27 +23,22 @@
#include "common/maths.h"
#include "build/build_config.h"
#include "platform.h"
#include "build/debug.h"
#include "build/build_config.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/filter.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "fc/config.h"
#include "fc/runtime_config.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
@ -51,6 +46,13 @@
#include "io/gps.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
/**
* In Cleanflight accelerometer is aligned in the following way:
* X-axis = Forward
@ -91,6 +93,17 @@ static float gyroScale;
static bool gpsHeadingInitialized = false;
PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
.dcm_kp_acc = 2500, // 0.25 * 10000
.dcm_ki_acc = 50, // 0.005 * 10000
.dcm_kp_mag = 10000, // 1.00 * 10000
.dcm_ki_mag = 0, // 0.00 * 10000
.small_angle = 25
);
#ifdef ASYNC_GYRO_PROCESSING
/* Asynchronous update accumulators */
static float imuAccumulatedRate[XYZ_AXIS_COUNT];
@ -139,13 +152,13 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
}
void imuConfigure(imuConfig_t *imuConfig, pidProfile_t *initialPidProfile)
void imuConfigure(pidProfile_t *initialPidProfile)
{
imuRuntimeConfig.dcm_kp_acc = imuConfig->dcm_kp_acc / 10000.0f;
imuRuntimeConfig.dcm_ki_acc = imuConfig->dcm_ki_acc / 10000.0f;
imuRuntimeConfig.dcm_kp_mag = imuConfig->dcm_kp_mag / 10000.0f;
imuRuntimeConfig.dcm_ki_mag = imuConfig->dcm_ki_mag / 10000.0f;
imuRuntimeConfig.small_angle = imuConfig->small_angle;
imuRuntimeConfig.dcm_kp_acc = imuConfig()->dcm_kp_acc / 10000.0f;
imuRuntimeConfig.dcm_ki_acc = imuConfig()->dcm_ki_acc / 10000.0f;
imuRuntimeConfig.dcm_kp_mag = imuConfig()->dcm_kp_mag / 10000.0f;
imuRuntimeConfig.dcm_ki_mag = imuConfig()->dcm_ki_mag / 10000.0f;
imuRuntimeConfig.small_angle = imuConfig()->small_angle;
pidProfile = initialPidProfile;
}

5
src/main/flight/imu.h
View file

@ -18,6 +18,7 @@
#pragma once
#include "common/time.h"
#include "config/parameter_group.h"
#define GRAVITY_CMSS 980.665f
@ -48,6 +49,8 @@ typedef struct imuConfig_s {
uint8_t small_angle;
} imuConfig_t;
PG_DECLARE(imuConfig_t, imuConfig);
typedef struct imuRuntimeConfig_s {
float dcm_kp_acc;
float dcm_ki_acc;
@ -57,7 +60,7 @@ typedef struct imuRuntimeConfig_s {
} imuRuntimeConfig_t;
struct pidProfile_s;
void imuConfigure(imuConfig_t *imuConfig, struct pidProfile_s *initialPidProfile);
void imuConfigure(struct pidProfile_s *initialPidProfile);
void imuUpdateAttitude(timeUs_t currentTimeUs);
void imuUpdateAccelerometer(void);