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.

Sign up for GitHub

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

AP_Mount: Fix GIMBAL_DEVICE_FLAGS in GIMBAL_DEVICE_ATTITUDE_STATUS #27073

Closed
wants to merge 3 commits into from
+26 −5
Closed

AP_Mount: Fix GIMBAL_DEVICE_FLAGS in GIMBAL_DEVICE_ATTITUDE_STATUS #27073

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
ab4df6c
AP_Mount: Don't set GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME if yaw i…
nexton-winjeel May 16, 2024
745c4e5
AP_Mount: Make backend get_gimbal_device_flags() function virtual
nexton-winjeel May 15, 2024
59a5d30
AP_Mount: In Servo backend, set flags to indicate attitude is in body…
nexton-winjeel May 16, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 6 additions & 4 deletions libraries/AP_Mount/AP_Mount_Backend.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -857,12 +857,14 @@ uint16_t AP_Mount_Backend::get_gimbal_device_flags() const
break;
}

const uint16_t yaw_ef_flags = GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME | GIMBAL_DEVICE_FLAGS_YAW_LOCK;
const uint16_t yaw_bf_flags = GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME;

const uint16_t flags = (get_mode() == MAV_MOUNT_MODE_RETRACT ? GIMBAL_DEVICE_FLAGS_RETRACT : 0) |
(get_mode() == MAV_MOUNT_MODE_NEUTRAL ? GIMBAL_DEVICE_FLAGS_NEUTRAL : 0) |
GIMBAL_DEVICE_FLAGS_ROLL_LOCK | // roll angle is always earth-frame
GIMBAL_DEVICE_FLAGS_PITCH_LOCK| // pitch angle is always earth-frame, yaw_angle is always body-frame
GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME | // yaw angle is always in vehicle-frame
Copy link
Contributor Author

@nexton-winjeel nexton-winjeel May 16, 2024 •
edited
Loading

Choose a reason for hiding this comment

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

@Davidsastresas: You made the change to set GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME unconditionally (in f12ca0b). My understanding is that it should only be set if yaw_lock_state is false?

(yaw_lock_state ? GIMBAL_DEVICE_FLAGS_YAW_LOCK : 0);
GIMBAL_DEVICE_FLAGS_ROLL_LOCK | // roll angle is always earth-frame
GIMBAL_DEVICE_FLAGS_PITCH_LOCK | // pitch angle is always earth-frame
(yaw_lock_state ? yaw_ef_flags : yaw_bf_flags); // yaw angle can be either earth-frame or body-frame
return flags;
}

Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_Mount/AP_Mount_Backend.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -288,7 +288,7 @@ class AP_Mount_Backend
void update_angle_target_from_rate(const MountTarget& rate_rad, MountTarget& angle_rad) const;

// helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message
uint16_t get_gimbal_device_flags() const;
virtual uint16_t get_gimbal_device_flags() const;

// sent warning to GCS
void send_warning_to_GCS(const char* warning_str);
Expand Down
16 changes: 16 additions & 0 deletions libraries/AP_Mount/AP_Mount_Servo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -139,6 +139,22 @@ bool AP_Mount_Servo::get_attitude_quaternion(Quaternion& att_quat)
return true;
}

// helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message
uint16_t AP_Mount_Servo::get_gimbal_device_flags() const
{
uint16_t flags = AP_Mount_Backend::get_gimbal_device_flags();
// per get_attitude_quaternion() above, angles are all body frame, so clear all the LOCK flags...
const uint16_t mask = ~(GIMBAL_DEVICE_FLAGS_ROLL_LOCK |
GIMBAL_DEVICE_FLAGS_PITCH_LOCK |
Copy link
Contributor Author

Choose a reason for hiding this comment

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

IMPORTANT: The clearing of GIMBAL_DEVICE_FLAGS_ROLL_LOCK and GIMBAL_DEVICE_FLAGS_PITCH_LOCK is only applicable if #27072 is merged.

GIMBAL_DEVICE_FLAGS_YAW_LOCK |
GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME);
flags &= mask;
// and set the YAW_IN_VEHICLE_FRAME flag
flags |= GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME;

return flags;
}

// private methods

// update body-frame angle outputs from earth-frame angle targets
Expand Down
3 changes: 3 additions & 0 deletions libraries/AP_Mount/AP_Mount_Servo.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,9 @@ class AP_Mount_Servo : public AP_Mount_Backend
// get attitude as a quaternion. returns true on success
bool get_attitude_quaternion(Quaternion& att_quat) override;

// helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message
uint16_t get_gimbal_device_flags() const override;

private:

// update body-frame angle outputs from earth-frame targets
Expand Down
Loading