Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00
Code Issues Wiki Activity

get current gps location

Browse source
This commit is contained in:
ctzsnooze 2024-10-07 22:40:31 +11:00
parent 0c8b636ddf
commit b45ab68c8a
2 changed files with 6 additions and 30 deletions
Download patch file Download diff file Expand all files Collapse all files

33
src/main/flight/pos_hold.c
View file

@ -41,7 +41,7 @@ posHoldState_t posHoldState;
void posHoldReset(void) void posHoldReset(void)
{ {
// resetPositionControl(); - need to add this in position_control.c // resetPositionControl(); - need to add this in position_control.c
// altHoldState.targetLocation = getCurrentLocation(); posHoldState.targetLocation = gpsSol.llh;
posHoldState.targetAdjustRate = 0.0f; posHoldState.targetAdjustRate = 0.0f;
} }
@ -65,44 +65,19 @@ void posHoldProcessTransitions(void) {
void posHoldUpdateTargetLocation(void) void posHoldUpdateTargetLocation(void)
{ {
// The user can adjust the target position using Roll and Pitch inputs // The user can adjust the target position by flying around in level mode
// There is a big deadband. // We need to provide a big deadband in rc.c
// Rate of change is proportional to stick deadband an a CLI value for the max rate
// 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 (!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... // most easily...
// fly the quad, in angle mode, enabling a deadband via rc.c (?) // fly the quad, in angle mode, enabling a deadband via rc.c (?)
// while sticks are inside the deadband, // while sticks are inside the deadband,
// set the target location to the current GPS location each iteration // set the target location to the current GPS location each iteration
// posHoldState.targetLocation = currentLocation; // posHoldState.targetLocation = currentLocation;
posHoldState.targetLocation = gpsSol.llh;
} }
rollAdjust *= 1.0f;
pitchAdjust *= 1.0f;
} }
void posHoldUpdate(void) void posHoldUpdate(void)

3
src/main/flight/pos_hold.h
View file

@ -21,12 +21,13 @@
#ifdef USE_ALT_HOLD_MODE #ifdef USE_ALT_HOLD_MODE
#include "common/time.h" #include "common/time.h"
#include "gps.h"
#define POSHOLD_TASK_RATE_HZ 100 // hz #define POSHOLD_TASK_RATE_HZ 100 // hz
typedef struct { typedef struct {
bool isPosHoldActive; bool isPosHoldActive;
float targetLocation; gpsLocation_t targetLocation;
float targetAdjustRate; float targetAdjustRate;
} posHoldState_t; } posHoldState_t;