So I did some math:
First the 5 Scenarios
Code: Select all
case |dLon |dLat| expected bearing
-----------------------------------
A | 0 | 3 | 0° --> Copter is North of target
B | 1 | 2 | 26,56 --> Copter is NE of target
C | 2 | 2 | 45 --> Copter is NE of target
D | 3 | 0 | 0 --> Copter is E
E | 0 | -3 | 180 --> Copter is south of target, Copter moves south
Now lets do the equations:
Code: Select all
if (dlat1 != dlat2)
*bearing = 180/PI*(atan2(dLon,dLat));
else
*bearing = 0;
There are different cases for the behavior of atan2
Code: Select all
for atan2:
atan2 = arctan(dLon/dLat) | dLat > 0
arctan(dLon/dLat)+PI | dLat < 0, dLon >= 0
arctan(dLon/dLat)-PI | dLat < 0, dLon < 0
PI/2 = 90° | dLat = 0, dLon > 0
-PI/2= -90° | dLat = 0, dLon < 0
0 = 0° | dLat = 0, dLon = 0
thies lead us to the following results
Code: Select all
A: *bearing = arctan(dLon/dLat) = arctan(0/3) = 0°
B: *bearing = arctan(dLon/dLat) = arctan(1/2) = 26,56°
C: *bearing = arctan(dLon/dLat) = arctan(2/2) = 45°
D: *bearing = PI/2 = 90° <-- Lat1 == Lat2: *bearing = 0;
E: *bearing = arctan(dLon/dLat)+PI= 0°+180° = 180°
You may have integrated the else clause to catch a div by zero I think, but the atan2 function already handles that.
In my opinion the else clause should set the bearing to 90° or -90° as you see above.
it should be:
Code: Select all
*bearing = 180/PI*(atan2(dLon,dLat));
Now lets move on with our results:
Code: Select all
-> GPS_directionToHome =bearing
-> GPS_dir = GPS_directionToHome;
-> radDiff = (GPS_dir-heading) //heading = 0
GPS_angle[ROLL] = constrain(P8[PIDGPS] * sin(radDiff) )
GPS_angle[PITCH] = constrain(P8[PIDGPS] * cos(radDiff) )
A: radDiff = 0° ->GPS_angle[ROLL]= 0 *blabla ,GPS_angle[PITCH] = 1 *blabla
B:
C: radDiff = 45° ->GPS_angle[ROLL]= 0,7 *blabla ,GPS_angle[PITCH] = 0,7 *blabla
D:
E: radDiff = 180° ->GPS_angle[ROLL]= 0 *blabla ,GPS_angle[PITCH] = -1 *blabla
The computed results will be subtracted from the curren roll and pitch situataion, everything seems to be okay.
Code: Select all
errorAngle = constrain(2*rcCommand[axis] - GPS_angle[axis],-500,+500) - angle[axis] + accTrim[axis]; //16 bits is ok here
My conclusion is, that the function is okay from the amth side, however, some pid tuning is neccesary, and you need to set your sensors correct.
Most important is a working glitchles mag as Alex stated.
However: I would change the arrow by include the current bearing of the copter, this looks very cool and will definetily come in handy for a HUD and is more easy to understand.
Code: Select all
*bearing = bearing-180°-copter_bearing
So thats all for now - I need some flying practise to see of it works.
Nils