wayBack library #6675

night-ghost · 2017-07-26T07:46:26Z

I can't get working Copter because I can't find a right place to inject points to RTL state machine, but library itself works fine in testing. Needed changes in code can be found by searching "USE_WAYBACK"

UPD. there should be call to wayback.end() somewhere on disarm

khancyr · 2017-07-26T10:32:50Z

ArduCopter/Copter.h

@@ -125,6 +125,11 @@
 #include "afs_copter.h"
 #endif

+
+#if USE_WAYBACK == ENABLED
+#include "/AP_WayBack/AP_WayBack.h"


you should add AP_Wayback to wscript and make.inc, instead of giving a path to the file !

yes but as I sayd I never build this library into real Copter code so all changes outside AP_Wayback folder are just "proof-of-concept", samples of usage and needs to be corrected

khancyr · 2017-07-26T10:34:56Z

libraries/AP_WayBack/AP_WayBack.cpp

+
+
+bool AP_WayBack::start(){
+    if(points==NULL) {


better use nullptr

magicrub · 2017-07-26T10:36:37Z

ping @squilter

khancyr · 2017-07-26T10:37:31Z

libraries/AP_WayBack/AP_WayBack.cpp

+{
+   float denom,numera,numerb;
+
+   denom  = (p4.y-p3.y) * (p2.x-p1.x) - (p4.x-p3.x) * (p2.y-p1.y);


better declare and instentiate directly !

this gives a fastest code, checked by debugger. Moreover this is a part of working-in-years plugin to PHP so I don't want to change anything for compatibility

fasted code.. on embedded ARM architecture?

and in CortexM4F too :) GCC don't likes to optimize out classes and arrays, so stores data to memory even it can be calculated in registers. I always check assembly listings of my code.

Moreover that Cortex has "composite" commands like vfma which used when compiling such code and not used in case load-mul-store. This code turns to
80a5a7a: ee07 1a10 vmov s14, r1
80a5a7e: 6859 ldr r1, [r3, #4]
80a5a80: 681b ldr r3, [r3, #0]
80a5a82: ee05 3a90 vmov s11, r3
80a5a86: 6843 ldr r3, [r0, #4]
80a5a88: ee08 1a10 vmov s16, r1
80a5a8c: ee05 3a10 vmov s10, r3
80a5a90: 6801 ldr r1, [r0, #0]
80a5a92: 686b ldr r3, [r5, #4]
80a5a94: ee08 1a90 vmov s17, r1
80a5a98: ee06 2a90 vmov s13, r2
80a5a9c: ee07 3a90 vmov s15, r3
80a5aa0: 6829 ldr r1, [r5, #0]
80a5aa2: ee75 5ae6 vsub.f32 s11, s11, s13
80a5aa6: ee35 9a67 vsub.f32 s18, s10, s15
80a5aaa: ee06 1a10 vmov s12, r1
80a5aae: ee38 8a47 vsub.f32 s16, s16, s14
80a5ab2: ee36 6a68 vsub.f32 s12, s12, s17
80a5ab6: ee29 9a25 vmul.f32 s18, s18, s11
80a5aba: ee76 8ae8 vsub.f32 s17, s13, s17
80a5abe: ee35 7a47 vsub.f32 s14, s10, s14
80a5ac2: eea8 9a06 vfma.f32 s18, s16, s12
80a5ac6: 4604 mov r4, r0
80a5ac8: ee28 8a28 vmul.f32 s16, s16, s17
80a5acc: ee77 7ac5 vsub.f32 s15, s15, s10
80a5ad0: eea5 8a87 vfma.f32 s16, s11, s14
80a5ad4: ee67 8aa8 vmul.f32 s17, s15, s17
80a5ad0: eea5 8a87 vfma.f32 s16, s11, s14
80a5ad4: ee67 8aa8 vmul.f32 s17, s15, s17
80a5ad8: eeb0 0a48 vmov.f32 s0, s16
80a5adc: eee6 8a07 vfma.f32 s17, s12, s14

one load per element - and all calculations in registers, almost without any VMOV

But if classes used and results saved to class members then optimization limited to one line

pos_vector = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f);

8027df2: ed9f 6a18 vldr s12, [pc, #96] ; 8027e54
8027df6: ed9d 7a16 vldr s14, [sp, #88] ; 0x58
8027dfa: eddd 7a17 vldr s15, [sp, #92] ; 0x5c
8027dfe: eddd 6a15 vldr s13, [sp, #84] ; 0x54
8027e02: ee27 7a06 vmul.f32 s14, s14, s12
8027e06: ee67 7ac6 vnmul.f32 s15, s15, s12
8027e0a: ee66 6a86 vmul.f32 s13, s13, s12
8027e12: edcd 6a08 vstr s13, [sp, #32]
8027e16: ed8d 7a09 vstr s14, [sp, #36] ; 0x24
8027e1a: edcd 7a0a vstr s15, [sp, #40]

but this line looks much better, sure

Also GCC don't likes to remove class methods even they are "inline" so all cool .is_zero() turns to real calls with creation of temporary variable in stack:
if (delta_pos_xyz.is_zero()) {
80a231a: 4628 mov r0, r5
80a231c: edcd 0a06 vstr s1, [sp, #24]
80a2320: edcd 7a07 vstr s15, [sp, #28]
80a2324: f7b1 f912 bl 805354c <_ZNK7Vector3IfE7is_zeroEv>
80a2328: b938 cbnz r0, 80a233a

and even const function much worse than #defines - we got real function instead of true constant:
float AP_Proximity_LightWareSF40C::distance_min() const
{
return 0.20f;
}
80a3290: ed9f 0a01 vldr s0, [pc, #4] ; 80a3298
80a3294: 4770 bx lr

khancyr · 2017-07-26T10:41:42Z

Well, we will need at good pass to correct the coding style ! and I saw some malloc and goto ...
Could you share a video of this working, and how to use it ? Is it working on SITL, or more work is need ?
@squilter is doing something similare for GSoC maybe you can share you idea and implementation !

night-ghost · 2017-07-26T10:44:44Z

coding style

I optimize programs for size and speed, not for style. Video was some time earlier in already closed issue.

night-ghost · 2017-07-26T10:48:51Z

old video - https://www.youtube.com/watch?v=D5UGfn47i8U
times are on stm32F1 without hard float

magicrub · 2017-07-26T10:51:45Z

libraries/AP_WayBack/AP_WayBack.cpp

+extern const AP_HAL::HAL& hal;
+
+AP_WayBack::Point* AP_WayBack::points;
+uint16_t          AP_WayBack::max_num_points=0; // size of storage


with C+11 all of these class variables are set to zero for you, manually setting them to zero just wastes flash space. Please remove all non-zero init values.

this is the above "code style", and definitely NO waste space because GCC just skips all 0-initialization.

whoops, sorry. I thought these were in the class header. The ones in the header are fine. However, what are these variables doing here? They belong in the class as a member variable. What happens if there are two instances of the class?

These variables are static members because this class is intended to be static by design: we have just only one point-of-start. Creation of another instances can be checked and prohibited if it needs

Creation of another instances can be checked

but only if you know to check for it. Best to be fully-static or a real class. Please see the entire codebase as an example.

Lots of drones are Linux based and don't care about another few KB of RAM. Please fix it so multiple class instances don't wreak havoc. All other classes work that way. We're not asking to be annoying, we want it to always work and to be n00b dev resistant as much as possible where possible.

Lots of drones are Linux based

may be but I use 6g flight controller in 110g to 3kg drones with only 196k of memory, and definitely do this library for own needs too.

Check for multiple class instances will be added, but if there will be variant of hal.scheduler->register_io_process() that accepts AP_HAL::Proc and not only AP_HAL::MemberProc then class could be fully static.

UPD. Fully static is much worse than current state, panic() will be called on 2 instantiate

You're indirectly using it like a singleton by making an assumption it will never be called twice.
Here's how DataFlash handles that check as an intentional singleton. For this type of class I suggest you simply make it like all the other classes so it's flexible.
https://github.com/ArduPilot/ardupilot/blob/master/libraries/DataFlash/DataFlash.h#L58

Sorry but DataFlash & Co are the worst code I've ever seen :)

Again, there will be check for 2nd instance

check for 2nd instantiate added.

magicrub · 2017-07-26T10:58:09Z

libraries/AP_WayBack/AP_WayBack.cpp

+
+
+// move forward on STEPS good points
+uint16_t AP_WayBack::move_forw(uint16_t from, uint16_t steps){


please see the style guide:
http://ardupilot.org/dev/docs/style-guide.html

specifically, functions have { on line below,
all if statements requires { }
space after "if" and before { so it's readable.
space between =

writing code for speed is good, but if its hard to read and is a different style than the rest of the codebase then it becomes unmaintainable and that's far worse to have in your codebase. Therefore, we have set up some guidelines. Nothing personal.

this can be fixed by any code formatter, isn't it? :)

yup! Should be quite easy for you to fix :)

amilcarlucas · 2017-07-26T15:00:35Z

libraries/AP_WayBack/test/test-wayback.ino

+char buffer[SERIAL_BUFSIZE];
+
+
+void getSerialLine(char *cp ){      // получение строки


Comments in russian :)

Yes sure, do you want to delete them all?

PS. Just open github in Chrome

please make comments in English (again, please use the rest of the codebase as an example)

There are only 2 possibilities: either I delete all comments by shell script, or them remains as it is. I do not want to waste my time

squilter · 2017-07-27T14:56:09Z

Ha! I wish I had known earlier.

The loop pruning and simplifying are almost exactly the same as I did it. That's pretty neat that we separately arrived at very similar algorithms.

This will definitely be a good topic for the dev call. I'll make sure to take a closer look at how this works. If you are going be available for the dev call, you should check out #6682 before.

magicrub · 2017-07-27T21:06:45Z

libraries/AP_WayBack/AP_WayBack.h

+    typedef struct POINT {
+        float x;
+        float y;
+//        enum POINT_STATUS status; // this field should be changed to NAN in X coord as boolean flag


points are only 2D? I'm not sure how useful that is in reality

We have already discussed this question before. The world does not have a roof so the return altitude does as it was set.

magicrub · 2017-07-27T21:15:28Z

@night-ghost have you seen #6682 ?

night-ghost · 2017-07-30T18:50:36Z

sure not, but why the question?

magicrub · 2017-07-30T19:33:32Z

but why the question?

because they are solving the same problem. It would not make sense to accept both of these PRs into master so a hard decision need to be made. I suggest @night-ghost and @squilter work together to get a single best solution.

night-ghost · 2017-07-31T04:47:30Z

because they are solving the same problem.

Looked. Probably it is intended to achieve the same goal, but the implementation is fundamentally different: that solution is designed just for those "copters with Linux and a bucket of memory," and a bunch of processor power in the addition. I process an array of points without allocating additional memory and without quadratic complexity like

for (i = 0; i < _prunable_loops.size() - 1;) {
```
   int chain_len = 0;
```
```
   uint32_t j;
```

   for(j = i+1; j < _prunable_loops.size() - 1; j++){

Is there any process timings?

UPD. But the best thing is that that code shows how to inject points into the RTL state machine, so I can now do it myself. So I can help to get rid of a hard decision and close this PR

added Panic on 2nd instantiate

… into wayBack2 Conflicts: libraries/AP_WayBack/AP_WayBack.cpp libraries/AP_WayBack/AP_WayBack.h

squilter · 2017-07-31T20:08:29Z

that solution is designed just for those "copters with Linux and a bucket of memory," and a bunch of processor power in the addition.

Let me go one-by-one

just for those "copters

The initial implementation is a library and a flight mode for copter. The library should work fine for rover as well, and the plan was to implement that after the library has been merged.

with Linux

No, it runs on a pixhawk

and a bucket of memory,"

My implementation uses about 2kb of ram. Yours uses more than 30kb. If you were referring to the std::vector that I am using, that is just temporary, and will get replaced by a small (<1kb) array.

and a bunch of processor power in the addition.

The cleanup algorithms should never use more than about 5% of the CPU time, but I expect that they will actually use more like 1%

night-ghost · 2017-08-01T04:52:24Z

No, it runs on a pixhawk

yeah, with only 100 points

My implementation uses about 2kb of ram. Yours uses more than 30kb.

I define a space for thousands points, not for only 100, so library tries to allocate as much memory as possible. But there is no additional allocations. If it gets 32k it will be place for 4000 points, with 100 points it requires 800 bytes only.

The cleanup algorithms should never use more than about 5% of the CPU time

Your algorithm has a places with quadratic complexity so that places will take more time that 490Hz loop allows or needs to break by time (as I see) and to start to work again from beginning. What it will do in case of boat which has 12 hour journey by the tributaries of the river? The boat can not to fly up higher and return by a straight line, but can to have a lots of working time.

My library checked on real GPS tracks like Moscow-Vladivostok with loops in town, and works for years here .

In any case I don't interested in this PR anymore

wayBack library

cb93af4

khancyr reviewed Jul 26, 2017

View reviewed changes

libraries/AP_WayBack/AP_WayBack.cpp Outdated

bool AP_WayBack::start(){

if(points==NULL) {

Copy link

Contributor

khancyr Jul 26, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

better use nullptr

khancyr reviewed Jul 26, 2017

View reviewed changes

magicrub reviewed Jul 26, 2017

View reviewed changes

amilcarlucas reviewed Jul 26, 2017

View reviewed changes

OXINARF added Copter Library NeedRework DevCallTopic labels Jul 27, 2017

magicrub reviewed Jul 27, 2017

View reviewed changes

night-ghost added 2 commits July 31, 2017 10:13

wayBack library

2c90c02

added Panic on 2nd instantiate

Merge branch 'wayBack2' of git+ssh://github.com/night-ghost/ardupilot…

d0d4476

… into wayBack2 Conflicts: libraries/AP_WayBack/AP_WayBack.cpp libraries/AP_WayBack/AP_WayBack.h

OXINARF removed the DevCallTopic label Aug 1, 2017

night-ghost closed this Aug 1, 2017

night-ghost deleted the wayBack2 branch August 1, 2017 04:53



		// move forward on STEPS good points
		uint16_t AP_WayBack::move_forw(uint16_t from, uint16_t steps){

		char buffer[SERIAL_BUFSIZE];


		void getSerialLine(char *cp ){ // получение строки

wayBack library #6675

wayBack library #6675

Conversation

night-ghost commented Jul 26, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

magicrub commented Jul 26, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

khancyr commented Jul 26, 2017

night-ghost commented Jul 26, 2017

night-ghost commented Jul 26, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

night-ghost Jul 26, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

night-ghost Jul 27, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

squilter commented Jul 27, 2017

Choose a reason for hiding this comment

night-ghost Jul 30, 2017 • edited Loading

Choose a reason for hiding this comment

magicrub commented Jul 27, 2017

night-ghost commented Jul 30, 2017

magicrub commented Jul 30, 2017

night-ghost commented Jul 31, 2017 • edited Loading

squilter commented Jul 31, 2017

night-ghost commented Aug 1, 2017

night-ghost commented Jul 26, 2017 •

edited

Loading

night-ghost commented Jul 26, 2017 •

edited

Loading

night-ghost Jul 26, 2017 •

edited

Loading

night-ghost Jul 27, 2017 •

edited

Loading

night-ghost Jul 30, 2017 •

edited

Loading

night-ghost commented Jul 31, 2017 •

edited

Loading