Resetting Ardupilot controller after a crash

Hi all,

I’m trying to use Reinforcement Learning to train an agent to produce waypoints for a drone to follow using the Ardupilot controller. To train it, I’m using a Gazebo simulation environment with the software stack from this github repo: https://github.com/Intelligent-Quads/iq_tutorials (ROS, mavros, ardupilot, mavproxy etc). The issue I’m having is that when training, the drone frequently crashes. When it does, I reset the drone position in Gazebo, but the Ardupilot controller no longer works properly. I’ve copied the log from the MAVProxy terminal to the bottom of this post.

What I’m hoping to achieve is to have the Ardupilot controller reset after every crash so the EKF2 is properly calibrated and is able to continue the training. I think I could re-run the launch script, but this takes 1-2 minutes so is not feasible when expecting 1000s of crashes. Is there a way to quickly reset the controller? (or whatever the problem is).

Here’s the MAVPoxy log and thanks a lot! (Please let me know if you’d need to see anything else):

online system 1
Mode STABILIZE
AP: Initialising APM
AP_IRLock_SITL::init()
AP: Calibrating barometer
AP: ArduCopter V4.0.4 (40502bd9)
AP: f5b158577ba8461b8473defab448b6ec
AP: Frame: QUAD
AP: Barometer 1 calibration complete
Init Gyro***

Ready to FLY AP: GPS 1: detected as u-blox at 115200 baud
AP: EKF2 IMU0 initial yaw alignment complete
AP: EKF2 IMU1 initial yaw alignment complete
AP: EKF2 IMU0 tilt alignment complete
AP: EKF2 IMU1 tilt alignment complete
AP: EKF2 IMU0 origin set
AP: EKF2 IMU1 origin set
AP: EKF2 IMU0 is using GPS
AP: EKF2 IMU1 is using GPS
Flight battery 100 percent
Got COMMAND_ACK: REQUEST_AUTOPILOT_CAPABILITIES: ACCEPTED
Got COMMAND_ACK: DO_SET_MODE: ACCEPTED
Mode GUIDED
AP: ArduCopter V4.0.4 (40502bd9)
AP: f5b158577ba8461b8473defab448b6ec
AP: Frame: QUAD
Got COMMAND_ACK: GET_HOME_POSITION: ACCEPTED
AP: Arming motors
Got COMMAND_ACK: COMPONENT_ARM_DISARM: ACCEPTED
Got COMMAND_ACK: COMPONENT_ARM_DISARM: ACCEPTED
Got COMMAND_ACK: COMPONENT_ARM_DISARM: ACCEPTED
Got COMMAND_ACK: COMPONENT_ARM_DISARM: ACCEPTED
Got COMMAND_ACK: COMPONENT_ARM_DISARM: ACCEPTED
Got COMMAND_ACK: COMPONENT_ARM_DISARM: ACCEPTED
ARMED
Got COMMAND_ACK: COMPONENT_ARM_DISARM: ACCEPTED
Got COMMAND_ACK: NAV_TAKEOFF: ACCEPTED
AP: EKF2 IMU0 in-flight yaw alignment complete
AP: EKF2 IMU1 in-flight yaw alignment complete
AP: EKF primary changed:1 // (problem starts around here I think)
AP: EKF variance
Mode LAND
height -6
AP: GPS Glitch
height -21
AP: EKF primary changed:0
AP: Vibration compensation ON
height 5
AP: Potential Thrust Loss (2)
height 14
AP: EKF2 IMU0 switching to compass 1
AP: EKF2 IMU1 switching to compass 1
height -17
height 6
height -10
height 0
height 9
AP: Vibration compensation OFF
AP: GPS Glitch cleared
AP: EKF primary changed:1
AP: GPS Glitch
AP: EKF variance
AP: EKF2 IMU1 switching to compass 2
height 20
Flight battery 90 percent
AP: EKF2 IMU0 switching to compass 2
AP: EKF2 IMU1 switching to compass 0
AP: Disarming motors
AP: EKF primary changed:0
DISARMED
height 10
AP: GPS Glitch cleared
AP: PreArm: EKF2 Yaw inconsistent by 20 deg
AP: PreArm: EKF2 Yaw inconsistent by 30 deg
AP: PreArm: EKF2 Yaw inconsistent by 33 deg
AP: PreArm: EKF2 Yaw inconsistent by 30 deg
AP: PreArm: EKF2 Yaw inconsistent by 23 deg

I think that the problem is the ‘gps glitch’ since I’m essentially teleporting the drone without resetting the gps, but not sure how to solve this (or if it’s even the issue)