Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[AVEM-1408] Planck copter 4.0.7 avem indago gimbal message check #54

Open
wants to merge 4 commits into
base: planck_Copter-4.0.7_AVEMIndago
Choose a base branch
from

Conversation

CraftyJax
Copy link

@CraftyJax CraftyJax commented Nov 10, 2022

AVEM-1408

This PR adds a check to verify a message from either a gimbal or the VSCI tablet has been received before using the payload yaw control logic in mode_plancktracking. Currently, it only checks whether a gimbal mount object has been created, which will be true if configured as such, even even no gimbal is present. This PR aims to verify a gimbal is present in addition to the gimbal mount object existing, before using any payload yaw logic.

Without these changes, the payload yaw logic will execute using incorrect data, resulting in unexpected yaw behavior. It has been observed that this results in yawing close to north.

@CraftyJax CraftyJax requested review from ptran and AubreyC November 10, 2022 23:00
@CraftyJax CraftyJax assigned ptran and CraftyJax and unassigned CraftyJax Nov 10, 2022
Copy link

@AubreyC AubreyC left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

tbh the logic is a bit hard to follow, maybe by defining variables like bool mount_good = (AP_HAL::micros64() - mount->get_last_mount_control_time_us()) > 50000) or something
could help

some minor comments, but overall looks good


//If we've stopped getting the mavlink yaw rate commands, use 0 yaw rate command
if(AP_HAL::micros64() - mount->get_last_mount_control_time_us() > 500000)
if(copter.mode_guided.mode()==Guided_Angle)
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

space in between ==

else if(copter.flightmode->auto_yaw.mode() == AUTO_YAW_RATE)
{
copter.flightmode->auto_yaw.set_fixed_yaw(
copter.flightmode->auto_yaw.rate_cds(),
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

need to be converted to deg ? (not centidegrees)

@@ -50,7 +52,7 @@ void ModePlanckTracking::run() {
// so we add that offset to the current vehicle heading to obtain the desired absoute heading.
// Note that the getter method "get_follow_yaw_rate()" is poorly named; it is not a rate,
// it's the differene between vehicle and gimbal heading.
if(mount != nullptr) {
if(mount != nullptr && ((mount->get_last_mount_control_time_us() > 0) || (mount->get_last_payload_update_us > 0))) {
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

get_last_payload_update_us -> get_last_payload_update_us() ?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants