flightplan_nav_fixes

[kadir4172]

Fixes on pulling auto_nav function in every ap mode and 'approaching time'

[kadir4172]

16Hz for 'nav_approaching_from' function , is it correct?

[flixr]

I think it would be easier to only call nav_periodic_task if in nav mode instead of checking the mode in that function again.
@gautierhattenberger any reason to still call this while not in NAV mode?

Btw, moved the ground_alt computation in master already, so it is only recalculated when the reference actually changed.

[flixr]

The whole navigation routines are a bit crude at points (mostly initially done to have something similar to fixedwing).
We really need a proper redesign of the API here...

The semantics of NavApproaching(target, from, t) in fixedwing firmware are different to rotorcraft.

• fixedwing: it will take an estimated t seconds until you are at target
• rotorcraft: you are within ARRIVED_AT_WAYPOINT (3m) for t seconds

In any case It might be nicer to actually get the time you are at the waypoint instead of continuing to try to get the frequency a function is called at right and then counting up...

[gautierhattenberger]

Calling the nav_periodic_task even if not in NAV mode allows to have the carrot correctly displayed in GCS in other modes. I'd like to keep this, since it is very useful while flight testing and switching between nav and other modes. At least you have an idea of what is suppose to happen.

Not sure why the time_at_wp was really needed (looks like I did that but I don't remember why), but the nav_approaching should work the same way it's done in FW nav.

[flixr]

True, that makes sense... added a comment...
But in fixedwing nav_approaching takes the time to wp * velocity to define whether you are - well - approaching the wp (close to it). Rotorcraft has a fixed radius around the waypoint (hardcoded 3m) and you need to be inside that for a bit for it to return TRUE.
So in my eyes it is quite different...

[kadir4172]

Felix im out of town now and cannot check the code ın detail. First of all CARROT time is generated by flight plan generator on default if approaching time is not presented and there is a compile error if CARROT is not defined in airframe .xml.
Secondly I think time_at_waypoint for a rotorcraft must represent the time spent in the arrived waypoint radius. I agree with you about to use a real timer on that issue.
Ultimately, nav periodic task counts up the block time and stage time in every call and it causes some problems when the flight plan has a first stage dependent on stage time. This is happened to me and my rotor craft started to flight plan from second stage directly when I switched to NAV mode, because the stage time for the first stage has already completed when I was not in NAV mode.

[flixr]

Right, previously the time was just ignored and hence CARROT not needed... but it would make more sense to put it in navigation.h instead of autopilot.h.

Really not sure about only calling nav_periodic_task and flightplan when in nav mode...
I think it is actually very useful in a lot of cases... .e.g. stage time is updated when you are in manual and auto_nav will correctly switch from the WaitForGps to the HoldingPoint after the 10seconds. Also exceptions can be used in the flight plan...

[kadir4172]

i propose to remove this part of code. Does it calculate an inner product or something else (i can't understand the math behind it) ? But it seems to me, it will return TRUE even if the rotorcraft is not on the desired wp when it cross over the normal line to the path(from_wp to next_wp). Flight plan may switch to the next wp, when there is a deviation from the path line (e.g. disturbance like wind) if the rotorcraft cross over this normal line? If this part will be removed , i propose to change the name of the function 'nav_approachong_from' since the aim will not be approaching from a waypoint, but it will be holding on a waypoint for a desired time. (e.g. 'nav_wp_successed')

[gautierhattenberger]

You got it correctly, it test if a segment is completed if you cross the normal line to the path at the final point. This is because you don't want to go back to your point while doing a segment or a path, even if you you miss the final point for a few meters. This is especially true for fixedwings, but I think it also applies to rotorcraft. The test is only done if the initial waypoint ID is valid (>0 and <NB_WAYPOINT).

[kadir4172]

BTW, the commit related with calling 'nav_periodic_task' is removed, you are right about the issue..

[gautierhattenberger]

The time_at_waypoint check (e.g. how much to spend at a waypoint before completing it) is really old stuff and I think it is pretty useless now. If you want to spend some time at a given waypoint you should do it with a flight plan command (like in line_p1_p2 in rotorcraft_basic.xml).
There is no reason to have a different behavior between FW and rotorcraft for this:

• proximity test is done by a "time to WP" test (dist = ground speed towards WP * time to WP, where the time is CARROT by default and FP approaching_time value otherwise)
• when calling a single 'go' instruction, only proximity test is use
• when flying over a segment ('go from wp' or 'path'), the test is based on proximity or crossing the normal line at the end of segment (in order to have smooth transitions especially when flying a path)

[kadir4172]

Hi Gautier, but in this way isn't there a risk to never reach a waypoint? For example, rotorcraft crosses the normal line without reaching wp then flightplan jumps to next stage(e.g. stay wp2 until stage_time> ..), and the rotorcraft cannot accomplish to reach the wp in specified stage_time.

[gautierhattenberger]

If it really matters for a specific flight plan to stay for n seconds at the WP (and not only in the stage), I guess it is possible to keep the time_at_waypoint variable (and make it extern) so you can use it in the until condition: the rotorcraft will have to reach exactly the WP before the timer incrementing the time.
But being forced to spend some time close enough of the WP is pretty annoying in the end to make a smooth path.

[flixr]

indentation please

[flixr]

Look like you will now always only wait 1second at the waypoint, why then bother to record the time at the waypoint at all?
Also wondering if it would be better to use sys_time.nb_sec instead of autopilot_flight_time (which can be reset by user) and only counts up if in_flight.

[kadir4172]

Hi Felix,
The original version of the nav_approaching_from function was returning TRUE if;
1)Rotorcraft is in a radius of 3m until time_at_wp counts up to 20(freq of this function is 16Hz , it is approx. 1 second, and i tried to keep this)
2)If there is a previous wp which rotorcraft is coming from, it returns TRUE when rotorcraft crosses the normal line.

My idea was to calculate the time at wp and make it reachable from flight plan to use in exceptions or loops etc with "<stay wp.. " commands . But i missed the point that "stay" commands is not calling the approaching function logically. Do you support to implement a discrete function to calculate time_at_wp which will be called from flight plan?

Proximity test(as Gautier mentioned for the fixed wing -> testing ground_speed*time_to_WP ) is not implemented yet for the rotorcrafts. Should we implement this?

[kadir4172]

sys_time.nb_sec is in directly dependent on sys_tick_handler. i guess autopilot_flight_time only counts up if in_flight, are you sure about using sys_time.nb_sec?

[kadir4172]

Approaching function returns TRUE;
1)When rotorcraft enters the 3m radius of wp
2)When rotorcraft crosses the normal line (for segment flights)

NavCheckWaypointTime function added to call from FP, e.g;
<stay wp="wp1" until="NavCheckWaypointTime(wp1, 5)"/>

[kadir4172]

stay "wp1" until= "NavCheckWaypointTime(..)"

[gautierhattenberger]

If you have a strong gust that makes you leave the proximity circle, the counter restart. Is it what you really want ?
Because if you're really unlucky this may last for a long time...

[kadir4172]

Hi Gautier,
I must take photos from the same coordinates(nearly the same) for a time period without an interrupt, and im planning not to fly with a weather which can drift rotorcraft larger than 3m in horizontal while hovering.

[gautierhattenberger]

I looks good like this, but we should implement the approaching_time correctly in the future anyway