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 compassConfig parameter group

Browse source
This commit is contained in:
Martin Budden 2017-01-01 21:29:57 +00:00
parent 3e1c158ad0
commit a690e49239
15 changed files with 139 additions and 125 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -58,7 +58,6 @@
#include "sensors/boardalignment.h" #include "sensors/boardalignment.h"
#include "sensors/barometer.h" #include "sensors/barometer.h"
#include "sensors/battery.h" #include "sensors/battery.h"
#include "sensors/compass.h"
#include "sensors/pitotmeter.h" #include "sensors/pitotmeter.h"
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
@ -70,7 +69,6 @@
#define boardAlignment(x) (&masterConfig.boardAlignment) #define boardAlignment(x) (&masterConfig.boardAlignment)
#define imuConfig(x) (&masterConfig.imuConfig) #define imuConfig(x) (&masterConfig.imuConfig)
#define barometerConfig(x) (&masterConfig.barometerConfig) #define barometerConfig(x) (&masterConfig.barometerConfig)
#define compassConfig(x) (&masterConfig.compassConfig)
#define pitotmeterConfig(x) (&masterConfig.pitotmeterConfig) #define pitotmeterConfig(x) (&masterConfig.pitotmeterConfig)
#define batteryConfig(x) (&masterConfig.batteryConfig) #define batteryConfig(x) (&masterConfig.batteryConfig)
#define gpsConfig(x) (&masterConfig.gpsConfig) #define gpsConfig(x) (&masterConfig.gpsConfig)
@ -144,8 +142,6 @@ typedef struct master_s {
barometerConfig_t barometerConfig; barometerConfig_t barometerConfig;
compassConfig_t compassConfig;
pitotmeterConfig_t pitotmeterConfig; pitotmeterConfig_t pitotmeterConfig;
batteryConfig_t batteryConfig; batteryConfig_t batteryConfig;

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

@ -16,61 +16,61 @@
*/ */
// FC configuration // FC configuration
#define PG_FAILSAFE_CONFIG 1 //#define PG_FAILSAFE_CONFIG 1
#define PG_BOARD_ALIGNMENT 2 //#define PG_BOARD_ALIGNMENT 2
#define PG_GIMBAL_CONFIG 3 //#define PG_GIMBAL_CONFIG 3
#define PG_MOTOR_MIXER 4 //#define PG_MOTOR_MIXER 4
#define PG_BLACKBOX_CONFIG 5 //#define PG_BLACKBOX_CONFIG 5
#define PG_MOTOR_AND_SERVO_CONFIG 6 //#define PG_MOTOR_AND_SERVO_CONFIG 6
#define PG_SENSOR_SELECTION_CONFIG 7 //#define PG_SENSOR_SELECTION_CONFIG 7
#define PG_SENSOR_ALIGNMENT_CONFIG 8 //#define PG_SENSOR_ALIGNMENT_CONFIG 8
#define PG_SENSOR_TRIMS 9 //#define PG_SENSOR_TRIMS 9
#define PG_GYRO_CONFIG 10 #define PG_GYRO_CONFIG 10
#define PG_BATTERY_CONFIG 11 //#define PG_BATTERY_CONFIG 11
#define PG_CONTROL_RATE_PROFILES 12 //#define PG_CONTROL_RATE_PROFILES 12
#define PG_SERIAL_CONFIG 13 //#define PG_SERIAL_CONFIG 13
#define PG_PID_PROFILE 14 //#define PG_PID_PROFILE 14
#define PG_GTUNE_CONFIG 15 //#define PG_GTUNE_CONFIG 15
#define PG_ARMING_CONFIG 16 //#define PG_ARMING_CONFIG 16
#define PG_TRANSPONDER_CONFIG 17 //#define PG_TRANSPONDER_CONFIG 17
#define PG_SYSTEM_CONFIG 18 //#define PG_SYSTEM_CONFIG 18
#define PG_FEATURE_CONFIG 19 //#define PG_FEATURE_CONFIG 19
#define PG_MIXER_CONFIG 20 //#define PG_MIXER_CONFIG 20
#define PG_SERVO_MIXER 21 //#define PG_SERVO_MIXER 21
#define PG_IMU_CONFIG 22 //#define PG_IMU_CONFIG 22
#define PG_PROFILE_SELECTION 23 //#define PG_PROFILE_SELECTION 23
#define PG_RX_CONFIG 24 //#define PG_RX_CONFIG 24
#define PG_RC_CONTROLS_CONFIG 25 //#define PG_RC_CONTROLS_CONFIG 25
#define PG_MOTOR_3D_CONFIG 26 //#define PG_MOTOR_3D_CONFIG 26
#define PG_LED_STRIP_CONFIG 27 //#define PG_LED_STRIP_CONFIG 27
#define PG_COLOR_CONFIG 28 //#define PG_COLOR_CONFIG 28
#define PG_AIRPLANE_ALT_HOLD_CONFIG 29 //#define PG_AIRPLANE_ALT_HOLD_CONFIG 29
#define PG_GPS_CONFIG 30 //#define PG_GPS_CONFIG 30
#define PG_TELEMETRY_CONFIG 31 //#define PG_TELEMETRY_CONFIG 31
#define PG_FRSKY_TELEMETRY_CONFIG 32 //#define PG_FRSKY_TELEMETRY_CONFIG 32
#define PG_HOTT_TELEMETRY_CONFIG 33 //#define PG_HOTT_TELEMETRY_CONFIG 33
#define PG_NAVIGATION_CONFIG 34 //#define PG_NAVIGATION_CONFIG 34
#define PG_ACCELEROMETER_CONFIG 35 #define PG_ACCELEROMETER_CONFIG 35
#define PG_RATE_PROFILE_SELECTION 36 //#define PG_RATE_PROFILE_SELECTION 36
#define PG_ADJUSTMENT_PROFILE 37 //#define PG_ADJUSTMENT_PROFILE 37
#define PG_BAROMETER_CONFIG 38 //#define PG_BAROMETER_CONFIG 38
#define PG_THROTTLE_CORRECTION_CONFIG 39 //#define PG_THROTTLE_CORRECTION_CONFIG 39
#define PG_COMPASS_CONFIGURATION 40 #define PG_COMPASS_CONFIG 40
#define PG_MODE_ACTIVATION_PROFILE 41 //#define PG_MODE_ACTIVATION_PROFILE 41
#define PG_SERVO_PROFILE 42 //#define PG_SERVO_PROFILE 42
#define PG_FAILSAFE_CHANNEL_CONFIG 43 //#define PG_FAILSAFE_CHANNEL_CONFIG 43
#define PG_CHANNEL_RANGE_CONFIG 44 //#define PG_CHANNEL_RANGE_CONFIG 44
#define PG_MODE_COLOR_CONFIG 45 //#define PG_MODE_COLOR_CONFIG 45
#define PG_SPECIAL_COLOR_CONFIG 46 //#define PG_SPECIAL_COLOR_CONFIG 46
// Driver configuration // Driver configuration
#define PG_DRIVER_PWM_RX_CONFIG 100 //#define PG_DRIVER_PWM_RX_CONFIG 100
#define PG_DRIVER_FLASHCHIP_CONFIG 101 //#define PG_DRIVER_FLASHCHIP_CONFIG 101
// OSD configuration (subject to change) // OSD configuration (subject to change)
#define PG_OSD_FONT_CONFIG 2047 //#define PG_OSD_FONT_CONFIG 2047
#define PG_OSD_VIDEO_CONFIG 2046 //#define PG_OSD_VIDEO_CONFIG 2046
#define PG_OSD_ELEMENT_CONFIG 2045 //#define PG_OSD_ELEMENT_CONFIG 2045
// 4095 is currently the highest number that can be used for a PGN due to the top 4 bits of the 16 bit value being reserved for the version when the PG is stored in an EEPROM. // 4095 is currently the highest number that can be used for a PGN due to the top 4 bits of the 16 bit value being reserved for the version when the PG is stored in an EEPROM.
#define PG_RESERVED_FOR_TESTING_1 4095 #define PG_RESERVED_FOR_TESTING_1 4095

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

@ -150,7 +150,6 @@ void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->axisAccelerationLimitRollPitch = 0; // dps/s pidProfile->axisAccelerationLimitRollPitch = 0; // dps/s
pidProfile->yaw_p_limit = YAW_P_LIMIT_DEFAULT; pidProfile->yaw_p_limit = YAW_P_LIMIT_DEFAULT;
pidProfile->mag_hold_rate_limit = MAG_HOLD_RATE_LIMIT_DEFAULT;
pidProfile->max_angle_inclination[FD_ROLL] = 300; // 30 degrees pidProfile->max_angle_inclination[FD_ROLL] = 300; // 30 degrees
pidProfile->max_angle_inclination[FD_PITCH] = 300; // 30 degrees pidProfile->max_angle_inclination[FD_PITCH] = 300; // 30 degrees
@ -475,18 +474,10 @@ void createDefaultConfig(master_t *config)
resetAccelerometerTrims(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain); resetAccelerometerTrims(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
config->compassConfig.mag_align = ALIGN_DEFAULT;
config->boardAlignment.rollDeciDegrees = 0; config->boardAlignment.rollDeciDegrees = 0;
config->boardAlignment.pitchDeciDegrees = 0; config->boardAlignment.pitchDeciDegrees = 0;
config->boardAlignment.yawDeciDegrees = 0; config->boardAlignment.yawDeciDegrees = 0;
#ifdef MAG
config->compassConfig.mag_hardware = MAG_AUTODETECT;
#else
config->compassConfig.mag_hardware = MAG_NONE;
#endif
#ifdef BARO #ifdef BARO
config->barometerConfig.baro_hardware = BARO_AUTODETECT; config->barometerConfig.baro_hardware = BARO_AUTODETECT;
#else #else
@ -581,8 +572,6 @@ void createDefaultConfig(master_t *config)
// for (int i = 0; i < CHECKBOXITEMS; i++) // for (int i = 0; i < CHECKBOXITEMS; i++)
// cfg.activate[i] = 0; // cfg.activate[i] = 0;
config->compassConfig.mag_declination = 0;
config->modeActivationOperator = MODE_OPERATOR_OR; // default is to OR multiple-channel mode activation conditions config->modeActivationOperator = MODE_OPERATOR_OR; // default is to OR multiple-channel mode activation conditions
resetBarometerConfig(&config->barometerConfig); resetBarometerConfig(&config->barometerConfig);

1
src/main/fc/fc_init.c
View file

@ -499,7 +499,6 @@ void init(void)
#endif #endif
if (!sensorsAutodetect( if (!sensorsAutodetect(
&masterConfig.compassConfig,
&masterConfig.barometerConfig, &masterConfig.barometerConfig,
&masterConfig.pitotmeterConfig &masterConfig.pitotmeterConfig
)) { )) {

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

@ -1128,7 +1128,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
#endif #endif
#ifdef MAG #ifdef MAG
sbufWriteU8(dst, currentProfile->pidProfile.mag_hold_rate_limit); sbufWriteU8(dst, compassConfig()->mag_hold_rate_limit);
sbufWriteU8(dst, MAG_HOLD_ERROR_LPF_FREQ); sbufWriteU8(dst, MAG_HOLD_ERROR_LPF_FREQ);
#else #else
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
@ -1146,7 +1146,11 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_SENSOR_CONFIG: case MSP_SENSOR_CONFIG:
sbufWriteU8(dst, accelerometerConfig()->acc_hardware); sbufWriteU8(dst, accelerometerConfig()->acc_hardware);
sbufWriteU8(dst, barometerConfig()->baro_hardware); sbufWriteU8(dst, barometerConfig()->baro_hardware);
#ifdef MAG
sbufWriteU8(dst, compassConfig()->mag_hardware); sbufWriteU8(dst, compassConfig()->mag_hardware);
#else
sbufWriteU8(dst, 0);
#endif
sbufWriteU8(dst, pitotmeterConfig()->pitot_hardware); sbufWriteU8(dst, pitotmeterConfig()->pitot_hardware);
sbufWriteU8(dst, 0); // rangefinder hardware sbufWriteU8(dst, 0); // rangefinder hardware
sbufWriteU8(dst, 0); // optical flow hardware sbufWriteU8(dst, 0); // optical flow hardware
@ -1385,7 +1389,11 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
rxConfig()->rssi_channel = sbufReadU8(src); rxConfig()->rssi_channel = sbufReadU8(src);
sbufReadU8(src); sbufReadU8(src);
#ifdef MAG
compassConfig()->mag_declination = sbufReadU16(src) * 10; compassConfig()->mag_declination = sbufReadU16(src) * 10;
#else
sbufReadU16(src);
#endif
batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
@ -1462,7 +1470,11 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_SENSOR_ALIGNMENT: case MSP_SET_SENSOR_ALIGNMENT:
gyroConfig()->gyro_align = sbufReadU8(src); gyroConfig()->gyro_align = sbufReadU8(src);
accelerometerConfig()->acc_align = sbufReadU8(src); accelerometerConfig()->acc_align = sbufReadU8(src);
#ifdef MAG
compassConfig()->mag_align = sbufReadU8(src); compassConfig()->mag_align = sbufReadU8(src);
#else
sbufReadU8(src);
#endif
break; break;
case MSP_SET_ADVANCED_CONFIG: case MSP_SET_ADVANCED_CONFIG:
@ -1537,7 +1549,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
sbufReadU16(src); sbufReadU16(src);
#endif #endif
#ifdef MAG #ifdef MAG
currentProfile->pidProfile.mag_hold_rate_limit = sbufReadU8(src); compassConfig()->mag_hold_rate_limit = sbufReadU8(src);
sbufReadU8(src); //MAG_HOLD_ERROR_LPF_FREQ sbufReadU8(src); //MAG_HOLD_ERROR_LPF_FREQ
#else #else
sbufReadU8(src); sbufReadU8(src);

2
src/main/fc/fc_tasks.c
View file

@ -144,7 +144,7 @@ void taskProcessGPS(timeUs_t currentTimeUs)
void taskUpdateCompass(timeUs_t currentTimeUs) void taskUpdateCompass(timeUs_t currentTimeUs)
{ {
if (sensors(SENSOR_MAG)) { if (sensors(SENSOR_MAG)) {
compassUpdate(currentTimeUs, &compassConfig()->magZero); compassUpdate(currentTimeUs);
} }
} }
#endif #endif

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

@ -426,7 +426,7 @@ static int imuCalculateAccelerometerConfidence(void)
static void imuCalculateEstimatedAttitude(float dT) static void imuCalculateEstimatedAttitude(float dT)
{ {
#if defined(MAG) #if defined(MAG)
const bool canUseMAG = sensors(SENSOR_MAG) && isCompassHealthy(); const bool canUseMAG = sensors(SENSOR_MAG) && compassIsHealthy();
#else #else
const bool canUseMAG = false; const bool canUseMAG = false;
#endif #endif

3
src/main/flight/pid.c
View file

@ -45,6 +45,7 @@
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/gyro.h" #include "sensors/gyro.h"
#include "sensors/acceleration.h" #include "sensors/acceleration.h"
#include "sensors/compass.h"
typedef struct { typedef struct {
@ -521,7 +522,7 @@ float pidMagHold(const pidProfile_t *pidProfile)
*/ */
magHoldRate = error * pidProfile->P8[PIDMAG] / 30; magHoldRate = error * pidProfile->P8[PIDMAG] / 30;
magHoldRate = constrainf(magHoldRate, -pidProfile->mag_hold_rate_limit, pidProfile->mag_hold_rate_limit); magHoldRate = constrainf(magHoldRate, -compassConfig()->mag_hold_rate_limit, compassConfig()->mag_hold_rate_limit);
magHoldRate = pt1FilterApply4(&magHoldRateFilter, magHoldRate, MAG_HOLD_ERROR_LPF_FREQ, dT); magHoldRate = pt1FilterApply4(&magHoldRateFilter, magHoldRate, MAG_HOLD_ERROR_LPF_FREQ, dT);
return magHoldRate; return magHoldRate;

8
src/main/flight/pid.h
View file

@ -23,10 +23,6 @@
#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter #define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter
#define YAW_P_LIMIT_DEFAULT 300 // Default value for yaw P limiter #define YAW_P_LIMIT_DEFAULT 300 // Default value for yaw P limiter
#define MAG_HOLD_RATE_LIMIT_MIN 10
#define MAG_HOLD_RATE_LIMIT_MAX 250
#define MAG_HOLD_RATE_LIMIT_DEFAULT 90
#define FW_ITERM_THROW_LIMIT_DEFAULT 165 #define FW_ITERM_THROW_LIMIT_DEFAULT 165
#define FW_ITERM_THROW_LIMIT_MIN 0 #define FW_ITERM_THROW_LIMIT_MIN 0
#define FW_ITERM_THROW_LIMIT_MAX 500 #define FW_ITERM_THROW_LIMIT_MAX 500
@ -73,9 +69,9 @@ typedef struct pidProfile_s {
int16_t max_angle_inclination[ANGLE_INDEX_COUNT]; // Max possible inclination (roll and pitch axis separately int16_t max_angle_inclination[ANGLE_INDEX_COUNT]; // Max possible inclination (roll and pitch axis separately
uint8_t mag_hold_rate_limit; //Maximum rotation rate MAG_HOLD mode can feed to yaw rate PID controller
float dterm_setpoint_weight; float dterm_setpoint_weight;
#ifdef USE_SERVOS
#ifdef USE_SERVOS
uint16_t fixedWingItermThrowLimit; uint16_t fixedWingItermThrowLimit;
#endif #endif
} pidProfile_t; } pidProfile_t;

28
src/main/io/serial_cli.c
View file

@ -513,6 +513,16 @@ static const clivalue_t valueTable[] = {
{ "accgain_x", VAR_INT16 | MASTER_VALUE, .config.minmax = { 1, 8192 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accGain.raw[X]) }, { "accgain_x", VAR_INT16 | MASTER_VALUE, .config.minmax = { 1, 8192 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accGain.raw[X]) },
{ "accgain_y", VAR_INT16 | MASTER_VALUE, .config.minmax = { 1, 8192 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accGain.raw[Y]) }, { "accgain_y", VAR_INT16 | MASTER_VALUE, .config.minmax = { 1, 8192 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accGain.raw[Y]) },
{ "accgain_z", VAR_INT16 | MASTER_VALUE, .config.minmax = { 1, 8192 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accGain.raw[Z]) }, { "accgain_z", VAR_INT16 | MASTER_VALUE, .config.minmax = { 1, 8192 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accGain.raw[Z]) },
#ifdef MAG
{ "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_align) },
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_HW_MAG }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_hardware) },
{ "mag_declination", VAR_INT16 | MASTER_VALUE, .config.minmax = { -18000, 18000 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_declination) },
{ "magzero_x", VAR_INT16 | MASTER_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, magZero.raw[X]) },
{ "magzero_y", VAR_INT16 | MASTER_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, magZero.raw[Y]) },
{ "magzero_z", VAR_INT16 | MASTER_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, magZero.raw[Z]) },
{ "mag_hold_rate_limit", VAR_UINT8 | MASTER_VALUE, .config.minmax = { MAG_HOLD_RATE_LIMIT_MIN, MAG_HOLD_RATE_LIMIT_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_hold_rate_limit) },
#endif
}; };
#else #else
@ -689,8 +699,6 @@ const clivalue_t valueTable[] = {
{ "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } }, { "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } },
{ "current_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->currentMeterType, .config.lookup = { TABLE_CURRENT_SENSOR } }, { "current_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->currentMeterType, .config.lookup = { TABLE_CURRENT_SENSOR } },
{ "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &compassConfig()->mag_align, .config.lookup = { TABLE_ALIGNMENT } },
{ "align_board_roll", VAR_INT16 | MASTER_VALUE, &boardAlignment()->rollDeciDegrees, .config.minmax = { -1800, 3600 } }, { "align_board_roll", VAR_INT16 | MASTER_VALUE, &boardAlignment()->rollDeciDegrees, .config.minmax = { -1800, 3600 } },
{ "align_board_pitch", VAR_INT16 | MASTER_VALUE, &boardAlignment()->pitchDeciDegrees, .config.minmax = { -1800, 3600 } }, { "align_board_pitch", VAR_INT16 | MASTER_VALUE, &boardAlignment()->pitchDeciDegrees, .config.minmax = { -1800, 3600 } },
{ "align_board_yaw", VAR_INT16 | MASTER_VALUE, &boardAlignment()->yawDeciDegrees, .config.minmax = { -1800, 3600 } }, { "align_board_yaw", VAR_INT16 | MASTER_VALUE, &boardAlignment()->yawDeciDegrees, .config.minmax = { -1800, 3600 } },
@ -763,12 +771,6 @@ const clivalue_t valueTable[] = {
{ "pitot_scale", VAR_FLOAT | MASTER_VALUE, &pitotmeterConfig()->pitot_scale, .config.minmax = { 0, 100 } }, { "pitot_scale", VAR_FLOAT | MASTER_VALUE, &pitotmeterConfig()->pitot_scale, .config.minmax = { 0, 100 } },
#endif #endif
#ifdef MAG
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &compassConfig()->mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &compassConfig()->mag_declination, .config.minmax = { -18000, 18000 } },
{ "mag_hold_rate_limit", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.mag_hold_rate_limit, .config.minmax = { MAG_HOLD_RATE_LIMIT_MIN, MAG_HOLD_RATE_LIMIT_MAX } },
#endif
{ "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], .config.minmax = { 0, 200 }, }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], .config.minmax = { 0, 200 }, },
{ "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], .config.minmax = { 0, 200 }, }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], .config.minmax = { 0, 200 }, },
{ "d_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PITCH], .config.minmax = { 0, 200 }, }, { "d_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PITCH], .config.minmax = { 0, 200 }, },
@ -809,12 +811,6 @@ const clivalue_t valueTable[] = {
{ "blackbox_device", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &blackboxConfig()->device, .config.lookup = { TABLE_BLACKBOX_DEVICE } }, { "blackbox_device", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &blackboxConfig()->device, .config.lookup = { TABLE_BLACKBOX_DEVICE } },
#endif #endif
#ifdef MAG
{ "magzero_x", VAR_INT16 | MASTER_VALUE, &compassConfig()->magZero.raw[X], .config.minmax = { -32768, 32767 } },
{ "magzero_y", VAR_INT16 | MASTER_VALUE, &compassConfig()->magZero.raw[Y], .config.minmax = { -32768, 32767 } },
{ "magzero_z", VAR_INT16 | MASTER_VALUE, &compassConfig()->magZero.raw[Z], .config.minmax = { -32768, 32767 } },
#endif
#ifdef LED_STRIP #ifdef LED_STRIP
{ "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &ledStripConfig()->ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } }, { "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &ledStripConfig()->ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } },
#endif #endif
@ -1066,17 +1062,20 @@ static void dumpPgValues(uint16_t valueSection, uint8_t dumpMask, pgn_t pgn, voi
static gyroConfig_t gyroConfigCopy; static gyroConfig_t gyroConfigCopy;
static accelerometerConfig_t accelerometerConfigCopy; static accelerometerConfig_t accelerometerConfigCopy;
static compassConfig_t compassConfigCopy;
static void backupConfigs(void) static void backupConfigs(void)
{ {
// make copies of configs to do differencing // make copies of configs to do differencing
gyroConfigCopy = *gyroConfig(); gyroConfigCopy = *gyroConfig();
accelerometerConfigCopy = *accelerometerConfig(); accelerometerConfigCopy = *accelerometerConfig();
compassConfigCopy = *compassConfig();
} }
static void restoreConfigs(void) static void restoreConfigs(void)
{ {
*gyroConfig() = gyroConfigCopy; *gyroConfig() = gyroConfigCopy;
*accelerometerConfig() = accelerometerConfigCopy; *accelerometerConfig() = accelerometerConfigCopy;
*compassConfig() = compassConfigCopy;
} }
#endif #endif
@ -1110,6 +1109,7 @@ static void dumpValues(uint16_t valueSection, uint8_t dumpMask, const master_t *
// gyroConfig() has been set to default, gyroConfigCopy contains current value // gyroConfig() has been set to default, gyroConfigCopy contains current value
dumpPgValues(MASTER_VALUE, dumpMask, PG_GYRO_CONFIG, &gyroConfigCopy, gyroConfig()); dumpPgValues(MASTER_VALUE, dumpMask, PG_GYRO_CONFIG, &gyroConfigCopy, gyroConfig());
dumpPgValues(MASTER_VALUE, dumpMask, PG_ACCELEROMETER_CONFIG, &accelerometerConfigCopy, accelerometerConfig()); dumpPgValues(MASTER_VALUE, dumpMask, PG_ACCELEROMETER_CONFIG, &accelerometerConfigCopy, accelerometerConfig());
dumpPgValues(MASTER_VALUE, dumpMask, PG_COMPASS_CONFIG, &compassConfigCopy, compassConfig());
return; return;
} }
#endif #endif

51
src/main/sensors/compass.c
View file

@ -25,6 +25,9 @@
#include "common/axis.h" #include "common/axis.h"
#include "common/maths.h" #include "common/maths.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/compass.h" #include "drivers/compass.h"
#include "drivers/compass_ak8963.h" #include "drivers/compass_ak8963.h"
#include "drivers/compass_ak8975.h" #include "drivers/compass_ak8975.h"
@ -52,6 +55,20 @@
mag_t mag; // mag access functions mag_t mag; // mag access functions
PG_REGISTER_WITH_RESET_TEMPLATE(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 0);
#ifdef MAG
#define MAG_HARDWARE_DEFAULT MAG_AUTODETECT
#else
#define MAG_HARDWARE_DEFAULT MAG_NONE
#endif
PG_RESET_TEMPLATE(compassConfig_t, compassConfig,
.mag_align = ALIGN_DEFAULT,
.mag_hardware = MAG_HARDWARE_DEFAULT,
.mag_declination = 0,
.mag_hold_rate_limit = MAG_HOLD_RATE_LIMIT_DEFAULT
);
#ifdef MAG #ifdef MAG
static int16_t magADCRaw[XYZ_AXIS_COUNT]; static int16_t magADCRaw[XYZ_AXIS_COUNT];
@ -213,39 +230,49 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
return true; return true;
} }
bool compassInit(const compassConfig_t *compassConfig) bool compassInit(void)
{ {
if (!compassDetect(&mag.dev, compassConfig()->mag_hardware)) {
return false;
}
// initialize and calibration. turn on led during mag calibration (calibration routine blinks it) // initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
LED1_ON; LED1_ON;
const bool ret = mag.dev.init(); const bool ret = mag.dev.init();
LED1_OFF; LED1_OFF;
if (ret) { if (ret) {
const int deg = compassConfig->mag_declination / 100; const int deg = compassConfig()->mag_declination / 100;
const int min = compassConfig->mag_declination % 100; const int min = compassConfig()->mag_declination % 100;
mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
magInit = 1; magInit = 1;
} else {
addBootlogEvent2(BOOT_EVENT_MAG_INIT_FAILED, BOOT_EVENT_FLAGS_ERROR);
sensorsClear(SENSOR_MAG);
} }
if (compassConfig()->mag_align != ALIGN_DEFAULT) {
mag.dev.magAlign = compassConfig()->mag_align;
}
return ret; return ret;
} }
bool isCompassHealthy(void) bool compassIsHealthy(void)
{ {
return (mag.magADC[X] != 0) || (mag.magADC[Y] != 0) || (mag.magADC[Z] != 0); return (mag.magADC[X] != 0) || (mag.magADC[Y] != 0) || (mag.magADC[Z] != 0);
} }
bool isCompassReady(void) bool compassIsReady(void)
{ {
return magUpdatedAtLeastOnce; return magUpdatedAtLeastOnce;
} }
void compassUpdate(timeUs_t currentTimeUs, flightDynamicsTrims_t *magZero) void compassUpdate(timeUs_t currentTimeUs)
{ {
static sensorCalibrationState_t calState; static sensorCalibrationState_t calState;
static timeUs_t calStartedAt = 0; static timeUs_t calStartedAt = 0;
static int16_t magPrev[XYZ_AXIS_COUNT]; static int16_t magPrev[XYZ_AXIS_COUNT];
// Check magZero // Check magZero
if ((magZero->raw[X] == 0) && (magZero->raw[Y] == 0) && (magZero->raw[Z] == 0)) { if ((compassConfig()->magZero.raw[X] == 0) && (compassConfig()->magZero.raw[Y] == 0) && (compassConfig()->magZero.raw[Z] == 0)) {
DISABLE_STATE(COMPASS_CALIBRATED); DISABLE_STATE(COMPASS_CALIBRATED);
} }
else { else {
@ -267,7 +294,7 @@ void compassUpdate(timeUs_t currentTimeUs, flightDynamicsTrims_t *magZero)
calStartedAt = currentTimeUs; calStartedAt = currentTimeUs;
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
magZero->raw[axis] = 0; compassConfig()->magZero.raw[axis] = 0;
magPrev[axis] = 0; magPrev[axis] = 0;
} }
@ -276,9 +303,9 @@ void compassUpdate(timeUs_t currentTimeUs, flightDynamicsTrims_t *magZero)
} }
if (magInit) { // we apply offset only once mag calibration is done if (magInit) { // we apply offset only once mag calibration is done
mag.magADC[X] -= magZero->raw[X]; mag.magADC[X] -= compassConfig()->magZero.raw[X];
mag.magADC[Y] -= magZero->raw[Y]; mag.magADC[Y] -= compassConfig()->magZero.raw[Y];
mag.magADC[Z] -= magZero->raw[Z]; mag.magADC[Z] -= compassConfig()->magZero.raw[Z];
} }
if (calStartedAt != 0) { if (calStartedAt != 0) {
@ -306,7 +333,7 @@ void compassUpdate(timeUs_t currentTimeUs, flightDynamicsTrims_t *magZero)
sensorCalibrationSolveForOffset(&calState, magZerof); sensorCalibrationSolveForOffset(&calState, magZerof);
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
magZero->raw[axis] = lrintf(magZerof[axis]); compassConfig()->magZero.raw[axis] = lrintf(magZerof[axis]);
} }
calStartedAt = 0; calStartedAt = 0;

