Ctzsnooze position hold 2

[ledvinap]

(all position-branch commits got reapplied ... force-push is not fun. Please, never do it again )

[ctzsnooze]

Do you mean that by putting lat first in the enum, which matches the gps llh enum order, and the gps_location_t order, I am not following the current Betaflight convention? Even in MSP and OSD we go latitude first.
At this point I don't understand why it's important to put longitude first.
Is that because subsequent vector maths expect longitude first?
If so, we need to change the sequence in gpsSol.llh and gps_location_t, don't we?
Can't changes of this kind be done in a separate PR that provides consistency across the codebase?

[ledvinap]

It's all about left-handed and right-handed coordinate system.

Current state in BS is a mess, usually NEU - that would correspond to lat,lon,alt. But this system is left-handed and almost never used.
It can be changed to NED, which is pretty common, but that would mean that altitude will be negative and decrease with increasing height (Z axis pointing down). I think almost nobody will approve that in BF. And another effect would be Z axis pointing down in body frame, not matching accelerometer etc ..

At this point I don't understand why it's important to put longitude first.

If you want to work with EF position as vector, you have to choose some coordinate system, otherwise some math won't make sense (mainly rotations)

Is that because subsequent vector maths expect longitude first?

Not exactly. Vector math works in any combination, but direction of angles change (angle is from X to Y).

If so, we need to change the sequence in gpsSol.llh and gps_location_t, don't we?

Not necessarily. lat/lon are angles and we do not apply vector math to them. They are converted to some cartesian system first (is this PR GPS_distance2d converts to EN(U)) - you must always subtract lat/lon first = set reference point of conversion. lan/lon represent sphere, but locally, the sphere is flat enough to pretend it is flat/planar/2d

So we should be more careful when converting llh to ENU, but than it is simple XYZ

Can't changes of this kind be done in a separate PR that provides consistency across the codebase?

Definitely. I'm trying to explain why continuing current practise of trying out signs and swapping sin/cos until it works as desired is bad idea.
Poshold/rescue is revealing problems with current codebase. IMO we should start doing it 'right', or at least stop making situation even worse ...

[ctzsnooze]

Where is this to be used?

[ledvinap]

It is possible that this is leftover. This PR is only draft to show ideas, I didn't spend much time polishing it.

[ctzsnooze]

Should the comment indicate the sign?
ie is this correct?
// angle is in radians and positive values rotate counterclockwise

[ctzsnooze]

For pilots, positive yaw stick results in right yaw. But I notice in Betaflight that positive yaw gyro values mean left turn, but positive attitude.values.yaw means a rightwards turn. What we should do here needs to be resolved.

[ledvinap]

This is simple vector rotation, as used in math. https://en.wikipedia.org/wiki/Rotations_and_reflections_in_two_dimensions

In right-handed system, angle is counterclockwise (it is always from X to Y)

For pilots, positive yaw stick results in right yaw. But I notice in Betaflight that positive yaw gyro values mean left turn, but positive attitude.values.yaw means a rightwards turn. What we should do here needs to be resolved.

Well, this is heritage from multiwii or even more ancient times. You want right stick movement to cause right yaw. And RC transmitter convention is that value increases to the right. So someone added * -1 to make it work this way.

Gyro uses normal right-handed coordinate system, Z axis is up and positive rotation is counterclockwise.

I don't think that we can resolve this one. It is here for too long, causing headaches for over a decade. For nowm, we can only document it, put warning in relevant places and live with it.

