Pose history #1737
Pose history #1737
Conversation
| #ifndef POSE_HISTORY_H | ||
| #define POSE_HISTORY_H | ||
|
|
||
| #include "state.h" |
There was a problem hiding this comment.
Should include "math/pprz_algebra_float.h" instead of state.h
|
Nice, that is a good start to test! |
|
Also do you really want Eulers? |
|
Fixed the includes and comments. Would you like it if I also log FloatQuat in my struct timeAndRotation? |
|
Was just wondering... no need to buffer what is not used... |
|
Done! |
| uint32_t closestIndex = 0; | ||
|
|
||
| // Seach for the timestamp closes to the timestamp argument of this function. | ||
| for (index_history = 0; index_history < location_history.ring_size; index_history++) { |
There was a problem hiding this comment.
Would be nicer and more efficient to do a:
uint16_t i = 0;
uint32_t diff_min;
uint32_t diff_cur = abs(timestamp - location_history.ring_data[0].timestamp);
do {
diff_min = diff_cur;
diff_cur = abs(timestamp - location_history.ring_data[++i].timestamp);
} while(diff < diff_min);
struct FloatEulers closest_angels;
EulersCopy(closest_angels, location_history.ring_data[--i].rotation);
This will stop searching as it reaches it minimum. This is true because the ring buffer is ordered by time.
There was a problem hiding this comment.
Note that this function is not thread safe(also your implementation). This should be fixed to make it useful.
There was a problem hiding this comment.
Not sure if your proposed function is better. The ring buffer is indeed ordered by time, but index zero can be way after our timestamp that we want to find... this means that we might increase our diff for a while before we jump back in time and are closer to the point we want to be at.
There was a problem hiding this comment.
You can start index_of_insert +1 and then do a round trip.. Note that it must be thread safe, and this new implementation is easier to make thread safe then your previous.
There was a problem hiding this comment.
@fvantienen you still want something changed here?
|
Roland you should add some mutex's to make it thread safe, but please add some |
|
Could you maybe also add the bodyRates? since I want to use that to de-rotate the image. |
|
@fvantienen these rates? |
|
Yes those 👍 Nice! |
|
|
||
| typedef struct { | ||
| uint32_t timestamp; | ||
| struct FloatEulers rotation; |
There was a problem hiding this comment.
should probably be rather called orientation or eulers
| } | ||
|
|
||
| void float_rates_copy(struct FloatRates *to, struct FloatRates *from); | ||
| void float_rates_copy(struct FloatRates *to, struct FloatRates *from) |
There was a problem hiding this comment.
Those functions already exist:
RATES_COPY and EULERS_COPY. https://github.com/paparazzi/paparazzi/blob/master/sw/airborne/math/pprz_algebra.h
There was a problem hiding this comment.
If you copy full structs of the same type, you should simply use a normal assignment...
|
Ok, now using the style guide (years of java programming ;) ) and fixed the assignment. |
| #include <sys/time.h> | ||
| #include "mcu_periph/sys_time.h" | ||
| #include "state.h" | ||
| #if LINUX |
|
Done |
| <module name="pose_history"> | ||
| <doc> | ||
| <description>Ask this module for the pose the drone had closest to a given timestamp</description> | ||
| <define name="POSE_HISTORY_SIZE" description="Length of the pose buffer"/> |
|
Updated conf! |
A modules that remembers the vehicle state for two seconds (1024 measurements). Searching in the buffer is not very efficient, but good enough for now to test the optical flow.
In #1729 @knmcguire and @flixr had the idea of creating a pose history module... So here it is!
Currently it records at 512hz, and remembers its state for two seconds (1024 measurements). Searching in the buffer is not very efficient, but good enough for now to test the optical flow.