Where to compute stereo cam pitch

[e267]

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

[g.rousseau]

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

[e267]

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)
    {
        const ::google::protobuf::Empty message;
        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

[g.rousseau]

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

[e267]

Thanks a lot !

[e267]

@g.rousseau,

I’ve just read again your answer, what do you mean by :

“it is better to do this check in the guidance mode”

Does this mean that I have to compute the pitch in the guidance mode (i.e. computing
asin(2*(q_w*q_y - q_z*.q_x))) ? Or I should compare the current pitch with the target pitch in the guidance mode and compute the current pitch in a service ?

Thanks !

[g.rousseau]

Sorry I was indeed unclear : I advise you do everything in the guidance mode directly.