Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 08:45:31 +03:00
Code Issues Wiki Activity

Rename MAG mode to HEADING LOCK mode; Allow to operate without compass

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-03-19 16:07:54 +10:00
parent 4f4bdbb976
commit 0e4833f41e
17 changed files with 88 additions and 93 deletions
Download patch file Download diff file Expand all files Collapse all files

2
docs/Cli.md
View file

@ -322,7 +322,7 @@ Re-apply any new defaults as desired.
| mode_range_logic_operator | OR | Control how Mode selection works in flight modes. If you example have Angle mode configured on two different Aux channels, this controls if you need both activated ( AND ) or if you only need one activated ( OR ) to active angle mode. |
| default_rate_profile | 0 | Default = profile number |
| mag_declination | 0 | Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl is turned on and you aquirre valid GPS fix. |
| mag_hold_rate_limit | 90 | This setting limits yaw rotation rate that MAG_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when MAG_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. |
| heading_hold_rate_limit | 90 | This setting limits yaw rotation rate that MAG_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when MAG_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. |
| `mag_calibration_time` | 30 | Adjust how long time the Calibration of mag will last. |
| `mc_p_pitch` | 40 | Multicopter rate stabilisation P-gain for PITCH               |
| `mc_i_pitch` | 30 | Multicopter rate stabilisation I-gain for PITCH |

4
src/main/fc/cli.c
View file

@ -544,7 +544,6 @@ static const clivalue_t valueTable[] = {
{ "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) },
{ "mag_calibration_time", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 30, 120 }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, magCalibrationTimeLimit) },
#endif
@ -753,6 +752,9 @@ static const clivalue_t valueTable[] = {
{ "rate_accel_limit_roll_pitch",VAR_UINT32 | PROFILE_VALUE | MODE_MAX, .config.max = { 500000 }, PG_PID_PROFILE, offsetof(pidProfile_t, axisAccelerationLimitRollPitch) },
{ "rate_accel_limit_yaw", VAR_UINT32 | PROFILE_VALUE | MODE_MAX, .config.max = { 500000 }, PG_PID_PROFILE, offsetof(pidProfile_t, axisAccelerationLimitYaw) },
{ "heading_hold_rate_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { HEADING_HOLD_RATE_LIMIT_MIN, HEADING_HOLD_RATE_LIMIT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, heading_hold_rate_limit) },
#ifdef NAV
{ "nav_mc_pos_z_p", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_POS_Z].P) },
{ "nav_mc_pos_z_i", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_POS_Z].I) },

19
src/main/fc/fc_core.c
View file

