Issues with the Lighthouse Deck oriented upside down or towards one side

[tmartindev]

Hello,
I am testing different placements of the Lighthouse deck on a custom brushless drone using the Bolt 1.1 deck (2023.02 firmware) + Flow deck v2 to avoid drift when there is no signal detected. I am using Lighthouse base stations v2.

Normally the Lighthouse deck is oriented +Z in the drone frame. However with a ribbon cable, I am able to place the Lighthouse deck upside down (-Z) or on the side of the drone (+Y).

-Z : In this configuration, the base stations are placed on the ground looking up. Calibration is made with the drone placed on a metal bar at about 1.4m supported by 2 tripods (Lighthouse deck upside down). The drone is quite stable with 2 base stations. However, the drone crashes using 1 base station only.

+Y : In this configuration, the base stations are placed normally, mounted on tripods. Calibration is made with the deck oriented +Z. The drone is never stable and crashes every time with 1 or 2 base stations.

I tried to modify the Lighthouse deck position in lines 207 to 213 of the file lighthouse_position_est.c and the normal vector of the deck in line 444. This resulted in the same behavior as with the original firmware : the drone goes fast on one side and crashes.

I don't know if such placement of the Lighthouse deck has been thought of or tested before but I am interested to know if it is possible. I also looked at the file mm_sweep_angles.c but couldn't find something to modify.

Any help is greatly appreciated.

[krichardsson]

Hi!

This is interesting, but not something I have tested. Some of the code will probably work OK, but there is some functionality that definitely will not.

Geometry estimation

I'm not exactly sure how well this will work. My guess is that the relative positions of the base stations will be correct, but that the coordinate system might be wrong. It should be easy to see in the lighthouse tab in the client, or save the config as a file.
The sensor positions are defined in lighthouse_types.py. If you update them I think it might work OK.

Position estimation

The position is estimated in mm_sweep_angles.c, and I think this code will work OK if you update the sensor positions.

Yaw estimation

The yaw estimation is a bit of a hack and is done outside the kalman filter, this will not work at all. The implementation assumes the deck is mounted the standard way.
You can try to remove this and see what happens. Comment out the line estimatorEnqueueYawError(&yawDeltaMeasurement); in lighthouse_position_est.c
This will remove the yaw estimation, and it will perhaps make it possible to fly? Since you do not get any yaw information, the estimator will assume you take off with the Crazyflie facing the positive X direction. You need to place the CF the same way as is required for the Loco system.

It would be interesting to hear how this works out, please keep us posted!

[tmartindev]

Hello,

I'm back with some results ! In each test the sensor position in the firmware is modified to respect the real position of the deck (similar to the position variables in lighthouse_types.py below). I use the takeoff and goto methods from the HighLevelCommander to command the drone.

Oriented -Z

Calibration:
I never had problem calibrating the drone in this situation. Changing the position of the sensors in lighthouse_types.py did not change the estimated position of the base station that much.

"Reversed" lighthouse geometry:

# lighthouse_types.py
positions = np.float32([
  (-_sensor_distance_length / 2, -_sensor_distance_width / 2, 0.0),
  (-_sensor_distance_length / 2, _sensor_distance_width / 2, 0.0),
  (_sensor_distance_length / 2, -_sensor_distance_width / 2, 0.0),
  (_sensor_distance_length / 2, _sensor_distance_width / 2, 0.0)])

geos:
  0:
    origin:
    - 0.2408921718597412
    - -0.7928484082221985
    - 1.402867317199707
    rotation:
    - - 0.014050799421966076
      - -0.9950081706047058
      - -0.09879935532808304
    - - 0.013117480091750622
      - -0.09861717373132706
      - 0.9950389862060547
    - - -0.9998152256011963
      - -0.015277092345058918
      - 0.01166634913533926
  1:
    origin:
    - 1.2731643915176392
    - -0.7632980942726135
    - 1.4207348823547363
    rotation:
    - - -0.16957174241542816
      - 0.9855102896690369
      - -0.0038656406104564667
    - - 0.0021334378980100155
      - -0.0035553525667637587
      - -0.9999914169311523
    - - -0.9855155348777771
      - -0.16957852244377136
      - -0.0014996376121416688

