Gazebo/SITL GUIDED_NOGPS not fully stable

[KiloNovemberDelta]

Issue details

Hello everyone, I'm really new to Github, so I hope I'm publishing in the right place. If not, please let me know where to explain my problem.

Before starting, thank you all for the great job you are doing here ! Documentation and wiki are great, and this open source project is beautiful.

My objectif is to fly an autonomous hexacopter in an indoor environment (so I'm using the mode GUIDED_NOGPS) through Gazebo. For this, I made the F550 model in Gazebo, and used the ArduCopter plugin like the IRIS model.

My problem is the following : The drone fly correctly in the GUIDED mode. But with the GUIDED_NOGPS, when I want to stabilize the drone by setting 0 in roll, pitch and yaw after a take off, the drone is still moving. So I suspect my problem is about the parameters used in the firmware, but I cannot find which one I have to change. I tried to set all the offset close to 0.

If you need the .sdf file for the model, I can provide it here.

If you have any clue, feel free to provide your idea. Thank you for your considation for my problem.

Version

Gazebo 7.x Firmware 3.5.5

Platform

[ ] All [ ] AntennaTracker [X] Copter [ ] Plane [ ] Rover [ ] Submarine

Airframe type

Hexacopter, type +

Hardware type

SITL (IRL : PixHawk)

Logs

The drone parameters file (/Tools/autotest/default_params/[].parm) used is the following :

Small values offset so #INS is recognised as being calibrated

Accelerometer offset close to 0

INS_ACCOFFS_X 0.001 INS_ACCOFFS_Y 0.001 INS_ACCOFFS_Z 0.001 INS_ACCSCAL_X 1.001 INS_ACCSCAL_Y 1.001 INS_ACCSCAL_Z 1.001 INS_ACC2OFFS_X 0.001 INS_ACC2OFFS_Y 0.001 INS_ACC2OFFS_Z 0.001 INS_ACC2SCAL_X 1.001 INS_ACC2SCAL_Y 1.001 INS_ACC2SCAL_Z 1.001 INS_ACC3OFFS_X 0.001 INS_ACC3OFFS_Y 0.001 INS_ACC3OFFS_Z 0.001 INS_ACC3SCAL_X 1.001 INS_ACC3SCAL_Y 1.001 INS_ACC3SCAL_Z 1.001

Position of INS

INS_POS1_X 0 INS_POS1_Y 0 INS_POS1_Z 0 PLND_ENABLED 0

ENABLE EKF2

AHRS_EKF_TYPE 2 #Use EKF2 as EKF

No offset in compass

COMPASS_LEARN 2 #Learning with EKF COMPASS_USE 1 #Using gyro for yaw, and not GPS COMPASS_OFS_X 0 COMPASS_OFS_Y 0 COMPASS_OFS_Z 0 COMPASS_OFS2_X 0 COMPASS_OFS2_Y 0 COMPASS_OFS2_Z 0

State Covariance Matrix ? Useful ?

EK2_GYRO_P_NSE 0 #initial 0 EK2_ACC_P_NSE 0 EK2_GBIAS_P_NSE 0 #Rate gyro bias stability rad/s/s EK2_GSCL_P_NSE 0 #Rate gyro scale factor stability 1/s

ARMING_CHECK 0 #No arming check for take off

RC

RC1_MAX 2000.000000 RC1_MIN 1000.000000 RC1_TRIM 1500.000000 RC2_MAX 2000.000000 RC2_MIN 1000.000000 RC2_TRIM 1500.000000 RC3_MAX 2000.000000 RC3_MIN 1000.000000 RC3_TRIM 1500.000000 RC4_MAX 2000.000000 RC4_MIN 1000.000000 RC4_TRIM 1500.000000 RC5_MAX 2000.000000 RC5_MIN 1000.000000 RC5_TRIM 1500.000000 RC6_MAX 2000.000000 RC6_MIN 1000.000000 RC6_TRIM 1500.000000 RC7_MAX 2000.000000 RC7_MIN 1000.000000 RC7_TRIM 1500.000000 RC8_MAX 2000.000000 RC8_MIN 1000.000000 RC8_TRIM 1500.000000

MOT_THST_EXPO 0.36 MOT_THST_HOVER 0.5 CH8_OPT 39

All parameters from SIM at 0

SIM_ACC_RND 0 SIM_GYR_RND 0 SIM_WIND_SPD 0 SIM_WIND_TURB 0 SIM_BARO_RND 0 SIM_MAG_RND 0

Adding Sonar

SIM_SONAR_SCALE 10 RNGFND_TYPE 1 RNGFND_SCALING 10 RNGFND_PIN 0 RNGFND_MAX_CM 5000

Remove GPS

AHRS_GPS_USE 0 FENCE_ENABLE 0

REMOVE GPS IN SIMULATION

SIM_GPS_DISABLE 1 SIM_GPS2_ENABLE 0

Hexatoper + :

FRAME_CLASS,2 FRAME_TYPE,0

GUIDED_NOGPS MODE

FLTMODE1 20 FLTMODE6 20

Pictures

As you can see, when I listen the IMU from mavros (/mavros/imu/data) and from Gazebo (get by a plugin publishing in ROS), I get this kind of graph during a take off and then flying by putting 0 to roll, pitch, yaw. In the x axis, the time is in 0,1 s (update rate at 10Hz).

ROLL (radians) PITCH (radians) Gyro X axis Gyro Y axis Accelerometer X axis Accelerometer Y axis

When the drone is not taking off : IMU RAW : from /mavros/imu/data_raw IMU G : from Gazebo through plugin IMU : from /mavros/imu/data (usually hiding the IMU RAW)

Linear acceleration X

Linear acceleration Y

Gyro X

Gyro Y

[SwiftGust]

Hello, It would be great to if you post your dataflash log too. (It would be in the home/ardupilot/logs )

with Gazebo Simulator there are lots of things could go wrong since Gazebo replaces actual hardware

and in my experience most of problem comes from setting up Gazebo SDF

for example just setting a parameter wrong causes model to go off...

I doubt it would be a problem of PID tuning but, you said it flew well in guided

I hope you to check it flies also well in ardupilot alone like auto, poshold mode with etc...

and please double check motor of your gazebo model also matches with control mixer of ardupilot too http://ardupilot.org/copter/docs/frame-type-configuration.html

[KiloNovemberDelta]

Thanks to be so fast to answer !

Indeed, Gazebo is not simple to manage ..

For the SDF file, I have adjusted the SDF model of the IRIS in order to suit with the F550.

I have double check that motors orientations are the same as described by ArduPilot website. The IMU orientation is correct too, the front X directed to motor 1, Y pointing to the right, and Z down. If the problem was here, it would be not possible for the drone to fly in GUIDED mode, right ?

I have found this log, I hope it would be helpful. Btw, how do you read this binary file ? From Mission Planner ? (I can do another experience and provide the logs from it, if this one is not enough) 00000064.zip

I will check if the drone flies well in other mode, like poshold etc. and I will let you know !

Is it possible that the problem comes from a mismatch between what the IMU of Gazebo send, and what the ArduCopter "believes" ? For example noise is not takes into account for filters or anything else ? Because as I can see from the graph, there is a noticeable gap from the gyroscope and the accelerometer between Gazebo and ArduCopter.

[SwiftGust]

@KiloNovemberDelta Logs can be read using Mission Planner or APMPlanner2 and MP also can convert .bin to .mat file

As I look through log, it's quite different from your graph. Is this same log of that flight? This log tells there's nothing much wrong, only commanded rotation of heading after the take-off is given, roll & pitch commanded 0, model is just not following setpoint quick enough with small oscillation and there are also bit of oscillation in altitude control, too.

It just would require little bit of PID tuning would be required if you didn't since.

Sensor noises are handled in ArduPilot's EKF2, there are tunable parameters if you need. Directions of data between Gazebo and Ardupilot could mismatch since ArduPilot uses North-East-Down coordinate as aerospace conventions, Gazebo uses North-West-Up coordinate, refer how Iris model handled this when ardupilot plugin is used and also check before flight whether it is correct or not by applying torque to the model using Gazebo's apply force/torque function. Usually it's coming from model's visual data( .dae / .stl )

[KiloNovemberDelta]

Thank you again for your help.

I will provide you another log ASAP of a similar experience to be sure. I agree with you, they are not coherent to each other. If we analyze only the IMU from ArduPilot, the drone is supposed to be stabilized and even if some PID tuning can improve it, all seems correct. But if we analyze the Gazebo part, we can see that the drone is not having the right behavior.

Besides, I checked also with Iris model in GUIDED_NOGPS mode, and the drone have the same behavior as my model. So I don’t think that it is coming from model’s visual..

I checked also the mismatch between ArduCopter and Gazebo’s coordinate, and it seems like the change of coordinate is taken into account in the source code of the ArduCopter plugin.

I will focus on the noise correction from the EKF2 in the ArduPilot's part, maybe it's coming from here. I will let you know about this point. If you have still ideas, do not hesitate, it is truly valuable for me !

[SwiftGust]

When you check coordinate mismatch, It is important to make sure all the its direction of position and rotation are correct and yes, it's handled in ArduCopter(or ArduPilot) plugin, like below, rotation matters most.

<plugin name="arducopter_plugin" filename="libArduPilotPlugin.so">
      <fdm_addr>127.0.0.1</fdm_addr>
      <fdm_port_in>9002</fdm_port_in>
      <fdm_port_out>9003</fdm_port_out>
    <modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
      <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
      <imuName>iris_demo::iris::iris/imu_link::imu_sensor</imuName>
      <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>

If you followed Gazebo Iris model and didn't add much noise then tuning of EKF wouldn't be necessary.

For the GUIDED_NOGPS with Iris model, mine works just fine, except the fact that default parameters of PID gains are little bit over-powered for the roll-axis

I think you should investigate command publisher from ROS through communication may have problem, whether it is correctly given to the ArduPilot side.

[KiloNovemberDelta]

Thanks again for your answer ! I was not able to answer you last week, but I'm available now !

So, in order to be sure that everything is correct by my side, do you have the same behavior with the IRIS drone when generating the world "iris_ardupilot.world" ?

state.log (I open it with the command roscore & rosrun gazebo_ros gazebo -u -p [state.log], but I suppose you already know it)

In GUIDED_NOGPS, I publish in the topic /mavros/setpoint_raw/attitude, by sending in quaternion the roll, pitch, yaw and throttle at high frequency (around 50Hz)

When you said that we need to tune a little bit the PID gains, do you speak about the ArduPilot side (with the folder .parm) or is it with the Gazebo (the 0.20 in the model.sdf of the iris_with_standoffs_demo) ? And are you tuning it "by trying step by step and hoping that Murphy's law is not working" or do you have a robust method ? :)

