working with crazy-swarm server and flow-deck

[GalBrandwine]

Hello all.

We have sucsesfuly flown 3 crazyflies with optitrack mocap.
the hover_swarm.launch, and the figure8_csv.py, worked perfectly.

Now we want to use flow deck:
for that we have changed the necesary changed in the hover_swarm.launch file:

(motion_capture_type: "none")

<?xml version="1.0"?>
<launch>
  <arg name="joy_dev" default="/dev/input/js0" />

  <rosparam command="load" file="$(find crazyswarm)/launch/crazyflieTypes.yaml" />
  <rosparam command="load" file="$(find crazyswarm)/launch/crazyflies.yaml" />

  <node pkg="crazyswarm" type="crazyswarm_server" name="crazyswarm_server" output="screen" >
    <rosparam>
      firmware: "bitcraze" # one of "crazyswarm", "bitcraze"
      broadcast_address: "FFE7E7E7E7"
      world_frame: "/world"
      genericLogTopics: ["log1"]
      genericLogTopicFrequencies: [10]
      # genericLogTopic_log1_Variables: ["ekfprof.usec_setup", "ekfprof.usec_innov", "ekfprof.usec_gain", "ekfprof.usec_corr", "ekfprof.usec_cov"]
      # genericLogTopic_log1_Variables: ["profiling.usec_ekf", "profiling.usec_traj", "profiling.usec_ctrl", "profiling.usec_idle"]
      # genericLogTopic_log1_Variables: ["stabilizer.x", "ctrltarget.x", "vicon.x", "stabilizer.z", "ctrltarget.z", "vicon.z"]
      # genericLogTopic_log1_Variables: ["stateEstimate.roll", "ctrlMel.rolld"]
      genericLogTopic_log1_Variables: ["ctrlStat.edist"]
      # genericLogTopic_log1_Variables: ["stateEstimate.yaw", "ctrltarget.yaw", "stateEstimate.x", "ctrltarget.x"] #, "ctrltarget.x", "stateEstimate.roll", "ctrlMel.rolld"]
      firmwareParams: # for all (independent of type)
        commander:
          enHighLevel: 1
        stabilizer:
          estimator: 2 # 1: complementary, 2: kalman, 3: kalmanUSC (only crazyswarm firmware)
          controller: 2 # 1: PID, 2: mellinger
        ring:
          effect: 16 # 6: double spinner, 7: solid color, 16: packetRate
          solidBlue: 255 # if set to solid color
          solidGreen: 0 # if set to solid color
          solidRed: 0 # if set to solid color
          headlightEnable: 0
        #ekf:
        #  ext_var_xy: 1.5e-7 # 1e-7 # 1.5e-7
        #  ext_var_vel: 2e-4 # 2e-4
        #  ext_var_q: 4.5e-3 # 2e-3 # 4.5e-3
        #  gyro_var_xy: 0.2e-2 # 0.2e-4
        #  gyro_var_z: 0.2e-2 # 0.2e-4
        #  acc_var_xyz: 2.4e-3 # 2.4e-3
      # tracking
      motion_capture_type: "none" # one of none,vicon,optitrack,qualisys,vrpn
      object_tracking_type: "libobjecttracker" # one of motionCapture,libobjecttracker
      # vicon_host_name: "vicon"
      optitrack_local_ip: "169.254.148.87"
      optitrack_server_ip: "169.254.148.86"
      # qualisys_host_name: "10.0.5.219"
      # qualisys_base_port: 22222
      # vrpn_host_name: "vicon"
      save_point_clouds: ~/pointCloud.ot
      print_latency: False
      write_csvs: False
      force_no_cache: False
      enable_parameters: True
      enable_logging: False
    </rosparam>
  </node>

  <node name="joy" pkg="joy" type="joy_node" output="screen">
    <param name="dev" value="$(arg joy_dev)" />
  </node>

  <node pkg="crazyswarm" type="crazyswarm_teleop" name="crazyswarm_teleop" output="screen">
    <param name="csv_file" value="$(find crazyswarm)/launch/figure8_smooth.csv" />
    <param name="timescale" value="0.8" />
  </node>

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find crazyswarm)/launch/test.rviz"/>

  <!-- <node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_x" args="/cf2/log1/values[0]"/> -->
  <!-- <node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_roll" args="/cf1/log1/values[2] /cf1/log1/values[3]"/> -->

</launch>

but when we test it by doing:
roslaunch crazyswarm hover_swarm.launch
and than:
rosservice call /cfX/takeoff...

But then the crazyflie is going crazy, and not taking off in a stable and elegant way.
we use the "log1", to log our stateEstimate.x & stateEstimate.y & stateEstimate.z.
they are publishing the initial location of the drone.
and when we try to manually lift the drone we receive the correct feedback.

what i want to know:
0. why the crazyflie is going crazy?

