# Where to compute stereo cam pitch

Hi,

I’m writing a guidance mode in which the drone moves its stereo camera and I’d like the mode to be exited once the stereo cam has reached a target pitch. The pitch is computed the following way : asin(2.*(q.w*q.y - q.z*q.x)), and when the pitch is approximately reached, a message is sent to trigger a transition (to exit the mode). I’ve tried to compute the pitch using a service or directly in the guidance mode. I’d like to know what’s the best way to do it because I don’t get the same results with the 2 methods.

Thanks

Hi @e267 !

The stereo camera pitch you’d like to compute is relative to the drone or to another vector basis ? e.g. NED, NED start Glossary - 7.2

The quaternion q you mention is the quaternion of the stereo camera in the drone vector basis or in a ground fixed vector basis (NED, NED start) ?

Best,
Gauthier

1 Like

Hi @g.rousseau !

I’d like to compute the stereo camera pitch in the best possible way using the NED start vector basis.

The quaternion q is in the NED start vector basis.

Here is the code :

What’s in the guidance mode :

``````void SetStereoCam::enter()
{
target_pitch = 0.f; // degrees
}
void SetStereoCam::beginStep()
{
mTlmConsumer->getSample(NULL, telemetry::Method::TLM_LATEST);
}
void SetStereoCam::generateDroneReference()
{
guidance::Output *output = getOutput();
lockFrontCam(output);
resetVerticalVelocity(output);
setStereoCamPitch(output, target_pitch);
}
void SetStereoCam::correctDroneReference()
{
// send a msg if pitch reached at +-5 degrees
if (target_pitch*M_PI/180.f-0.09.f < pitch && pitch < target_pitch*M_PI/180.f+0.09.f)
{
msg_evt_sender.pitchReached(message);
}
}

``````

Where the function setStereoCamPitch() is :

``````void setStereoCamPitch(guidance::Output *output, float pitch)
{
pitch = pitch*M_PI/180.f;

output->mHasStereoCamReference = true;
output->mStereoCamReference.mutable_pitch()->set_ctrl_mode(CamController::Messages::ControlMode::Enum::POSITION);
output->mStereoCamReference.mutable_pitch()->set_frame_of_ref(CamController::Messages::FrameOfReference::Enum::NED_START);
output->mStereoCamReference.mutable_pitch()->set_position(pitch);
}
``````

The variable pitch (used in the if statement) is computed in a service and then shared through telemetry data :

``````float pitch = asin(2.f*(input->quaternions.q_w*input->quaternions.q_y - input->quaternions.q_z*input->quaternions.q_x));
``````

with :

``````static const struct tlm_reg_field s_tlm_data_in_fields[] = {
TLM_REG_FIELD_SCALAR_EX(struct tlm_data_in, quaternions.q_w,
"orientation_ned_start_q.w", TLM_TYPE_FLOAT32),
TLM_REG_FIELD_SCALAR_EX(struct tlm_data_in, quaternions.q_x,
"orientation_ned_start_q.x", TLM_TYPE_FLOAT32),
TLM_REG_FIELD_SCALAR_EX(struct tlm_data_in, quaternions.q_y,
"orientation_ned_start_q.y", TLM_TYPE_FLOAT32),
TLM_REG_FIELD_SCALAR_EX(struct tlm_data_in, quaternions.q_z,
"orientation_ned_start_q.z", TLM_TYPE_FLOAT32),
};
static const struct tlm_reg_field s_tlm_data_out_fields[] = {
TLM_REG_FIELD_SCALAR(struct tlm_data_out, algo.pitch,
TLM_TYPE_FLOAT32),
};
``````

I’d like to know if it’s better to compute the pitch in the guidance mode or not. I need the transition triggered by the message pitchReached to be the more accurate as possible

Thank you,
Eliott

Ok thank you for the details.

If you are writing a custom guidance mode dedicated to this use case, then it is better to do this check in the guidance mode directly and, as you did, send a message when the target pitch is reached.

Notice that, since the stereo camera only has one stabilization axis, its reachable pitch values in NED start depend on the attitude of the drone. (An extreme example that may help visualize this limitation is the following : roll the drone 90°, now whatever the stereo rotation on its axis, its pitch in NED start will always be 0°).
If this is an issue, you might want to check the stereo pitch angle relatively to the drone instead.

I hope this helps,
Gauthier

Thanks a lot !