Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +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/barometer.h"
#include "sensors/battery.h"
#include "sensors/compass.h"
#include "sensors/pitotmeter.h"
#include "telemetry/telemetry.h"
@ -70,7 +69,6 @@
#define boardAlignment(x) (&masterConfig.boardAlignment)
#define imuConfig(x) (&masterConfig.imuConfig)
#define barometerConfig(x) (&masterConfig.barometerConfig)
#define compassConfig(x) (&masterConfig.compassConfig)
#define pitotmeterConfig(x) (&masterConfig.pitotmeterConfig)
#define batteryConfig(x) (&masterConfig.batteryConfig)
#define gpsConfig(x) (&masterConfig.gpsConfig)
@ -144,8 +142,6 @@ typedef struct master_s {
barometerConfig_t barometerConfig;
compassConfig_t compassConfig;
pitotmeterConfig_t pitotmeterConfig;
batteryConfig_t batteryConfig;

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

@ -16,61 +16,61 @@
*/
// FC configuration
#define PG_FAILSAFE_CONFIG 1
#define PG_BOARD_ALIGNMENT 2
#define PG_GIMBAL_CONFIG 3
#define PG_MOTOR_MIXER 4
#define PG_BLACKBOX_CONFIG 5
#define PG_MOTOR_AND_SERVO_CONFIG 6
#define PG_SENSOR_SELECTION_CONFIG 7
#define PG_SENSOR_ALIGNMENT_CONFIG 8
#define PG_SENSOR_TRIMS 9
//#define PG_FAILSAFE_CONFIG 1
//#define PG_BOARD_ALIGNMENT 2
//#define PG_GIMBAL_CONFIG 3
//#define PG_MOTOR_MIXER 4
//#define PG_BLACKBOX_CONFIG 5
//#define PG_MOTOR_AND_SERVO_CONFIG 6
//#define PG_SENSOR_SELECTION_CONFIG 7
//#define PG_SENSOR_ALIGNMENT_CONFIG 8
//#define PG_SENSOR_TRIMS 9
#define PG_GYRO_CONFIG 10
#define PG_BATTERY_CONFIG 11
#define PG_CONTROL_RATE_PROFILES 12
#define PG_SERIAL_CONFIG 13
#define PG_PID_PROFILE 14
#define PG_GTUNE_CONFIG 15
#define PG_ARMING_CONFIG 16
#define PG_TRANSPONDER_CONFIG 17
#define PG_SYSTEM_CONFIG 18
#define PG_FEATURE_CONFIG 19
#define PG_MIXER_CONFIG 20
#define PG_SERVO_MIXER 21
#define PG_IMU_CONFIG 22
#define PG_PROFILE_SELECTION 23
#define PG_RX_CONFIG 24
#define PG_RC_CONTROLS_CONFIG 25
#define PG_MOTOR_3D_CONFIG 26
#define PG_LED_STRIP_CONFIG 27
#define PG_COLOR_CONFIG 28
#define PG_AIRPLANE_ALT_HOLD_CONFIG 29
#define PG_GPS_CONFIG 30
#define PG_TELEMETRY_CONFIG 31
#define PG_FRSKY_TELEMETRY_CONFIG 32
#define PG_HOTT_TELEMETRY_CONFIG 33
#define PG_NAVIGATION_CONFIG 34
//#define PG_BATTERY_CONFIG 11
//#define PG_CONTROL_RATE_PROFILES 12
//#define PG_SERIAL_CONFIG 13
//#define PG_PID_PROFILE 14
//#define PG_GTUNE_CONFIG 15
//#define PG_ARMING_CONFIG 16
//#define PG_TRANSPONDER_CONFIG 17
//#define PG_SYSTEM_CONFIG 18
//#define PG_FEATURE_CONFIG 19
//#define PG_MIXER_CONFIG 20
//#define PG_SERVO_MIXER 21
//#define PG_IMU_CONFIG 22
//#define PG_PROFILE_SELECTION 23
//#define PG_RX_CONFIG 24
//#define PG_RC_CONTROLS_CONFIG 25
//#define PG_MOTOR_3D_CONFIG 26
//#define PG_LED_STRIP_CONFIG 27
//#define PG_COLOR_CONFIG 28
//#define PG_AIRPLANE_ALT_HOLD_CONFIG 29
//#define PG_GPS_CONFIG 30
//#define PG_TELEMETRY_CONFIG 31
//#define PG_FRSKY_TELEMETRY_CONFIG 32
//#define PG_HOTT_TELEMETRY_CONFIG 33
//#define PG_NAVIGATION_CONFIG 34
#define PG_ACCELEROMETER_CONFIG 35
#define PG_RATE_PROFILE_SELECTION 36
#define PG_ADJUSTMENT_PROFILE 37
#define PG_BAROMETER_CONFIG 38
#define PG_THROTTLE_CORRECTION_CONFIG 39
#define PG_COMPASS_CONFIGURATION 40
#define PG_MODE_ACTIVATION_PROFILE 41
#define PG_SERVO_PROFILE 42
#define PG_FAILSAFE_CHANNEL_CONFIG 43
#define PG_CHANNEL_RANGE_CONFIG 44
#define PG_MODE_COLOR_CONFIG 45
#define PG_SPECIAL_COLOR_CONFIG 46
//#define PG_RATE_PROFILE_SELECTION 36
//#define PG_ADJUSTMENT_PROFILE 37
//#define PG_BAROMETER_CONFIG 38
//#define PG_THROTTLE_CORRECTION_CONFIG 39
#define PG_COMPASS_CONFIG 40
//#define PG_MODE_ACTIVATION_PROFILE 41
//#define PG_SERVO_PROFILE 42
//#define PG_FAILSAFE_CHANNEL_CONFIG 43
//#define PG_CHANNEL_RANGE_CONFIG 44
//#define PG_MODE_COLOR_CONFIG 45
//#define PG_SPECIAL_COLOR_CONFIG 46
// Driver configuration
#define PG_DRIVER_PWM_RX_CONFIG 100
#define PG_DRIVER_FLASHCHIP_CONFIG 101
//#define PG_DRIVER_PWM_RX_CONFIG 100
//#define PG_DRIVER_FLASHCHIP_CONFIG 101
// OSD configuration (subject to change)
#define PG_OSD_FONT_CONFIG 2047
#define PG_OSD_VIDEO_CONFIG 2046
#define PG_OSD_ELEMENT_CONFIG 2045
//#define PG_OSD_FONT_CONFIG 2047
//#define PG_OSD_VIDEO_CONFIG 2046
//#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.
#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->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_PITCH] = 300; // 30 degrees
@ -475,18 +474,10 @@ void createDefaultConfig(master_t *config)
resetAccelerometerTrims(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
config->compassConfig.mag_align = ALIGN_DEFAULT;
config->boardAlignment.rollDeciDegrees = 0;
config->boardAlignment.pitchDeciDegrees = 0;
config->boardAlignment.yawDeciDegrees = 0;
#ifdef MAG
config->compassConfig.mag_hardware = MAG_AUTODETECT;
#else
config->compassConfig.mag_hardware = MAG_NONE;
#endif
#ifdef BARO
config->barometerConfig.baro_hardware = BARO_AUTODETECT;
#else
@ -581,8 +572,6 @@ void createDefaultConfig(master_t *config)
// for (int i = 0; i < CHECKBOXITEMS; i++)
// cfg.activate[i] = 0;
config->compassConfig.mag_declination = 0;
config->modeActivationOperator = MODE_OPERATOR_OR; // default is to OR multiple-channel mode activation conditions
resetBarometerConfig(&config->barometerConfig);

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

@ -499,7 +499,6 @@ void init(void)
#endif
if (!sensorsAutodetect(
&masterConfig.compassConfig,
&masterConfig.barometerConfig,
&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);
#endif
#ifdef MAG
sbufWriteU8(dst, currentProfile->pidProfile.mag_hold_rate_limit);
sbufWriteU8(dst, compassConfig()->mag_hold_rate_limit);
sbufWriteU8(dst, MAG_HOLD_ERROR_LPF_FREQ);
#else
sbufWriteU8(dst, 0);
@ -1146,7 +1146,11 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_SENSOR_CONFIG:
sbufWriteU8(dst, accelerometerConfig()->acc_hardware);
sbufWriteU8(dst, barometerConfig()->baro_hardware);
#ifdef MAG
sbufWriteU8(dst, compassConfig()->mag_hardware);
#else
sbufWriteU8(dst, 0);
#endif
sbufWriteU8(dst, pitotmeterConfig()->pitot_hardware);
sbufWriteU8(dst, 0); // rangefinder 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);
sbufReadU8(src);
#ifdef MAG
compassConfig()->mag_declination = sbufReadU16(src) * 10;
#else
sbufReadU16(src);
#endif
batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
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:
gyroConfig()->gyro_align = sbufReadU8(src);
accelerometerConfig()->acc_align = sbufReadU8(src);
#ifdef MAG
compassConfig()->mag_align = sbufReadU8(src);
#else
sbufReadU8(src);
#endif
break;
case MSP_SET_ADVANCED_CONFIG:
@ -1537,7 +1549,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
sbufReadU16(src);
#endif
#ifdef MAG
currentProfile->pidProfile.mag_hold_rate_limit = sbufReadU8(src);
compassConfig()->mag_hold_rate_limit = sbufReadU8(src);
sbufReadU8(src); //MAG_HOLD_ERROR_LPF_FREQ
#else
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)
{
if (sensors(SENSOR_MAG)) {
compassUpdate(currentTimeUs, &compassConfig()->magZero);
compassUpdate(currentTimeUs);
}
}
#endif

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

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

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

@ -45,6 +45,7 @@
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
#include "sensors/compass.h"
typedef struct {
@ -521,7 +522,7 @@ float pidMagHold(const pidProfile_t *pidProfile)
*/
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);
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_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_MIN 0
#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
uint8_t mag_hold_rate_limit; //Maximum rotation rate MAG_HOLD mode can feed to yaw rate PID controller
float dterm_setpoint_weight;
#ifdef USE_SERVOS
#ifdef USE_SERVOS
uint16_t fixedWingItermThrowLimit;
#endif
} 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_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]) },
#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
@ -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 } },
{ "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_pitch", VAR_INT16 | MASTER_VALUE, &boardAlignment()->pitchDeciDegrees, .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 } },
#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 }, },
{ "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 }, },
@ -809,12 +811,6 @@ const clivalue_t valueTable[] = {
{ "blackbox_device", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &blackboxConfig()->device, .config.lookup = { TABLE_BLACKBOX_DEVICE } },
#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
{ "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &ledStripConfig()->ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } },
#endif
@ -1066,17 +1062,20 @@ static void dumpPgValues(uint16_t valueSection, uint8_t dumpMask, pgn_t pgn, voi
static gyroConfig_t gyroConfigCopy;
static accelerometerConfig_t accelerometerConfigCopy;
static compassConfig_t compassConfigCopy;
static void backupConfigs(void)
{
// make copies of configs to do differencing
gyroConfigCopy = *gyroConfig();
accelerometerConfigCopy = *accelerometerConfig();
compassConfigCopy = *compassConfig();
}
static void restoreConfigs(void)
{
*gyroConfig() = gyroConfigCopy;
*accelerometerConfig() = accelerometerConfigCopy;
*compassConfig() = compassConfigCopy;
}
#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
dumpPgValues(MASTER_VALUE, dumpMask, PG_GYRO_CONFIG, &gyroConfigCopy, gyroConfig());
dumpPgValues(MASTER_VALUE, dumpMask, PG_ACCELEROMETER_CONFIG, &accelerometerConfigCopy, accelerometerConfig());
dumpPgValues(MASTER_VALUE, dumpMask, PG_COMPASS_CONFIG, &compassConfigCopy, compassConfig());
return;
}
#endif

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

@ -25,6 +25,9 @@
#include "common/axis.h"
#include "common/maths.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/compass.h"
#include "drivers/compass_ak8963.h"
#include "drivers/compass_ak8975.h"
@ -52,6 +55,20 @@
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
static int16_t magADCRaw[XYZ_AXIS_COUNT];
@ -213,39 +230,49 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
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)
LED1_ON;
const bool ret = mag.dev.init();
LED1_OFF;
if (ret) {
const int deg = compassConfig->mag_declination / 100;
const int min = compassConfig->mag_declination % 100;
const int deg = 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
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;
}
bool isCompassHealthy(void)
bool compassIsHealthy(void)
{
return (mag.magADC[X] != 0) || (mag.magADC[Y] != 0) || (mag.magADC[Z] != 0);
}
bool isCompassReady(void)
bool compassIsReady(void)
{
return magUpdatedAtLeastOnce;
}
void compassUpdate(timeUs_t currentTimeUs, flightDynamicsTrims_t *magZero)
void compassUpdate(timeUs_t currentTimeUs)
{
static sensorCalibrationState_t calState;
static timeUs_t calStartedAt = 0;
static int16_t magPrev[XYZ_AXIS_COUNT];
// 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);
}
else {
@ -267,7 +294,7 @@ void compassUpdate(timeUs_t currentTimeUs, flightDynamicsTrims_t *magZero)
calStartedAt = currentTimeUs;
for (int axis = 0; axis < 3; axis++) {
magZero->raw[axis] = 0;
compassConfig()->magZero.raw[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
mag.magADC[X] -= magZero->raw[X];
mag.magADC[Y] -= magZero->raw[Y];
mag.magADC[Z] -= magZero->raw[Z];
mag.magADC[X] -= compassConfig()->magZero.raw[X];
mag.magADC[Y] -= compassConfig()->magZero.raw[Y];
mag.magADC[Z] -= compassConfig()->magZero.raw[Z];
}
if (calStartedAt != 0) {
@ -306,7 +333,7 @@ void compassUpdate(timeUs_t currentTimeUs, flightDynamicsTrims_t *magZero)
sensorCalibrationSolveForOffset(&calState, magZerof);
for (int axis = 0; axis < 3; axis++) {
magZero->raw[axis] = lrintf(magZerof[axis]);
compassConfig()->magZero.raw[axis] = lrintf(magZerof[axis]);
}
calStartedAt = 0;

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

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

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

@ -63,7 +63,7 @@ hardwareSensorStatus_e getHwCompassStatus(void)
{
#if defined(MAG)
if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
if (isCompassHealthy()) {
if (compassIsHealthy()) {
return HW_SENSOR_OK;
}
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(
compassConfig_t *compassConfig,
barometerConfig_t *baroConfig,
pitotmeterConfig_t *pitotConfig)
{
@ -76,15 +75,7 @@ bool sensorsAutodetect(
// 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.
#ifdef MAG
if (compassDetect(&mag.dev, compassConfig->mag_hardware)) {
// calculate magnetic declination
if (!compassInit(compassConfig)) {
addBootlogEvent2(BOOT_EVENT_MAG_INIT_FAILED, BOOT_EVENT_FLAGS_ERROR);
sensorsClear(SENSOR_MAG);
}
}
#else
UNUSED(compassConfig);
compassInit();
#endif
#ifdef SONAR
@ -92,10 +83,6 @@ bool sensorsAutodetect(
rangefinderInit(rangefinderType);
#endif
if (compassConfig->mag_align != ALIGN_DEFAULT) {
mag.dev.magAlign = compassConfig->mag_align;
}
if (accelerometerConfig()->acc_hardware == ACC_AUTODETECT) {
accelerometerConfig()->acc_hardware = detectedSensors[SENSOR_INDEX_ACC];
eepromUpdatePending = true;
@ -106,8 +93,8 @@ bool sensorsAutodetect(
eepromUpdatePending = true;
}
if (compassConfig->mag_hardware == MAG_AUTODETECT) {
compassConfig->mag_hardware = detectedSensors[SENSOR_INDEX_MAG];
if (compassConfig()->mag_hardware == MAG_AUTODETECT) {
compassConfig()->mag_hardware = detectedSensors[SENSOR_INDEX_MAG];
eepromUpdatePending = true;
}

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

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