"Normal" lighthouse geometry:

# lighthouse_types.py
positions = np.float32([
  (-_sensor_distance_length / 2, _sensor_distance_width / 2, 0.0),
  (-_sensor_distance_length / 2, -_sensor_distance_width / 2, 0.0),
  (_sensor_distance_length / 2, _sensor_distance_width / 2, 0.0),
  (_sensor_distance_length / 2, -_sensor_distance_width / 2, 0.0)])

geos:
  0:
    origin:
    - 0.13189394772052765
    - -0.3556433320045471
    - 1.417639970779419
    rotation:
    - - 0.01106199249625206
      - -0.9980206489562988
      - -0.0619065947830677
    - - -0.010305184870958328
      - -0.06202087923884392
      - 0.99802166223526
    - - -0.9998857378959656
      - -0.010402149520814419
      - -0.01097086165100336
  1:
    origin:
    - 1.1564503908157349
    - -0.3651542067527771
    - 1.4368541240692139
    rotation:
    - - -0.16193869709968567
      - 0.9863114953041077
      - -0.03107239305973053
    - - -0.015395857393741608
      - -0.034009456634521484
      - -0.9993029236793518
    - - -0.9866806864738464
      - -0.161347433924675
      - 0.020692558959126472

(calibs data are exactly the same)

I forgot to mention it in my initial message, but as you can see, the position estimated by the cfclient is as if the deck was in its normal configuration. So they appears in altitude looking down rather than on the ground looking up. Then, I rotate the rotation matrix of 180° around the X-axis and substract the calibrated Z to a value close to the height of the drone during calibration to have a coherent positive Z position.
Otherwise, the pose estimation is wrong, although without this modification it worked 1 or 2 times but I don't know why.

Pose estimation:
I managed to fly the drone with 2 and 1 base stations.
So the problem I had before was either I moved one of the base stations by mistake after calibration or I placed the drone too far from the origin of the world frame before take off.

The drone was also stable with 1 base station without the flow deck ! I had to place the drone before take off high up so it can always detect the signal and have a stable position. Position estimation with the BMI088 & BMP388 only is very bad (fast drift in position).
Here, it can also work starting from the ground not far from the base station. However, the estimator needs to be restarted just before taking off or when detecting the signal to have a chance to find its exact position.

I had no problem with the yaw estimation of the Lighthouse.

Oriented +Y

Calibration:
I couldn't calibrate the Lighthouse deck with the position of the sensors modified in lighthouse_types.py. The 8 attempts to calibrate the deck failed. Often the calculation (Last Step) could take 1 or 2 minutes before showing that it failed, instead of about 5 to 10 seconds usually.

positions = np.float32([
  (-_sensor_distance_length / 2, 0.0, -_sensor_distance_width / 2),
  (-_sensor_distance_length / 2, 0.0, _sensor_distance_width / 2),
  (_sensor_distance_length / 2, 0.0, -_sensor_distance_width / 2),
  (_sensor_distance_length / 2, 0.0, _sensor_distance_width / 2)])

So I calibrated using the deck in its normal position.

Pose estimation:
I only tested the drone with the yaw estimation deactivated as you proposed. It helped a lot as the drone was stable with 2 base stations. However, when the signal of one of the base station disapeared, the drone gets unstable and runs towards or away from the base stations. When the lost signal reappears the drone gets stable again. So 1 base station do not work in this configuration. I haven't tested without the flow deck.

Conclusion

Lighthouse deck reversed is all good but it seems to be more sensible to small base station displacements.

Lighthouse deck on the side works only with 2 Lighthouses. If 1 only is detected it will crash. (tested only with the flow deck on)

[krichardsson]

Thanks for sharing!