@ -261,7 +261,7 @@ void mwArm(void)
ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
resetMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
@ -421,16 +421,19 @@ void processRx(timeUs_t currentTimeUs)
}
#endif
#if defined(MAG)
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
if (IS_RC_MODE_ACTIVE(BOXMAG)) {
if (!FLIGHT_MODE(MAG_MODE)) {
resetMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
ENABLE_FLIGHT_MODE(MAG_MODE);
if (sensors(SENSOR_ACC)) {
if (IS_RC_MODE_ACTIVE(BOXHEADINGHOLD)) {
if (!FLIGHT_MODE(HEADING_MODE)) {
resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
ENABLE_FLIGHT_MODE(HEADING_MODE);
}
} else {
DISABLE_FLIGHT_MODE(MAG_MODE);
DISABLE_FLIGHT_MODE(HEADING_MODE);
}
}
#if defined(MAG)
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) {
if (!FLIGHT_MODE(HEADFREE_MODE)) {
ENABLE_FLIGHT_MODE(HEADFREE_MODE);

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

@ -115,7 +115,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXANGLE, "ANGLE;", 1 },
{ BOXHORIZON, "HORIZON;", 2 },
{ BOXNAVALTHOLD, "NAV ALTHOLD;", 3 }, // old BARO
{ BOXMAG, "MAG;", 5 },
{ BOXHEADINGHOLD, "HEADING HOLD;", 5 },
{ BOXHEADFREE, "HEADFREE;", 6 },
{ BOXHEADADJ, "HEADADJ;", 7 },
{ BOXCAMSTAB, "CAMSTAB;", 8 },
@ -261,8 +261,9 @@ static void initActiveBoxIds(void)
activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
activeBoxIds[activeBoxIdCount++] = BOXHEADINGHOLD;
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
activeBoxIds[activeBoxIdCount++] = BOXMAG;
activeBoxIds[activeBoxIdCount++] = BOXHEADFREE;
activeBoxIds[activeBoxIdCount++] = BOXHEADADJ;
}
@ -333,7 +334,7 @@ static uint32_t packFlightModeFlags(void)
// It would be preferable to setting the enabled bits based on BOXINDEX.
const uint32_t tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE |
IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON |
IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG |
IS_ENABLED(FLIGHT_MODE(HEADING_MODE)) << BOXHEADINGHOLD |
IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB |
@ -806,7 +807,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU8(dst, NAV_Status.activeWpNumber);
sbufWriteU8(dst, NAV_Status.error);
//sbufWriteU16(dst, (int16_t)(target_bearing/100));
sbufWriteU16(dst, getMagHoldHeading());
sbufWriteU16(dst, getHeadingHoldTarget());
break;
#endif
@ -1116,13 +1117,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
#endif
#ifdef MAG
sbufWriteU8(dst, compassConfig()->mag_hold_rate_limit);
sbufWriteU8(dst, MAG_HOLD_ERROR_LPF_FREQ);
#else
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
#endif
sbufWriteU8(dst, pidProfile()->heading_hold_rate_limit);
sbufWriteU8(dst, HEADING_HOLD_ERROR_LPF_FREQ);
sbufWriteU16(dst, mixerConfig()->yaw_jump_prevention_limit);
sbufWriteU8(dst, gyroConfig()->gyro_lpf);
sbufWriteU8(dst, pidProfile()->acc_soft_lpf_hz);
@ -1304,7 +1300,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_HEAD:
updateMagHoldHeading(sbufReadU16(src));
updateHeadingHoldTarget(sbufReadU16(src));
break;
case MSP_SET_RAW_RC:
@ -1593,13 +1589,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
sbufReadU16(src);
sbufReadU16(src);
#endif
#ifdef MAG
compassConfigMutable()->mag_hold_rate_limit = sbufReadU8(src);
sbufReadU8(src); //MAG_HOLD_ERROR_LPF_FREQ
#else
sbufReadU8(src);
sbufReadU8(src);
#endif
pidProfileMutable()->heading_hold_rate_limit = sbufReadU8(src);
sbufReadU8(src); //HEADING_HOLD_ERROR_LPF_FREQ
mixerConfigMutable()->yaw_jump_prevention_limit = sbufReadU16(src);
gyroConfigMutable()->gyro_lpf = sbufReadU8(src);
pidProfileMutable()->acc_soft_lpf_hz = sbufReadU8(src);

2
src/main/fc/rc_controls.h
View file

@ -24,7 +24,7 @@ typedef enum {
BOXANGLE,
BOXHORIZON,
BOXNAVALTHOLD, // old BOXBARO
BOXMAG,
BOXHEADINGHOLD, // old MAG
BOXHEADFREE,
BOXHEADADJ,
BOXCAMSTAB,

2
src/main/fc/runtime_config.h
View file

@ -45,7 +45,7 @@ extern uint16_t armingFlags;
typedef enum {
ANGLE_MODE = (1 << 0),
HORIZON_MODE = (1 << 1),
MAG_MODE = (1 << 2),
HEADING_MODE = (1 << 2),
NAV_ALTHOLD_MODE= (1 << 3), // old BARO
NAV_RTH_MODE = (1 << 4), // old GPS_HOME
NAV_POSHOLD_MODE= (1 << 5), // old GPS_HOLD

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

@ -492,6 +492,9 @@ static void imuCalculateEstimatedAttitude(float dT)
// Re-initialize quaternion from known Roll, Pitch and GPS heading
imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
gpsHeadingInitialized = true;
// Force reset of heading hold target
resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
}
// If we can't use COG and there's MAG available - fallback

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

@ -93,8 +93,8 @@ extern uint8_t motorCount;
extern bool motorLimitReached;
extern float dT;
int16_t magHoldTargetHeading;
static pt1Filter_t magHoldRateFilter;
int16_t headingHoldTarget;
static pt1Filter_t headingHoldRateFilter;
// Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied
static bool pidGainsUpdateRequired = false;
@ -106,7 +106,7 @@ int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_I[FLIGHT_DYNAMICS_INDEX_
static pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 0);
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 1);
PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.bank_mc = {
@ -186,6 +186,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.max_angle_inclination[FD_PITCH] = 300, // 30 degrees
.fixedWingItermThrowLimit = FW_ITERM_THROW_LIMIT_DEFAULT,
.pidSumLimit = PID_SUM_LIMIT_DEFAULT,
.heading_hold_rate_limit = HEADING_HOLD_RATE_LIMIT_DEFAULT,
);
void pidInit(void)
@ -525,30 +527,25 @@ static void pidApplyMulticopterRateController(pidState_t *pidState, flight_dynam
#endif
}
void updateMagHoldHeading(int16_t heading)
void updateHeadingHoldTarget(int16_t heading)
{
magHoldTargetHeading = heading;
headingHoldTarget = heading;
}
void resetMagHoldHeading(int16_t heading)
void resetHeadingHoldTarget(int16_t heading)
{
updateMagHoldHeading(heading);
pt1FilterReset(&magHoldRateFilter, 0.0f);
updateHeadingHoldTarget(heading);
pt1FilterReset(&headingHoldRateFilter, 0.0f);
}
int16_t getMagHoldHeading() {
return magHoldTargetHeading;
int16_t getHeadingHoldTarget() {
return headingHoldTarget;
}
uint8_t getMagHoldState()
static uint8_t getHeadingHoldState()
{
#ifndef MAG
return MAG_HOLD_DISABLED;
#endif
if (!sensors(SENSOR_MAG) || !STATE(SMALL_ANGLE)) {
return MAG_HOLD_DISABLED;
if (!STATE(SMALL_ANGLE)) {
return HEADING_HOLD_DISABLED;
}
#if defined(NAV)
@ -557,28 +554,28 @@ uint8_t getMagHoldState()
if (navHeadingState != NAV_HEADING_CONTROL_NONE) {
// Apply maghold only if heading control is in auto mode
if (navHeadingState == NAV_HEADING_CONTROL_AUTO) {
return MAG_HOLD_ENABLED;
return HEADING_HOLD_ENABLED;
}
}
else
#endif
if (ABS(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) {
return MAG_HOLD_ENABLED;
if (ABS(rcCommand[YAW]) < 15 && FLIGHT_MODE(HEADING_MODE)) {
return HEADING_HOLD_ENABLED;
} else {
return MAG_HOLD_UPDATE_HEADING;
return HEADING_HOLD_UPDATE_HEADING;
}
return MAG_HOLD_UPDATE_HEADING;
return HEADING_HOLD_UPDATE_HEADING;
}
/*
* MAG_HOLD P Controller returns desired rotation rate in dps to be fed to Rate controller
* HEADING_HOLD P Controller returns desired rotation rate in dps to be fed to Rate controller
*/
float pidMagHold(void)
float pidHeadingHold(void)
{
float magHoldRate;
float headingHoldRate;
int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHoldTargetHeading;
int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headingHoldTarget;
/*
* Convert absolute error into relative to current heading
@ -621,11 +618,11 @@ float pidMagHold(void)
New controller for 2deg error requires 2,6dps. 4dps for 3deg and so on up until mag_hold_rate_limit is reached.
*/
magHoldRate = error * pidBank()->pid[PID_HEADING].P / 30;
magHoldRate = constrainf(magHoldRate, -compassConfig()->mag_hold_rate_limit, compassConfig()->mag_hold_rate_limit);
magHoldRate = pt1FilterApply4(&magHoldRateFilter, magHoldRate, MAG_HOLD_ERROR_LPF_FREQ, dT);
headingHoldRate = error * pidBank()->pid[PID_HEADING].P / 30;
headingHoldRate = constrainf(headingHoldRate, -pidProfile()->heading_hold_rate_limit, pidProfile()->heading_hold_rate_limit);
headingHoldRate = pt1FilterApply4(&headingHoldRateFilter, headingHoldRate, HEADING_HOLD_ERROR_LPF_FREQ, dT);
return magHoldRate;
return headingHoldRate;
}
#ifdef USE_FLM_TURN_ASSIST
@ -652,10 +649,10 @@ static void pidTurnAssistant(pidState_t *pidState)
void pidController(void)
{
uint8_t magHoldState = getMagHoldState();
uint8_t headingHoldState = getHeadingHoldState();
if (magHoldState == MAG_HOLD_UPDATE_HEADING) {
updateMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
if (headingHoldState == HEADING_HOLD_UPDATE_HEADING) {
updateHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
}
for (int axis = 0; axis < 3; axis++) {
@ -665,8 +662,8 @@ void pidController(void)
// Step 2: Read target
float rateTarget;
if (axis == FD_YAW && magHoldState == MAG_HOLD_ENABLED) {
rateTarget = pidMagHold();
if (axis == FD_YAW && headingHoldState == HEADING_HOLD_ENABLED) {
rateTarget = pidHeadingHold();
} else {
rateTarget = pidRcCommandToRate(rcCommand[axis], currentControlRateProfile->rates[axis]);
}

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

@ -28,13 +28,17 @@
#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 HEADING_HOLD_RATE_LIMIT_MIN 10
#define HEADING_HOLD_RATE_LIMIT_MAX 250
#define HEADING_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
#define AXIS_ACCEL_MIN_LIMIT 50
#define MAG_HOLD_ERROR_LPF_FREQ 2
#define HEADING_HOLD_ERROR_LPF_FREQ 2
typedef enum {
/* PID MC FW */
@ -74,6 +78,8 @@ typedef struct pidProfile_s {
uint8_t yaw_lpf_hz;
uint16_t yaw_p_limit;
uint8_t heading_hold_rate_limit; // Maximum rotation rate HEADING_HOLD mode can feed to yaw rate PID controller
uint16_t rollPitchItermIgnoreRate; // Experimental threshold for ignoring iterm for pitch and roll on certain rates
uint16_t yawItermIgnoreRate; // Experimental threshold for ignoring iterm for yaw on certain rates
@ -116,11 +122,11 @@ float pidRateToRcCommand(float rateDPS, uint8_t rate);
int16_t pidAngleToRcCommand(float angleDeciDegrees, int16_t maxInclination);
enum {
MAG_HOLD_DISABLED = 0,
MAG_HOLD_UPDATE_HEADING,
MAG_HOLD_ENABLED
HEADING_HOLD_DISABLED = 0,
HEADING_HOLD_UPDATE_HEADING,
HEADING_HOLD_ENABLED
};
void updateMagHoldHeading(int16_t heading);
void resetMagHoldHeading(int16_t heading);
int16_t getMagHoldHeading();
void updateHeadingHoldTarget(int16_t heading);
void resetHeadingHoldTarget(int16_t heading);
int16_t getHeadingHoldTarget();

4
src/main/io/ledstrip.c
View file

@ -440,9 +440,7 @@ static const struct {
uint8_t ledMode;
} flightModeToLed[] = {
{HEADFREE_MODE, LED_MODE_HEADFREE},
#ifdef MAG
{MAG_MODE, LED_MODE_MAG},
#endif
{HEADING_MODE, LED_MODE_MAG},
#ifdef BARO
{NAV_ALTHOLD_MODE, LED_MODE_BARO},
#endif

4
src/main/navigation/navigation_fixedwing.c
View file

@ -300,7 +300,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
// Update magHold heading lock in case pilot is using MAG mode (prevent MAGHOLD to fight navigation)
posControl.desiredState.yaw = wrap_36000(posControl.actualState.yaw + headingError);
updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw));
updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw));
// Add pitch compensation
//posControl.rcAdjustment[PITCH] = -CENTIDEGREES_TO_DECIDEGREES(ABS(rollAdjustment)) * 0.50f;
@ -498,7 +498,7 @@ void calculateFixedWingInitialHoldPosition(t_fp_vector * pos)
void resetFixedWingHeadingController(void)
{
updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw));
updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw));
}
void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)

4
src/main/navigation/navigation_multicopter.c
View file

@ -613,12 +613,12 @@ void calculateMulticopterInitialHoldPosition(t_fp_vector * pos)
void resetMulticopterHeadingController(void)
{
updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw));
updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw));
}
static void applyMulticopterHeadingController(void)
{
updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw));
updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw));
}
void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)

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