1. is there a way to "reset" the kalman on start?
2. where can i find a list of all available parameters to log with the genericLogTopic_log1_Variables

[whoenig]

0. Most likely there is a problem with the state estimate. We did test the figure8_csv example just by using the flow deck and it worked fine for us. Which firmware version are you using?
1. Yes, you can set some parameters (see https://github.com/whoenig/crazyflie_ros/blob/master/crazyflie_demo/scripts/publish_external_position_vicon.py#L15-L22).
2. You can run rosrun crazyflie_tools listParams --uri <URI-of-CF> to find a list of all parameters and you can use listLogVariables to find a list of all variables you can log. Unfortunately, there is no documentation on what the variables actually mean (and what the units are), apart from reading the crazyflie firmware code.

[jpreiss]

@GalBrandwine did you ever get good results using the Flow deck with crazyswarm?

[whoenig]

I tested this with one CF a few days ago. It works. I'll soon share a PR that adds pose visualization for that case (and the case where other loco systems are used). One problem I see is that sometimes the EKF diverges while the quad is still on the ground and in that case one should obviously not take off.

[MauroPfister]

Are there any updates on this? I also experience a diverging EKF when flying only with the flow deck. Usually I can take off and land once or twice before the EFK diverges while the CF is sitting on the ground. Sometimes rebooting helps but often I have to move the drone a bit and reboot again. My settings for the crazyswarm_server are below and I'm using the default crazyflie type. I first tried with pNAcc_xy: 1.0 and pNAcc_z: 2 but that seemed even worse.

  <node pkg="crazyswarm" type="crazyswarm_server" name="crazyswarm_server" output="screen" >
    <rosparam>
      world_frame: "/world"
      # Logging configuration (Use enable_logging to actually enable logging)
      genericLogTopics: ["log1"]
      genericLogTopicFrequencies: [10]
      genericLogTopic_log1_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"]
      # firmware parameters for all drones (use crazyflieTypes.yaml to set per type, or
      # allCrazyflies.yaml to set per drone)
      firmwareParams:
        commander:
          enHighLevel: 1
        stabilizer:
          estimator: 2 # 1: complementary, 2: kalman
          controller: 2 # 1: PID, 2: mellinger
        ring:
          effect: 16 # 6: double spinner, 7: solid color, 16: packetRate
          solidBlue: 255 # if set to solid color
          solidGreen: 0 # if set to solid color
          solidRed: 0 # if set to solid color
          headlightEnable: 0
        # locSrv:
           extPosStdDev: 1e-3
           extQuatStdDev: 0.5e-1
        kalman:
          resetEstimation: 1
      # tracking
      motion_capture_type: "none" # one of none,vicon,optitrack,qualisys,vrpn
      object_tracking_type: "libobjecttracker" # one of motionCapture,libobjecttracker
      send_position_only: True # set to False to send position+orientation; set to True to send position only
      # vicon_host_name: "vicon"
      optitrack_host_name: "192.168.1.249"
      # qualisys_host_name: "10.0.5.219"
      # qualisys_base_port: 22222
      # vrpn_host_name: "vicon"
      save_point_clouds: ~/pointCloud.ot
      print_latency: False
      write_csvs: False
      force_no_cache: False
      enable_parameters: True
      enable_logging: True
    </rosparam>
  </node>

[whoenig]

This is most likely an issue with the EKF in the official firmware. Did you pick their recent improvement (bitcraze/crazyflie-firmware#608)?

[MauroPfister]

I flashed the master branch of the firmware a few days ago, so I think this improvement should be included.

[MauroPfister]

I did a few more tests today made the following observations. We are always flying above textured rubber mats with a black-white pattern. When standing on a white spot, the EFK does not diverte. However, when moving it over a dark/black spot, it diverges almost immediately. When moving from a black to a white spot, it stops diverging and keeps the offset.

When using the tools provided by crazyswarm, starting and landing on white spots works quite well. When landing on a black spot, the EFK diverges and taking off again is not possible.

When I fly manually with the cfclient in "assisted flight" hover mode, I do not see any difference between starting from a black spot or a white spot.

I checked the parameters in cfclient and as far as I can tell, all of them were the same in "manual flight" and "crazyswarm flight". I used PID controller and EFK estimator in both cases.

Does anyone else observed a similar behaviour?

[whoenig]

The relevant parameters would be in the kalman group. Which crazyflie type are you using? If you use defaultSingleMarker, make sure to comment out https://github.com/USC-ACTLab/crazyswarm/blob/master/ros_ws/src/crazyswarm/launch/crazyflieTypes.yaml#L34-L38. Apart from different settings, the behavior should really not differ between the two modes of operation.

[MauroPfister]

I was using the default crazyflie type and made sure all parameters were the same. However, after some more testing I also noticed the diverging EFK in manual flight. Therefore, I opened an issue on the firmware repo: bitcraze/crazyflie-firmware#617.