18
src/main/sensors/compass.h
View file

@ -19,6 +19,8 @@
#include "common/time.h" #include "common/time.h"
#include "config/parameter_group.h"
#include "drivers/compass.h" #include "drivers/compass.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
@ -37,6 +39,10 @@ typedef enum {
MAG_MAX = MAG_FAKE MAG_MAX = MAG_FAKE
} magSensor_e; } magSensor_e;
#define MAG_HOLD_RATE_LIMIT_MIN 10
#define MAG_HOLD_RATE_LIMIT_MAX 250
#define MAG_HOLD_RATE_LIMIT_DEFAULT 90
typedef struct mag_s { typedef struct mag_s {
magDev_t dev; magDev_t dev;
float magneticDeclination; float magneticDeclination;
@ -51,11 +57,13 @@ typedef struct compassConfig_s {
sensor_align_e mag_align; // mag alignment sensor_align_e mag_align; // mag alignment
uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device
flightDynamicsTrims_t magZero; flightDynamicsTrims_t magZero;
uint8_t mag_hold_rate_limit; //Maximum rotation rate MAG_HOLD mode can feed to yaw rate PID controller
} compassConfig_t; } compassConfig_t;
PG_DECLARE(compassConfig_t, compassConfig);
bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse); bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse);
bool compassInit(const compassConfig_t *compassConfig); bool compassInit(void);
union flightDynamicsTrims_u; void compassUpdate(timeUs_t currentTimeUs);
void compassUpdate(timeUs_t currentTimeUs, union flightDynamicsTrims_u *magZero); bool compassIsReady(void);
bool isCompassReady(void); bool compassIsHealthy(void);
bool isCompassHealthy(void);