@ -56,7 +56,7 @@
mag_t mag; // mag access functions
PG_REGISTER_WITH_RESET_TEMPLATE(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 0);
PG_REGISTER_WITH_RESET_TEMPLATE(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 1);
#ifdef MAG
#define MAG_HARDWARE_DEFAULT MAG_AUTODETECT
@ -67,7 +67,6 @@ 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,
.magCalibrationTimeLimit = 30
);

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

@ -39,10 +39,6 @@ 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;
@ -57,7 +53,7 @@ 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
uint8_t __dummy_1; // Maximum rotation rate MAG_HOLD mode can feed to yaw rate PID controller
uint8_t magCalibrationTimeLimit; // Time for compass calibration (seconds)
} compassConfig_t;

2
src/main/telemetry/ibus.c
View file

@ -291,7 +291,7 @@ static void dispatchMeasurementRequest(ibusAddress_t address) {
if (ARMING_FLAG(ARMED)) status = 1; //Rate
if (FLIGHT_MODE(HORIZON_MODE)) status = 2;
if (FLIGHT_MODE(ANGLE_MODE)) status = 3;
if (FLIGHT_MODE(HEADFREE_MODE) || FLIGHT_MODE(MAG_MODE)) status = 4;
if (FLIGHT_MODE(HEADFREE_MODE) || FLIGHT_MODE(HEADING_MODE)) status = 4;
if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) status = 5;
if (FLIGHT_MODE(NAV_POSHOLD_MODE)) status = 6;
if (FLIGHT_MODE(NAV_RTH_MODE)) status = 7;

2
src/main/telemetry/ltm.c
View file

@ -189,7 +189,7 @@ void ltm_sframe(sbuf_t *dst)
lt_flightmode = 13;
else if (FLIGHT_MODE(NAV_POSHOLD_MODE))
lt_flightmode = 9;
else if (FLIGHT_MODE(HEADFREE_MODE) || FLIGHT_MODE(MAG_MODE))
else if (FLIGHT_MODE(HEADFREE_MODE) || FLIGHT_MODE(HEADING_MODE))
lt_flightmode = 11;
else if (FLIGHT_MODE(NAV_ALTHOLD_MODE))
lt_flightmode = 8;

2
src/main/telemetry/smartport.c
View file

@ -423,7 +423,7 @@ void handleSmartPortTelemetry(void)
if (FLIGHT_MODE(PASSTHRU_MODE))
tmpi += 40;
if (FLIGHT_MODE(MAG_MODE))
if (FLIGHT_MODE(HEADING_MODE))
tmpi += 100;
if (FLIGHT_MODE(NAV_ALTHOLD_MODE))
tmpi += 200;