Using loco system with the crazyswarm

[aarsht7]

I tried to use loco positioning system with single crazyflie using the crazyswarm package.

What I did

I modified the hover_swarm.launch line 34

# tracking
      motion_capture_type: "none" # one of none,vicon,optitrack,optitrack_closed_source,qualisys,vrpn

What I get in the terminal

... logging to /home/aarsh/.ros/log/f8e603a4-c475-11ec-a774-211f3031ad4f/roslaunch-aarsh-pc-21355.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://aarsh-pc:36987/

SUMMARY
========

PARAMETERS
      Lots of parameters

NODES
  /
    crazyswarm_server (crazyswarm/crazyswarm_server)
    crazyswarm_teleop (crazyswarm/crazyswarm_teleop)
    joy (joy/joy_node)
    rviz (rviz/rviz)

auto-starting new master
process[master]: started with pid [21225]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to c2a0d756-c475-11ec-a774-211f3031ad4f
process[rosout-1]: started with pid [21250]
started core service [/rosout]
process[crazyswarm_server-2]: started with pid [21257]
process[joy-3]: started with pid [21258]
process[crazyswarm_teleop-4]: started with pid [21260]
process[rviz-5]: started with pid [21265]
[ERROR] [1650877060.207232792]: Couldn't open joystick /dev/input/js0. Will retry every second.
[ INFO] [1650877060.213615074]: Wait for services...
[ INFO] [1650877060.214782278]: waitForService: Service [/emergency] has not been advertised, waiting...
[ INFO] [1650877060.235958513]: waitForService: Service [/emergency] is now available.
[ INFO] [1650877060.237626234]: Manager ready.
ch: 1
[ INFO] [1650877060.369218049]: --- 4 --- 
[ INFO] [1650877060.369793128]: Parsed crazyflies.yaml successfully.
[ INFO] [1650877060.369801752]: Adding CF: cf4 (radio://0/80/2M/E7E7E7E7E4, cf4)...
[ INFO] [1650877060.377958431]: CF ctor: 0.008142 s
[ INFO] [1650877060.381953148]: [cf4] SYS: ----------------------------
[ INFO] [1650877060.385086702]: [cf4] SYS: Crazyflie 2.1 is up and running!
[ INFO] [1650877060.387631420]: [cf4] SYS: Build 0:4d476a20f811 (2022.03) CLEAN
[ INFO] [1650877060.391323111]: [cf4] SYS: I am 0x203936564E4B50160035001D and I have 1024KB of flash!
[ INFO] [1650877060.392784514]: [cf4] CFGBLK: v1, verification [OK]
[ INFO] [1650877060.394293975]: [cf4] DECK_CORE: 2 deck(s) found
[ INFO] [1650877060.397300581]: [cf4] DECK_CORE: Calling INIT on driver bcDWM1000 for deck 0
[ INFO] [1650877060.400197452]: [cf4] DECK_CORE: Calling INIT on driver bcLedRing for deck 1
[ INFO] [1650877060.402598125]: [cf4] IMU: BMI088: Using I2C interface.
[ INFO] [1650877060.405047633]: [cf4] IMU: BMI088 Gyro connection [OK].
[ INFO] [1650877060.407602215]: [cf4] IMU: BMI088 Accel connection [OK]
[ INFO] [1650877060.409898878]: [cf4] IMU: BMP388 I2C connection [OK]
[ INFO] [1650877060.412336959]: [cf4] ESTIMATOR: Using Kalman (2) estimator
[ INFO] [1650877060.414928853]: [cf4] CONTROLLER: Using PID (1) controller
[ INFO] [1650877060.417471375]: [cf4] MTR-DRV: Using brushed motor driver
[ INFO] [1650877060.419999061]: [cf4] SYS: About to run tests in system.c.
[ INFO] [1650877060.421511921]: [cf4] SYS: NRF51 version: 2022.0
[ INFO] [1650877060.423565544]: [cf4] EEPROM: I2C connection [OK].
[ INFO] [1650877060.425275517]: [cf4] STORAGE: Storage check [OK].
[ INFO] [1650877060.427762417]: [cf4] IMU: BMI088 gyro self-test [OK]
[ INFO] [1650877060.429373194]: [cf4] DECK_CORE: Deck 0 test [OK].
[ INFO] [1650877060.430864870]: [cf4] SYS: Self test passed!
[ INFO] [1650877060.433586777]: [cf4] STAB: Wait for sensor calibration...
[ INFO] [1650877060.435158042]: [cf4] SYS: Free heap: 17736 bytes
[ INFO] [1650877060.436590592]: [cf4] STAB: Ready to fly.
[ INFO] [1650877060.439202254]: [cf4] DWM: Automatic mode: detected TWR
[ INFO] [1650877060.440306923]: [cf4] Requesting parameters...
[ INFO] [1650877060.471825372]: Found variables in cache.
[ INFO] [1650877060.723690037]: [cf4] reqParamTOC: 0.345699 s (233 params)
[ INFO] [1650877060.723720607]: Requesting memories...
[ INFO] [1650877060.726079244]: Memories: 9
[ INFO] [1650877060.762224061]: [cf4] Ready. Elapsed: 0.384244 s
[ INFO] [1650877060.762240210]: CF run: 0.384289 s
update commander/enHighLevel to 1
update kalman/resetEstimation to 1
update locSrv/extPosStdDev to 0.001
update locSrv/extQuatStdDev to 0.05
update ring/effect to 15
update ring/headlightEnable to 0
update ring/solidBlue to 0
update ring/solidGreen to 0
update ring/solidRed to 0
update stabilizer/controller to 1
update stabilizer/estimator to 2
update ctrlMel/i_range_m_z to 1500
update ctrlMel/i_range_xy to 2
update ctrlMel/i_range_z to 0.4
update ctrlMel/kR_xy to 60000
update ctrlMel/kR_z to 50000
update ctrlMel/kd_omega_rp to 200
update ctrlMel/kd_xy to 0.2
update ctrlMel/kd_z to 0.4
update ctrlMel/ki_m_z to 500
update ctrlMel/ki_xy to 0.05
update ctrlMel/ki_z to 0.05
update ctrlMel/kp_xy to 0.2
update ctrlMel/kp_z to 0.99
update ctrlMel/kw_xy to 20000
update ctrlMel/kw_z to 12000
update ctrlMel/mass to 0.032
update ctrlMel/massThrust to 132000
[ INFO] [1650877060.768289737]: [cf4] Update parameters
[ WARN] [1650877060.802365076]: [cf4] Link Quality low (0.580000)
[ INFO] [1650877060.843953929]: Update params: 0.081697 s
[ INFO] [1650877060.844240222]: Started 1 threads

It gets connected for a second (I see the blink of green light on the led deck) and suddenly it gets disconnected I tried it several times, gets the same results. Any help would be appreciated. Thanks

[knmcguire]

Hi!

What did you use to setup your loco positioning system? Did you use this tutorial from the bitcraze website?

[aarsht7]

Yes, I have used the same tutorial. First I was trying with TWR. I thought it might be an issue.

After changing TWR to TDoA2. I do not get the weak link warning (80% of time) But it still does not show constant light for the connection (It blink between white and green with white majority of the time). I tried to run hello_world.py sometime it overshoot to the ceiling and sometimes it doesn't even fly. I think something with the position estimation. I tried both the estimator and the controller from the hover_swarm.launch

[knmcguire]

The estimator should never be changed to the complementary filter (0) as it does not handle position data, so that would have made sense that it went completely up. Always put that on the extended kalman filter (1). Check out this explanation. However, all the controllers should work, but perhaps the PID controller (0) is less aggressively tuned so it is a bit more stable perhaps.

With the loco positioning system, it might be that the z-position is estimated under the ground, and that the setpoint is not high enough to make it go up. You can try to plot the stateEstimate.z to see where the current z position estimate is. The can be either done on the cfclient or ros topics with logging enabled in the launch file.

[aarsht7]

Yes perhaps the z estimation is unstable with the loco TDoA2/3 mode (not only Z but whole pose). Atleast that's what I have seen it in the Loco tab inside the cfclient gui. I think after using kalman filter it should be more stable. I will try to plot it whenever I get hands on again with the system.
Thanks for the help 👍🏻