(one aspect of left-handed NEU system used may be that this isn't too visible in IMU. It may be bigger problem with ENU).

[ctzsnooze]

Karatebrot explicitly requested that I add earthFrame_s as the name here. IDK who to follow.

[ledvinap]

TLDR: Follow @KarateBrot advice. This is just a minor detail, and adding tag is convention in BF

• BF overuses typedefs (https://lkml.org/lkml/2006/2/3/169)
• _s suffix has little usefulness - struct, union and enum use tag namespace. The isn't much space for collisions and compiler checks that you use tag correctly ( struct x { int a; }; union x var; will fail). struct something vs struct something_s
• specifying struct tag is useful only in forward declaration. There are only few forward declarations in BF, so we can tag only few structs where id ids relevant

[ctzsnooze]

These parameters are all earth frame related values, they have nothing to do with pitch or roll.
By re-naming to the word Axis, it isn't clear if the values are for pitch or roll or for earth frame things.
I think earthFrameAxis_t might be better?

[ledvinap]

These parameters are all earth frame related values, they have nothing to do with pitch or roll. By re-naming to the word Axis, it isn't clear if the values are for pitch or roll or for earth frame things. I think earthFrameAxis_t might be better?

The idea was 'axis over which PIDs are applied', like 'graph X axis', not 'axis of rotation'. With PID being important.

You decompose 2d problem into 2 dimensions and apply control over each dimension independently. Actual PID code is almost independent of decomposition used, you just need to compose back into 2d correctly (ordering/direction of X,Y is important during decomposition and merging, not during PID calculation).

Maybe pidDimensionEF_t. pidDimEF_t ? But earthFrameAxis_t is fine

[ctzsnooze]

I ended up using efPidAxis_t for now - let me share it to the main PR once I've better flight tested it.

I do occasionally get a serious altitude control anomaly in gps rescue code (old and new variants) where the quad's altitude control goes crazy and it doesn't stop climbing. That's the only issue I have found, and it is very random, so not easy to identify, especially while focusing on non-altitude stuff.

[ctzsnooze]

And I agree that if this code can sort of be a 'generic' two axis control function, that makes sense. For now while reading the code I do like clear separations in the naming between EF and BF. Am tempted to say BodyF not BF since BF often means Betaflight :-)

[ctzsnooze]

@ledvinap - I've build your suggested changes, and am checking carefully.

It works very well, and I really do appreciate the time taken to propose the vector math solutions. I could never have done that!

I'll push the changes here to the main PR when I finish testing.

One thing I did, out of curiosity and to simplify the code (and perhaps simplify other places also), is as follows. Maybe you'll 'hate' it :-) but it does highlight something with axis naming....

I wanted to directly access gpsSol.llh by index, so openAI suggested this:

typedef union gpsLocation_u {
    struct {
        int32_t lat;                // latitude * 1e+7
        int32_t lon;                // longitude * 1e+7
        int32_t altCm;              // altitude in 0.01m
    };
    int32_t coords[3];              // added to provide direct access within loops
} gpsLocation_t;

It allowed me to use the for loop index to directly get the lat and lon values, like this:

 ap.targetLocation.coords[efAxisIdx] = gpsSol.llh.coords[efAxisIdx];

That avoids using this function:

void setTargetLocationByAxis(const gpsLocation_t* newTargetLocation, axisEF_e efAxisIdx)
{
if (efAxisIdx == LON) {
ap.targetLocation.lon = newTargetLocation->lon; // update East-West / / longitude position
} else {
ap.targetLocation.lat = newTargetLocation->lat; // update North-South / latitude position
}
}

But of course it didn't work as nicely as I'd hoped, because the for loop, after your changes, has lon == 0, and lat == 1, whereas gpsSol.llh uses lat == 0, lon == 1. So the simple direct assignment crossed the axes over, incorrectly.

Inverting the 0 and 1 sets the assignments to the correct axes. I'll put it in the working PR so you can comment there, if I don't hear back from you about it here in the meantime.

It's at least as efficient as calling the external function, and we don't need the 'if (efAxisIdx == LON)check insetTargetLocationByAxis`.

The bigger question is whether we should make an attempt, at this point, to standardise "latitude == 0" consistently across the code base, which might be worth considering. It might help with the more general use of vector operations on latitude and longitude. In the update to get GPS rescue to use this code, I am still using sine and cosine methods.