[SwiftGust]

yes, I used "iris_ardupilot.world" in my repository, PID tuning is not critical, don't bother.

how do you convert quaternion to Euler angles? And is this rotation well defined in NED frame?

Just sending quaternion without any conversion or modification in ArduPilot side won't work there's quaternion input acessor for attitude control

I meant PID gains in ArduCopter parameter itself, PID gains in Gazebo are actuator gains Effects of each gains of PID control in terms of state response is well-known, just you need to know how PID controller is structured in ArduPilot or any system you are tune to

[KiloNovemberDelta]

For the state.log that I published in the last comment, I just send all the angle to 0. Inside a class, I use this code :

import tf
(...)
        while not rospy.is_shutdown():
            quaternion = tf.transformations.quaternion_from_euler(self.roll, self.pitch, self.yaw)

            AT = AttitudeTarget()
            AT.header = self.h
            AT.type_mask = 0
            AT.orientation.x = quaternion[0]
            AT.orientation.y = quaternion[1]
            AT.orientation.z = quaternion[2]
            AT.orientation.w = quaternion[3]
            AT.body_rate.x = 0.1
            AT.body_rate.y = 0.1
            AT.body_rate.z = 0.1
            AT.thrust = self.thrust

            self.pub.publish(AT)
            self.r.sleep() #For frequency

