Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 16:55:36 +03:00
Code Issues Wiki Activity

lenient sanity check, message for noMag, possible DA vector limit code

Browse source
This commit is contained in:
ctzsnooze 2024-10-31 13:37:59 +11:00
parent 2c4d45d716
commit 93e8cb6415
3 changed files with 33 additions and 24 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1062,9 +1062,7 @@ void processRxModes(timeUs_t currentTimeUs)
// 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)
// Need Mag unless a BRAVE user tries it without
&& (sensors(SENSOR_MAG) || allowPosHoldWithoutMag()))
&& sensors(SENSOR_ACC))
{
if (!FLIGHT_MODE(POS_HOLD_MODE)) {
ENABLE_FLIGHT_MODE(POS_HOLD_MODE);

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

@ -30,6 +30,7 @@
#include "fc/runtime_config.h"
#include "flight/imu.h"
#include "flight/pos_hold.h"
#include "flight/position.h"
#include "rx/rx.h"
#include "sensors/gyro.h"
@ -120,6 +121,7 @@ void resetPositionControl(gpsLocation_t initialTargetLocation) { // set only at
posHold.peakInitialGroundspeed = 0.0f;
posHold.direction[NORTH_SOUTH].integral = 0.0f;
posHold.direction[EAST_WEST].integral = 0.0f;
posHold.sanityCheckDistance = gpsSol.groundSpeed > 1000 ? gpsSol.groundSpeed : 1000.0f;
}
void autopilotInit(const autopilotConfig_t *config)
@ -201,23 +203,25 @@ void setTargetLocation(gpsLocation_t newTargetLocation) {
bool positionControl(void) {
// exit if we don't have a GPS 3D fix
if (!STATE(GPS_FIX)) {
return false; // cannot proceed without a GPS location
return false; // cannot proceed; this could happen mid-flight, e.g. early after takeoff without a fix
}
// and if no valid heading from GPS, or no mag
if (!canUseGPSHeading
#ifdef USE_MAG
&& !compassIsHealthy()
#endif
) {
if (
#ifdef USE_MAG
!compassIsHealthy() && // if compass is OK, don't worry about GPS; this is not likely to change in-flight
#endif
(!allowPosHoldWithoutMag() || !canUseGPSHeading) // no compass, check if GPS heading is OK, which changes during the flight
) {
return false;
}
if (!wasThrottleRaised()) {
return false;
}
// note: returning false dispays POS_HOLD_FAIL warning in OSD
if (isNewGPSDataAvailable()) {
posHold.gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.01s to 1.0s
posHold.gpsDataFreqHz = 1.0f / posHold.gpsDataIntervalS;
@ -242,18 +246,14 @@ bool positionControl(void) {
// ** Sanity check **
// primarily to detect flyaway from no Mag or badly oriented Mag
// must accept some overshoot at the start, especially if entering at high speed
if (posHold.direction[NORTH_SOUTH].isStarting || posHold.direction[EAST_WEST].isStarting) {
posHold.sanityCheckDistance = gpsSol.groundSpeed > 1000 ? gpsSol.groundSpeed : 1000.0f;
// larger threshold if faster at start
// 1s of flight at current speed or 10m, in cm
}
if (distanceCm > posHold.sanityCheckDistance) {
return false;
}
earthFrame_t *direction[] = { &posHold.direction[NORTH_SOUTH], &posHold.direction[EAST_WEST]};
// static float prevPidDASquared = 0.0f; // if we limit DA on true vector length
for (int i = 0; i < 2; i++) {
earthFrame_t *latLong = direction[i];
earthFrame_t *latLong = &posHold.direction[i];
// separate PID controllers for latitude (NorthSouth or NS) and longitude (EastWest or EW)
@ -290,6 +290,15 @@ bool positionControl(void) {
const float pidDA = constrainf(pidD + pidA, -maxDAAngle, maxDAAngle);
// note: an angle of more than 35 degrees can still be achieved as P and I grow
/* possible economical alternative method using true length:
float pidDASquared = pidDA * pidDA;
float mag = sqrtf(pidDASquared + prevPidDASquared)
if (mag > maxDAAngle) {
pidDA *= (maxDAAngle / mag);
}
prevPidDASquared = pidDASquared
*/
// ** PID Sum **
float pidSum = pidP + pidI + pidDA;
@ -303,6 +312,7 @@ bool positionControl(void) {
currentTargetLocation.lon = gpsSol.llh.lon;
}
latLong->distance = 0.0f;
posHold.sanityCheckDistance = 1000.0f; // 10m, once stable
latLong->isStarting = false;
}
latLong->pidSum = pidSum;
@ -330,6 +340,7 @@ bool positionControl(void) {
// ** Final output to pid.c Angle Mode at 100Hz with primitive upsampling**
autopilotAngle[AI_ROLL] = pt3FilterApply(&posHold.upsample[AI_ROLL], posHold.pidSum[AI_ROLL]);
autopilotAngle[AI_PITCH] = pt3FilterApply(&posHold.upsample[AI_PITCH], posHold.pidSum[AI_PITCH]);
// note: upsampling should really be done in earth frame, to avoid 10Hz wobbles if pilot yaws and the controller is applying significant pitch or roll
if (gyroConfig()->gyro_filter_debug_axis == FD_ROLL) {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(posHold.pidSum[AI_ROLL] * 10));

10
src/main/osd/osd_elements.c
View file

@ -1047,7 +1047,7 @@ static void osdElementFlymode(osdElementParms_t *element)
// Note that flight mode display has precedence in what to display.
// 1. FS
// 2. GPS RESCUE
// 3. ANGLE, HORIZON, ACRO TRAINER, ALTHOLD
// 3. HEAD, POSHOLD, ALTHOLD, ANGLE, HORIZON, ACRO TRAINER
// 4. AIR
// 5. ACRO
@ -1057,12 +1057,12 @@ static void osdElementFlymode(osdElementParms_t *element)
strcpy(element->buff, "RESC");
} else if (FLIGHT_MODE(HEADFREE_MODE)) {
strcpy(element->buff, "HEAD");
} else if (FLIGHT_MODE(ANGLE_MODE)) {
strcpy(element->buff, "ANGL");
} else if (FLIGHT_MODE(ALT_HOLD_MODE)) {
strcpy(element->buff, "ALTH");
} else if (FLIGHT_MODE(POS_HOLD_MODE)) {
strcpy(element->buff, "POSH");
} else if (FLIGHT_MODE(ALT_HOLD_MODE)) {
strcpy(element->buff, "ALTH");
} else if (FLIGHT_MODE(ANGLE_MODE)) {
strcpy(element->buff, "ANGL");
} else if (FLIGHT_MODE(HORIZON_MODE)) {
strcpy(element->buff, "HOR ");
} else if (IS_RC_MODE_ACTIVE(BOXACROTRAINER)) {