-
Notifications
You must be signed in to change notification settings - Fork 1.1k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
If you copy full structs of the same type, you should simply use a normal assignment...
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Done!
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 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
should be #ifdef __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"/> |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
missing value...
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.