And the roll, pitch and yaw, is based on the drone frame.

[SwiftGust]

If you send a quaternion then you should modify ArduCopter side, that you should use void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat) instead of Euler angle input accessor. I wonder why body rate is constantly set to 0.1?

[KiloNovemberDelta]

I was not aware of this possibility, that's a good thing to know ! And if I want to publish directly to Euler angle (so no need to transform it into quaternion), how am I supposed to publish it ? X for roll, Y for pitch, Z for yaw, and nothing in W ? Or do I need to publish in an another topic ? Or maybe something else ?

For the body rate, to be honest, I don't know what it is representing. As I understood, it is the desired speed (rad/sec for example) to reach our desired position. So I tried to do something simple, by considering this rate as a constant. http://mavlink.org/messages/common#SET_ATTITUDE_TARGET Is it supposed to be a dynamic value ?

Thanks again for your patience, GUIDED_NOGPS mode seems to be quit new in ArduCopter and I don't find a lot of documentation about it (maybe I can participate about it if needed btw)

[SwiftGust]

Well, I can't find there is another topic publishes Euler angle target on mavros, probably due to the fact that PX4 only uses quaternion internally.

In your link, it says SET_ATTITUDE_TARGET Message can ignore by setting type_mask body rate command is used in Acro mode, it would be easy to understand how acro mode works so rate command is better to set to constant zeros, keep commanding aircraft to roll, pitch, yaw and 0.1 rad/sec is very dangerous. and it says the order of quaternion is [w, x ,y, z] not [x, y, z, w].