2
src/main/sensors/diagnostics.c
View file

@ -63,7 +63,7 @@ hardwareSensorStatus_e getHwCompassStatus(void)
{ {
#if defined(MAG) #if defined(MAG)
if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
if (isCompassHealthy()) { if (compassIsHealthy()) {
return HW_SENSOR_OK; return HW_SENSOR_OK;
} }
else { else {

19
src/main/sensors/initialisation.c
View file

@ -43,7 +43,6 @@ uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE,
bool sensorsAutodetect( bool sensorsAutodetect(
compassConfig_t *compassConfig,
barometerConfig_t *baroConfig, barometerConfig_t *baroConfig,
pitotmeterConfig_t *pitotConfig) pitotmeterConfig_t *pitotConfig)
{ {
@ -76,15 +75,7 @@ bool sensorsAutodetect(
// FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
#ifdef MAG #ifdef MAG
if (compassDetect(&mag.dev, compassConfig->mag_hardware)) { compassInit();
// calculate magnetic declination
if (!compassInit(compassConfig)) {
addBootlogEvent2(BOOT_EVENT_MAG_INIT_FAILED, BOOT_EVENT_FLAGS_ERROR);
sensorsClear(SENSOR_MAG);
}
}
#else
UNUSED(compassConfig);
#endif #endif
#ifdef SONAR #ifdef SONAR
@ -92,10 +83,6 @@ bool sensorsAutodetect(
rangefinderInit(rangefinderType); rangefinderInit(rangefinderType);
#endif #endif
if (compassConfig->mag_align != ALIGN_DEFAULT) {
mag.dev.magAlign = compassConfig->mag_align;
}
if (accelerometerConfig()->acc_hardware == ACC_AUTODETECT) { if (accelerometerConfig()->acc_hardware == ACC_AUTODETECT) {
accelerometerConfig()->acc_hardware = detectedSensors[SENSOR_INDEX_ACC]; accelerometerConfig()->acc_hardware = detectedSensors[SENSOR_INDEX_ACC];
eepromUpdatePending = true; eepromUpdatePending = true;
@ -106,8 +93,8 @@ bool sensorsAutodetect(
eepromUpdatePending = true; eepromUpdatePending = true;
} }
if (compassConfig->mag_hardware == MAG_AUTODETECT) { if (compassConfig()->mag_hardware == MAG_AUTODETECT) {
compassConfig->mag_hardware = detectedSensors[SENSOR_INDEX_MAG]; compassConfig()->mag_hardware = detectedSensors[SENSOR_INDEX_MAG];
eepromUpdatePending = true; eepromUpdatePending = true;
} }

1
src/main/sensors/initialisation.h
View file

@ -18,6 +18,5 @@
#pragma once #pragma once
bool sensorsAutodetect( bool sensorsAutodetect(
compassConfig_t *compassConfig,
barometerConfig_t *baroConfig, barometerConfig_t *baroConfig,
pitotmeterConfig_t *pitotConfig); pitotmeterConfig_t *pitotConfig);