Vertical Trajectory in C++

[e267]

Hi,

I’m trying to make the drone move with a guidance mode written in cpp with the following code :

    mOutput->mHasVerticalReference = true;
    DroneController::Messages::VerticalTrajectory test = mOutput->mVerticalReference.trajectory();
    ColibryLite::Messages::TrajectoryPoint test2 = test.ref();
    test2.set_x(-10.);
    test2.set_dx(0.5);

but it does not work and I get this error : ‘E dronectrl (drone-controlle-94) : unknown vertical reference variant: 0’

Would it be possible to have an example ? The goal is to move 10 meters vertically. Also, I’d like to know how to have access to telemetry data in cpp and especially to have access to quaternions estimations.

Thanks a lot !

[g.rousseau]

Hi @e267,

Concerning the error you are receiving, I think the problem is just the method you are using to access and set the command type and values.
The following code will request the drone to move vertically to the altitude 10m above take off position.

	guidance::Output *output = getOutput();

	output->mHasVerticalReference = true;
	auto verticalTrajectoryRef = output->mVerticalReference.mutable_trajectory()->mutable_ref();

	verticalTrajectoryRef->set_x(-10.f);
	verticalTrajectoryRef->set_dx(0.f);
	verticalTrajectoryRef->set_ddx(0.f);

Note : I did not specify a vertical velocity since I don’t know whether your drone is above or below this reference altitude.

If you want to increase the current altitude by 10m, you should first get the current altitude from the telemetry, add 10m to it and send the result as altitude reference.
In order to access the telemetry, you need a telemetry consumer in your guidance mode.

The documentation of the telemetry API is available here Library API - 7.2 and the list of telemetry sections and data here Telemetry list - 7.2

Example, in the .hpp

#include <libtelemetry.hpp>

// ...

class MyMode : public guidance::Mode {
private:
// declare a telemetry consumer as class member

// telemetry consumer
telemetry::Consumer *mTlmConsumer;

// you will also need class members in which to copy each telemetry data you want to access
// e.g. the current drone ATO altitude

// drone altitude ATO [m]
float mDroneAltitudeAto;

// ...
}

in the .cpp :

• instantiate and register the telemetry consumer in the mode constructor
• destroy the telemetry consumer in the mode destructor
• update the telemetry at each mode step (and wherever you may also need up to date telemetry, for instance in the configure() method)

// create the telemetry consumer and register to the variables you need in the constructor
MyMode::MyMode(guidance::Guidance *guidance)
{
	// ...

	// create the telemetry consumer
	mTlmConsumer = telemetry::Consumer::create();

	// register all the variables you need, e.g. the current drone ATO altitude
	mTlmConsumer->reg(mDroneAltitudeAto, "drone_controller.altitude_ato");

	// ... register all the data you need the same way

	// and end registration
	mTlmConsumer->regComplete();

}

// do not forget to destroy the telemetry consumer in the destructor of the mode to prevent memory leaks
MyMode::~MyMode()
{
	telemetry::Consumer::release(mTlmConsumer);

	// ...
}

// wherever you need an up to date telemetry, call the getSample function
// it is usually a good idea to do it at the mode entry (in the enter() or in the configure() methods) and in the beginStep method
void MyMode::beginStep()
{
	mTlmConsumer->getSample(NULL, telemetry::Method::TLM_LATEST);

	// ...
}

this allows you to use this data in the generateDroneReference() method to generate a vertical trajectory

void MyMode::generateDroneReference()
{
	// ...

	// note : the ATO altitude must be converted into a vertical position in the NED basis (z axis downward)
	float verticalPositionRef = -mDroneAltitudeAto - 10.f;

	guidance::Output *output = getOutput();

	output->mHasVerticalReference = true;
	auto verticalTrajectoryRef = output->mVerticalReference.mutable_trajectory()->mutable_ref();

	verticalTrajectoryRef->set_x(verticalPositionRef);
	verticalTrajectoryRef->set_dx(0.f);
	verticalTrajectoryRef->set_ddx(0.f);

	// ...
}

Hope this helps

Best,
Gauthier

[e267]

Wonderful ! Thank you @g.rousseau