There is a problem that we can’t get rid of and looks a lot like a bug in the
GUIDED mode.
We are controlling the drone’s attitude using the SET_ATTITUDE_TARGET message
and sending quaternions, with the bitmask for the yaw set to absolute yaw angle
(instead of yaw rate). The goal of that is to control the drone’s position
around an object using a lidar.
The problem is that sometimes (often and randomly) the drone will rotate 20º to
40º on its yaw axis, throwing off position control. We checked and double
checked, the yaw we are sending in the quaternion is constant. It really is
Ardupilot drifting away from is setpoint. In the logs, the Yaw and DesYaw (that
we believe is Ardupilot’s internal yaw setpoint) are totally desynchronised
during the event.
The drone is a 6 kilograms octa-quad with ArduCopter 3.5 running on a pixhawk,
with the SET_ATTITUDE_TARGET messages sent from Dronekit on a Raspberry Pi
through serial.
I will possibly post this message in a Github issue if it indeed looks like a
GUIDED mode bug.
Thanks in advance for your insights.
NB: This is from the code that we used to send the messages:
def send_attitude_target(self, roll_angle=0.0, pitch_angle=0.0, yaw_angle=0.0, thrust=0.5): """Send an attitude command to the drone. At the time of writting, the drone's attitude must be commanded using a quaternion, so all angles must be commanded simultaneously. roll_angle, pitch_angle and yaw_angle are in radians. thrust is a number between 0 and 1, 0.5 having the special meaning of "maintain your current altitude". """ msg = self.copter.message_factory.set_attitude_target_encode( 0, 0, # target system 0, # target component 0b00000100, # type mask: bit 1 is LSB self._to_quaternion(roll_angle, pitch_angle, yaw_angle), # q 0, # body roll rate in radian 0, # body pitch rate in radian 0, # body yaw rate in radian thrust) # thrust: < 0.5 descend, = 0.5 hold, > 0.5 ascend self.copter.send_mavlink(msg) self._log("send_attitude target: roll=%7.4f, pitch=%7.4f, yaw=%7.4f, thrust=%6.4f" % (roll_angle, pitch_angle, yaw_angle, thrust))
def _to_quaternion(self, roll=0.0, pitch=0.0, yaw=0.0): """Convert Euler angles in radians to quaternions.""" t0 = math.cos(yaw * 0.5) t1 = math.sin(yaw * 0.5) t2 = math.cos(roll * 0.5) t3 = math.sin(roll * 0.5) t4 = math.cos(pitch * 0.5) t5 = math.sin(pitch * 0.5) w = t0 * t2 * t4 + t1 * t3 * t5 x = t0 * t3 * t4 - t1 * t2 * t5 y = t0 * t2 * t5 + t1 * t3 * t4 z = t1 * t2 * t4 - t0 * t3 * t5 return [w, x, y, z]
Logs of a short flight where the bug was reproduced: https://drive.google.com/open?id=0B7L1T2gKDuFocHlacWxyRHdHdkp5aG51R1JfN093UlItbmxF