diff --git a/mk/source.mk b/mk/source.mk
index 1db5ce3054..51d402a362 100644
--- a/mk/source.mk
+++ b/mk/source.mk
@@ -163,7 +163,6 @@ COMMON_SRC = \
fc/rc_modes.c \
flight/alt_hold.c \
flight/autopilot.c \
- flight/pos_hold.c \
flight/dyn_notch_filter.c \
flight/failsafe.c \
flight/gps_rescue.c \
@@ -174,6 +173,7 @@ COMMON_SRC = \
flight/pid.c \
flight/pid_init.c \
flight/position.c \
+ flight/pos_hold.c \
flight/rpm_filter.c \
flight/servos.c \
flight/servos_tricopter.c \
diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index f38a6b6864..09237b4284 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -50,19 +50,6 @@
#define sinPolyCoef9 2.600054768e-6f // Double: 2.600054767890361277123254766503271638682e-6
#endif
-// original sin_approx code for MCUs without FPU, expecting argument close to -2pi..2pi range
-// float sin_approx(float x)
-// {
-// int32_t xint = x;
-// if (xint < -32 || xint > 32) return 0.0f; // Stop here on error input (5 * 360 Deg)
-// while (x > M_PIf) x -= (2.0f * M_PIf); // always wrap input angle to -PI..PI
-// while (x < -M_PIf) x += (2.0f * M_PIf);
-// if (x > (0.5f * M_PIf)) x = (0.5f * M_PIf) - (x - (0.5f * M_PIf)); // We just pick -90..+90 Degree
-// else if (x < -(0.5f * M_PIf)) x = -(0.5f * M_PIf) - ((0.5f * M_PIf) + x);
-// float x2 = x * x;
-// return x + x * x2 * (sinPolyCoef3 + x2 * (sinPolyCoef5 + x2 * (sinPolyCoef7 + x2 * sinPolyCoef9)));
-// }
-
float sin_approx(float x)
{
// Normalize input to the range -PI to PI
diff --git a/src/main/fc/core.c b/src/main/fc/core.c
index aa5292260b..a93fc699c6 100644
--- a/src/main/fc/core.c
+++ b/src/main/fc/core.c
@@ -1036,10 +1036,10 @@ void processRxModes(timeUs_t currentTimeUs)
#ifdef USE_ALT_HOLD_MODE
// only if armed; can coexist with position hold
if (ARMING_FLAG(ARMED)
- // and either the alt_hold switch is activated, or are in failsafe
- && (IS_RC_MODE_ACTIVE(BOXALTHOLD) || failsafeIsActive())
- // but not in GPS_RESCUE_MODE, ie if failsafe is active, must be in Landing Mode
+ // and not in GPS_RESCUE_MODE, to give it priority over Altitude Hold
&& !FLIGHT_MODE(GPS_RESCUE_MODE)
+ // and either the alt_hold switch is activated, or are in failsafe landing mode
+ && (IS_RC_MODE_ACTIVE(BOXALTHOLD) || failsafeIsActive())
// and we have Acc for self-levelling
&& sensors(SENSOR_ACC)
// and we have altitude data
@@ -1057,15 +1057,15 @@ void processRxModes(timeUs_t currentTimeUs)
#ifdef USE_POS_HOLD_MODE
// only if armed; can coexist with altitude hold
if (ARMING_FLAG(ARMED)
- // and the position hold switch is activate
- && IS_RC_MODE_ACTIVE(BOXPOSHOLD)
- // but not in GPS_RESCUE_MODE, ie if failsafe is active, must be in Landing Mode
+ // and not in GPS_RESCUE_MODE, to give it priority over Position Hold
&& !FLIGHT_MODE(GPS_RESCUE_MODE)
+ // and either the alt_hold switch is activated, or are in failsafe landing mode
+ && (IS_RC_MODE_ACTIVE(BOXPOSHOLD) || failsafeIsActive())
// and we have Acc for self-levelling
&& sensors(SENSOR_ACC)
- // and we have altitude data
- && (sensors(SENSOR_MAG) || allowPosHoldWithoutMag())
// Need Mag unless a BRAVE user tries it without
+ && (sensors(SENSOR_MAG) || allowPosHoldWithoutMag())
+ // and we have altitude data
&& isAltitudeAvailable()
// prevent activation until after takeoff
&& wasThrottleRaised()) {
diff --git a/src/main/flight/autopilot.c b/src/main/flight/autopilot.c
index 1fddd63540..13b5d70b69 100644
--- a/src/main/flight/autopilot.c
+++ b/src/main/flight/autopilot.c
@@ -211,7 +211,7 @@ bool positionControl(float deadband) {
posHold.previousDistanceCm = posHold.distanceCm;
}
}
- const uint8_t startLogger = posHold.justStarted ? 2 : 1;;
+ const uint8_t startLogger = posHold.justStarted ? 2 : 1;
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, startLogger);
// simple (very simple) sanity check
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index ce87c778a4..0de6021f00 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -659,12 +659,15 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
// 0.0 - 10.0, heuristic based on GPS speed and stick state
groundspeedGain = imuCalcGroundspeedGain(dt);
}
+
DEBUG_SET(DEBUG_ATTITUDE, 2, lrintf(groundspeedGain * 100.0f));
+
const float courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
const float imuCourseError = imuCalcCourseErr(courseOverGround);
- cogErr = imuCalcCourseErr(courseOverGround) * groundspeedGain;
-
+ cogErr = imuCourseError * groundspeedGain;
+ // cogErr is greater with larger heading errors and greater speed in straight pitch forward flight
+ // ** ATTEMPT TO DETECT SUCCESSFUL IMU ORIENTATION TO GPS **
static float gpsHeadingTruth = 0;
// groundspeedGain can be 5.0 in clean forward flight, up to 10.0 max
// fabsf(imuCourseError) is 0 when headings are aligned, 1 when 90 degrees error or worse
diff --git a/src/main/osd/osd_warnings.c b/src/main/osd/osd_warnings.c
index 9cd0e71804..60f40c7cf0 100644
--- a/src/main/osd/osd_warnings.c
+++ b/src/main/osd/osd_warnings.c
@@ -251,7 +251,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
#ifdef USE_POS_HOLD_MODE
if (osdWarnGetState(OSD_WARNING_POSHOLD_FAILED) && showPosHoldWarning()) {
- tfp_sprintf(warningText, "POS_HOLD_FAIL");
+ tfp_sprintf(warningText, "POS HOLD FAIL");
*displayAttr = DISPLAYPORT_SEVERITY_WARNING;
*blinking = true;
return;
diff --git a/src/main/pg/alt_hold.c b/src/main/pg/alt_hold.c
index 1f6896808e..9550c2bf09 100644
--- a/src/main/pg/alt_hold.c
+++ b/src/main/pg/alt_hold.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software 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.
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software 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.
+ *
+ * Betaflight 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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 this software.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/main/pg/alt_hold.h b/src/main/pg/alt_hold.h
index 414e3cf7fb..6218dc5d8b 100644
--- a/src/main/pg/alt_hold.h
+++ b/src/main/pg/alt_hold.h
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software 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.
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software 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.
+ *
+ * Betaflight 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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 this software.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/main/pg/autopilot.c b/src/main/pg/autopilot.c
index a7967a4c5f..55e3e468aa 100644
--- a/src/main/pg/autopilot.c
+++ b/src/main/pg/autopilot.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software 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.
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software 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.
+ *
+ * Betaflight 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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 this software.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/main/pg/autopilot.h b/src/main/pg/autopilot.h
index 773f7c1d94..4f8df94e53 100644
--- a/src/main/pg/autopilot.h
+++ b/src/main/pg/autopilot.h
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software 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.
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software 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.
+ *
+ * Betaflight 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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 this software.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
diff --git a/src/main/pg/pos_hold.c b/src/main/pg/pos_hold.c
index 861833f4fe..a4003620d3 100644
--- a/src/main/pg/pos_hold.c
+++ b/src/main/pg/pos_hold.c
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software 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.
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software 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.
+ *
+ * Betaflight 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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 this software.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/
@@ -29,7 +30,7 @@
#include "pos_hold.h"
-PG_REGISTER_WITH_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig, PG_POSHOLD_CONFIG, 1);
+PG_REGISTER_WITH_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig, PG_POSHOLD_CONFIG, 0);
PG_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig,
.pos_hold_without_mag = false, // position hold within this percentage stick deflection
diff --git a/src/main/pg/pos_hold.h b/src/main/pg/pos_hold.h
index 65d49ae25b..9cb9ec7588 100644
--- a/src/main/pg/pos_hold.h
+++ b/src/main/pg/pos_hold.h
@@ -1,19 +1,20 @@
/*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Betaflight.
*
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software 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.
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software 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.
+ *
+ * Betaflight 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.
*
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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 this software.
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
*
* If not, see .
*/