How to log bare DWM1000 positioning data?

All discussions related to the Loco Positioning system
Post Reply
salocinx
Beginner
Posts: 20
Joined: Wed Feb 06, 2019 7:58 pm
Contact:

How to log bare DWM1000 positioning data?

Post by salocinx » Mon Dec 02, 2019 2:48 pm

Hi!

As I understand, the Kalman filter is for sensor fusion to achieve more accurate positioning results (IMU/AHRS+Positioning Data).

I am currently trying to log the DWM1000 positioning data before it gets into the Kalman filter, because I need the "raw" positioning data. For this purpose I introduced a new logging block as follows (estimator_kalman.c):

Code: Select all

LOG_GROUP_START(dwm1000_raw)
  LOG_ADD(LOG_FLOAT, positionX, &coreData.B[0])
  LOG_ADD(LOG_FLOAT, positionY, &coreData.B[1])
  LOG_ADD(LOG_FLOAT, positionZ, &coreData.B[2])
LOG_GROUP_STOP(dwm1000_raw)
This works fine and is displayed as new log block option in the log tab in the PC-Client application. Moreover I extended the kalmanCoreData_t structure (kalman_core.h) with a float array, called B[] where I plan to put the raw DWM1000 positioning data into.

I initialize the B[] array with 0.0 in kalmanCoreInit() function at kalman_core.c. This function is called successfully, tested with my ST-Link/v2 debugger and also with some initial values other 0.0 and it displayed fine in the PC-Client.

However, I tried many different functions to assign the DWM1000 raw positioning values to the B[] array; such as:

https://github.com/bitcraze/crazyflie-f ... man.c#L542
https://github.com/bitcraze/crazyflie-f ... ore.c#L319
https://github.com/bitcraze/crazyflie-f ... tor.c#L159

Unfortunately to no avail. These functions seem not to be called at all; although my LocoPositioning setup works fine, since I get positioning data in the PC-Client's LocoPos tab.

Any hints / ideas at which code line I could grab the raw DMW1000 positioning data?



arnaud
Bitcraze
Posts: 2212
Joined: Tue Feb 06, 2007 12:36 pm

Re: How to log bare DWM1000 positioning data?

Post by arnaud » Tue Dec 03, 2019 9:45 am

The problem here is that there is no such things as "raw DMW1000 positioning data" in the Crazyflie, not in the shape of an X,Y,Z position at least.

In Crazyflie, the UWB positioning system has two modes, Two Way Ranging (TWR) and Time Difference of Arrival (TDoA). Measurements from each modes are pushed directly in the kalman filter (EKF).

In TWR mode, the raw measurements are distance to anchors which, for each anchor, puts the Crazyflie on a sphere around the anchor. The data pushed in the EKF in that case is the distance to a point in space (the anchor): https://github.com/bitcraze/crazyflie-f ... #L366-L395.

In TDoA mode, the raw measurement is the difference of distance between two anchors, this puts the Crazyflie on a paraboloid in space. The data pushed in the EKF is the difference of distance between two point in space: https://github.com/bitcraze/crazyflie-f ... #L398-L459.

For TWR there is an existing mechanism that can stream every ranges measured by the LPS deck back to the PC: https://github.com/bitcraze/crazyflie-f ... ice.c#L223. This function is called by the LPS TWR code each time a range to an anchor is measured.There is a function in the python used to receive the data: https://github.com/bitcraze/crazyflie-l ... ion.py#L99.

Post Reply