I don't sure how it is decoded so it would be best try both ways construct quaternion from received data over mavros or your idea [0, roll, pitch, yaw].

[KiloNovemberDelta]

I changed the type_mask as 0b0000111 in order to not consider the body_rate x, y, z. For the order of quaternion orientations, it is already done by the object AttitudeTarget(), by using the attribute orientation.x etc. So no problem from here. I prefer to publish quaternion for a quaternion topic, and change the code of ArduCopter.

I have found that the function void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat) is in the location ardupilot/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp, can you please indicate me which code do I have to change for the input accessor works in quaternion ?

For now, I still have the little drift in the roll rotation (like you saw in state.log file). Do you have the same behavior which needs to be corrected by the PID tuning ? Or do you think it is needed to change ArduCopter code ?

Thanks again for your time, I'm sure we will succeed !

[SwiftGust]

In the code of Guided_NoGPS Mode https://github.com/ArduPilot/ardupilot/blob/17c12dab2a9898f1dc540b89d39ec9586d416468/ArduCopter/mode_guided_nogps.cpp#L20

the actual code is defined in Guided Mode, Following code calls Euler angle input accessor. https://github.com/ArduPilot/ardupilot/blob/17c12dab2a9898f1dc540b89d39ec9586d416468/ArduCopter/mode_guided.cpp#L631-L635

which can be replaced with attitude_control->input_quaternion(Quaternion attitude_desired_quat)

What I saw in the state.log was yaw drift, if there is some problem with Ardupilot, then we need Ardupilot log rather than log gazebo or ROS. Probably it wouldn't be an issue of PID tuning rather it would be an issue of command you given, also check the order of quaternion in Ardupilot side too. The log would be in the logs folder that generated automatically where you called sim_vehicle.py

[KiloNovemberDelta]

As you asked, I provide you another experience which is just a take off of the IRIS in Guided_NoGPS mode. I use the Copter-3.5 branch, and nothing is modify from it. Gazebo state : state.log ArduCopter logs : 00000002.zip

I hope it helps. Thanks !

I tried to modify the code, but to be honest, I don't know enough the structure of it to know where to take directly the quaternion data.. So I prefer to not touch it yet, the time to understand well all the code.

[SwiftGust]

After reviewing your log, position drift is due to slight attitude error at take-off, no problem at all. Since this is due to you are not using position or velocity controller which is in the flight mode like Loiter, Guided, PosHold, Auto. If you use one of these flight modes, it will have no problem.

[KiloNovemberDelta]

Thank you for your analyze, if you say that this behavior is normal, I will carry on the simulation by including a return of positionning (with a closing loop) to stabilize the drone. I think the variable "type_mask" of the message "SET_ATTITUDE_TARGET" was also a part of the solution.
I let you know if it works well in this issue topic, and I hope to close it soon ! I really appreciate your motivation to develop this project and maintain this community :D

[KiloNovemberDelta]

Hello again SwiftGust ! The simulation is going well, so the problem is supposed to be about the communication (with the type_mask) and the need of return of position. Thank you and I hope your project will going well !

I'm closing the issue :)

[mhrrrr]

can i get the files to spawn a hexa frame insteadd of default quad iris in gazebo?