From b45ab68c8a353bf089a37b4a582f5b8b1f8febdc Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Mon, 7 Oct 2024 22:40:31 +1100 Subject: [PATCH] get current gps location --- src/main/flight/pos_hold.c | 33 ++++----------------------------- src/main/flight/pos_hold.h | 3 ++- 2 files changed, 6 insertions(+), 30 deletions(-) diff --git a/src/main/flight/pos_hold.c b/src/main/flight/pos_hold.c index 00cf6295be..32c86b5e64 100644 --- a/src/main/flight/pos_hold.c +++ b/src/main/flight/pos_hold.c @@ -41,7 +41,7 @@ posHoldState_t posHoldState; void posHoldReset(void) { // resetPositionControl(); - need to add this in position_control.c -// altHoldState.targetLocation = getCurrentLocation(); + posHoldState.targetLocation = gpsSol.llh; posHoldState.targetAdjustRate = 0.0f; } @@ -65,44 +65,19 @@ void posHoldProcessTransitions(void) { void posHoldUpdateTargetLocation(void) { - // The user can adjust the target position using Roll and Pitch inputs - // There is a big deadband. - // Rate of change is proportional to stick deadband an a CLI value for the max rate + // The user can adjust the target position by flying around in level mode + // We need to provide a big deadband in rc.c - // code to get roll and pitch adjusters (might not be the best way to go) - const float rcRoll = rcCommand[ROLL]; - const float rcPitch = rcCommand[PITCH]; - - const float lowThreshold = 0.5f * (rxConfig()->midrc + PWM_RANGE_MIN); // halfway between center and min - const float highThreshold = 0.5f * (rxConfig()->midrc + PWM_RANGE_MAX); // halfway between center and max - - float rollAdjust = 0.0f; - float pitchAdjust = 0.0f; - - // if failsafe is active, and we get here, we are in failsafe landing mode, don't use stick adjustments if (!failsafeIsActive()) { - if (rcRoll < lowThreshold) { - rollAdjust = scaleRangef(rcRoll, PWM_RANGE_MIN, lowThreshold, -1.0f, 0.0f); - } else if (rcRoll > highThreshold) { - rollAdjust = scaleRangef(rcRoll, highThreshold, PWM_RANGE_MAX, 0.0f, 1.0f); - } - - if (rcPitch < lowThreshold) { - pitchAdjust = scaleRangef(rcPitch, PWM_RANGE_MIN, lowThreshold, -1.0f, 0.0f); - } else if (rcPitch > highThreshold) { - pitchAdjust = scaleRangef(rcPitch, highThreshold, PWM_RANGE_MAX, 0.0f, 1.0f); - } // most easily... // fly the quad, in angle mode, enabling a deadband via rc.c (?) // while sticks are inside the deadband, // set the target location to the current GPS location each iteration // posHoldState.targetLocation = currentLocation; + posHoldState.targetLocation = gpsSol.llh; } - rollAdjust *= 1.0f; - pitchAdjust *= 1.0f; - } void posHoldUpdate(void) diff --git a/src/main/flight/pos_hold.h b/src/main/flight/pos_hold.h index 7c57465f1c..e56f3f074b 100644 --- a/src/main/flight/pos_hold.h +++ b/src/main/flight/pos_hold.h @@ -21,12 +21,13 @@ #ifdef USE_ALT_HOLD_MODE #include "common/time.h" +#include "gps.h" #define POSHOLD_TASK_RATE_HZ 100 // hz typedef struct { bool isPosHoldActive; - float targetLocation; + gpsLocation_t targetLocation; float targetAdjustRate; } posHoldState_t;