Drone rotation not working

Hi,

I’m trying to rotate the drone by x rad using the following code :

guidance::Output *output = getOutput();

float x = M_PI;
float theta = 2.f*acos(mDroneNedQuaternionW);
float angle = theta + x;

output->mHasYawReference = true;
output->mYawReference.mutable_trajectory_ned()->set_config(ColibryLite::Messages::YawControlCon::Enum::DEFAULT);
output->mYawReference.mutable_trajectory_ned()->mutable_ref()->set_x(angle);
output->mYawReference.mutable_trajectory_ned()->mutable_ref()->set_dx(0.f);

However I can’t manage to make it rotate the right angle. Any ideas on how to make it rotate correctly ?

Thanks,

Hi @e267!

Just to be sure : can you confirm that the code you provided is in the generateAttitudeReferences() function of your custom guidance mode ?

I think there is a typo in the following line

output->mYawReference.mutable_trajectory_ned()->set_config(ColibryLite::Messages::YawControlCon::Enum::DEFAULT);

The message type should be YawControlConfig rather than YawControlCon.

Also, I guess you are getting mDroneNedQuaternionW from the following telemetry variable drone_controller.attitude_ned_q.w ? Did you take care to update this variable (using the getSample function, like in Vertical Trajectory in C++ - #2 by g.rousseau) ?

Finally, maybe you could check the result of your operation 2 acos(q_w), see if there is any unexpected result ? I guess you are making an approximation that the drone is hovering flat and that the drone quaternion in NED only has a yaw component, but maybe you need to use the complete formula to extract exactly the yaw from the quaternion ?

Best,
Gauthier

1 Like

Hi @g.rousseau !

The code is in the function generateDroneReference(), is that a problem ?
There is indeed a typo, I’ve fixed it and I’m using getSample() to update the drone_controller.attitude_ned_q.w telemetry variable in the function beginStep().

This is indeed the approximation I’m making. I’ve checked the value of 2*acos(q_w), and changed it to 2*acos(q_w)*sign(q_z). It’s now working, however I’m new to quaternion and not completely sure about what I’m doing, what do you mean by ‘the complete formula’ ?

Thanks a lot !

Hi,

No it is not a problem to put it in the generateDroneReference(), it will work fine and it’s ok for very simple guidance modes. For more complex ones, you might want to separate drone position and drone+camera rotation generation however, for readability Guidance API - 7.2. It is just a generic guideline though, feel free to organize your mode as you prefer.

Regarding the quaternion, I think you are trying to extract the yaw angle of the ZYX Euler angles decomposition of the drone attitude. The exact formula for this is the following
psi = atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y^2 + q.z^2))
(see Conversion between quaternions and Euler angles - Wikipedia)

The Euler angles are available in the telemetry but not documented yet We are planning to add them, it will make that kind of use case simpler to implement.

Best,
Gauthier

1 Like

Thank you @g.rousseau :slight_smile:

This topic was automatically closed 3 days after the last reply. New replies are no longer allowed.