Quad not following the circular trajectory

[sivasriram2112]

Hi, I am getting the following error when I fly execute . The quad doesn't startup and wobbles in and around the starting point

roslaunch geometric_controller sitl_trajectory_track_circle.launch

[INFO] [1639431613.853444, 0.148000]: Spawn status: SpawnModel: Successfully spawned entity
[ INFO] [1639431613.865262085, 0.148000000]: Physics dynamic reconfigure ready.
INFO  [simulator] Simulator connected on TCP port 4560.
[ INFO] [1639431614.001515641, 0.152000000]: Waiting for home [pose...]
INFO  [commander] LED: open /dev/led0 failed (22)
INFO  [init] Mixer: etc/mixers/quad_w.main.mix on /dev/pwm_output0
INFO  [init] setting PWM_AUX_OUT none
INFO  [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550
INFO  [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540
INFO  [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14280 remote port 14030
INFO  [mavlink] mode: Gimbal, data rate: 400000 B/s on udp port 13030 remote port 13280
[ INFO] [1639431614.227571183, 0.376000000]: udp0: Remote address: 127.0.0.1:14580
[ INFO] [1639431614.227691948, 0.376000000]: IMU: High resolution IMU detected!
INFO  [logger] logger started (mode=all)
INFO  [logger] Start file log (type: full)
INFO  [logger] [logger] ./log/2021-12-13/21_40_14.ulg   
INFO  [logger] Opened full log file: ./log/2021-12-13/21_40_14.ulg
INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [px4] Startup script returned successfully
pxh> INFO  [mavlink] partner IP: 127.0.0.1
[vehicle_spawn_sivasriram_All_Series_19343_5015075089446036047-8] process has finished cleanly
log file: /home/sivasriram/.ros/log/19a13108-5c5d-11ec-b434-0862662d0a79/vehicle_spawn_sivasriram_All_Series_19343_5015075089446036047-8*.log
[ INFO] [1639431615.211665526, 1.356000000]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot
[ INFO] [1639431615.214171617, 1.356000000]: IMU: High resolution IMU detected!
[ INFO] [1639431616.220172499, 2.360000000]: GF: Using MISSION_ITEM_INT
[ INFO] [1639431616.220272987, 2.360000000]: RP: Using MISSION_ITEM_INT
[ INFO] [1639431616.220333019, 2.360000000]: WP: Using MISSION_ITEM_INT
[ INFO] [1639431616.220406327, 2.364000000]: VER: 1.1: Capabilities         0x000000000000e4ef
[ INFO] [1639431616.220477963, 2.364000000]: VER: 1.1: Flight software:     010d0040 (35c7c1bb79000000)
[ INFO] [1639431616.220558718, 2.364000000]: VER: 1.1: Middleware software: 010d0040 (35c7c1bb79000000)
[ INFO] [1639431616.220613233, 2.364000000]: VER: 1.1: OS software:         050400ff (d4c06e9dfb7bb14d)
[ INFO] [1639431616.220664372, 2.364000000]: VER: 1.1: Board hardware:      00000001
[ INFO] [1639431616.220717185, 2.364000000]: VER: 1.1: VID/PID:             0000:0000
[ INFO] [1639431616.220775540, 2.364000000]: VER: 1.1: UID:                 4954414c44494e4f
[ INFO] [1639431618.927855675, 5.068000000]: IMU: Attitude quaternion IMU detected!
[ INFO] [1639431619.013621701, 5.152000000]: Offboard enabled
INFO  [tone_alarm] notify negative
[ INFO] [1639431620.013014697, 6.152000000]: Home pose initialized to: position: 
  x: 0.00809383
  y: 0.000434098
  z: -0.0616224
orientation: 
  x: -0.000873638
  y: -0.000735852
  z: 0.0193442
  w: -0.999812

[ INFO] [1639431620.512134435, 6.652000000]: Got pose! Drone Ready to be armed.
INFO  [tone_alarm] home set
INFO  [tone_alarm] notify negative
[ INFO] [1639431624.866792742, 11.000000000]: Offboard enabled
[ WARN] [1639431624.880110821, 11.012000000]: CMD: Unexpected command 176, result 0
[ INFO] [1639431630.224896701, 16.356000000]: WP: mission received
[ INFO] [1639431630.225095333, 16.356000000]: GF: mission received
[ INFO] [1639431630.225204472, 16.356000000]: RP: mission received
INFO  [commander] Armed by external command 
[ INFO] [1639431630.884415418, 17.016000000]: Vehicle armed
INFO  [commander] Takeoff detected  
WARN  [commander] Imbalanced propeller detected, landing advised
WARN  [ekf2] primary EKF changed 5 (filter fault) -> 0
WARN  [ekf2] primary EKF changed 0 (filter fault) -> 4
WARN  [ekf2] primary EKF changed 4 (filter fault) -> 1
WARN  [ekf2] primary EKF changed 1 (filter fault) -> 5
WARN  [ekf2] primary EKF changed 5 (filter fault) -> 2
WARN  [ekf2] primary EKF changed 2 (filter fault) -> 4
WARN  [ekf2] primary EKF changed 4 (filter fault) -> 5
WARN  [ekf2] primary EKF changed 5 (filter fault) -> 4
WARN  [ekf2] primary EKF changed 4 (filter fault) -> 5
WARN  [ekf2] primary EKF changed 5 (filter fault) -> 0
WARN  [ekf2] primary EKF changed 0 (filter fault) -> 4
WARN  [ekf2] primary EKF changed 4 (filter fault) -> 1
WARN  [ekf2] primary EKF changed 1 (filter fault) -> 2
WARN  [ekf2] primary EKF changed 2 (filter fault) -> 4
INFO  [commander] Landing detected  
INFO  [commander] Disarmed by landing   
INFO  [logger] closed logfile, bytes written: 3502933
INFO  [commander] Armed by external command 
INFO  [logger] Start file log (type: full)
INFO  [logger] [logger] ./log/2021-12-13/21_41_10.ulg   
INFO  [logger] Opened full log file: ./log/2021-12-13/21_41_10.ulg
[ INFO] [1639431670.915461308, 57.012000000]: Vehicle armed
INFO  [commander] Takeoff detected  
INFO  [commander] Landing detected  
INFO  [commander] Disarmed by landing   

[mzahana]

@Jaeyoung-Lim I am facing a similar problem. The iris quadcopter model basically shoots up and the goes unstable before it crashes into the ground in Gazebo. I am just using the `roslaunch geometric_controller sitl_trajectory_track_circle.launch with no modifications.

Any hints on what would be the issue? I didn't face this issue using old versions of the geometric_controller, can't remember which version(s) exactly.

[mzahana]

Update: After some tuning, for the iris Gazebo model, I was able to track the circular trajectory

<node pkg="geometric_controller" type="geometric_controller_node" name="geometric_controller" output="screen">
        <param name="mav_name" type="string" value="$(arg mav_name)" />
          <remap from="command/bodyrate_command" to="/mavros/setpoint_raw/attitude"/>
          <param name="ctrl_mode" value="$(arg command_input)" />
          <param name="enable_sim" value="$(arg gazebo_simulation)" />
          <param name="enable_gazebo_state" value="true"/>
          <param name="max_acc" value="20.0" />
          <param name="attctrl_constant" value="0.3"/>
          <param name="normalizedthrust_constant" value="0.06"/>
          <param name="normalizedthrust_offset" value="0.1"/>
          <param name="Kp_x" value="10.0"/>
          <param name="Kp_y" value="10.0"/>
          <param name="Kp_z" value="20.0"/>
          <param name="Kv_x" value="5.0"/>
          <param name="Kv_y" value="5.0"/>
          <param name="Kv_z" value="10.0"/>
  </node>

[Jaeyoung-Lim]

@mzahana Awesome! Would you be able to make a PR with the additional tuning?

[sivasriram2112]

Thank you @Jaeyoung-Lim and @mzahana for your replies, I was able to try the previous branch devel-parameter-estimator without any issues. I'll try the tuning parameters from @mzahana.

[RBJin]

@mzahana Thank you very much for providing these parameters! I am curious about how to tune parameters such as attctrl_constant, normalizedthrust_constant. How are they calculated, and is there a more efficient way to tune the Kp Kv parameters?

[mzahana]

I believe @Jaeyoung-Lim can answer that.