Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: add get_attitude_quaternion method

This commit is contained in:
Randy Mackay 2024-12-06 20:42:58 +09:00
parent 051c2dcd35
commit 5a1a8d1c10
2 changed files with 16 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_Mount/AP_Mount.cpp
View File

@ -655,18 +655,25 @@ bool AP_Mount::get_poi(uint8_t instance, Quaternion &quat, Location &loc, Locati
} }
#endif #endif
// get mount's current attitude in euler angles in degrees. yaw angle is in body-frame // get attitude as a quaternion. returns true on success.
// returns true on success // att_quat will be an earth-frame quaternion rotated such that
bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg) // yaw is in body-frame.
bool AP_Mount::get_attitude_quaternion(uint8_t instance, Quaternion& att_quat)
{ {
auto *backend = get_instance(instance); auto *backend = get_instance(instance);
if (backend == nullptr) { if (backend == nullptr) {
return false; return false;
} }
return backend->get_attitude_quaternion(att_quat);
}
// get mount's current attitude in euler angles in degrees. yaw angle is in body-frame
// returns true on success
bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg)
{
// re-use get_attitude_quaternion and convert to Euler angles // re-use get_attitude_quaternion and convert to Euler angles
Quaternion att_quat; Quaternion att_quat;
if (!backend->get_attitude_quaternion(att_quat)) { if (!get_attitude_quaternion(instance, att_quat)) {
return false; return false;
} }

5
libraries/AP_Mount/AP_Mount.h
View File

@ -206,6 +206,11 @@ public:
bool get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc) const; bool get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc) const;
#endif #endif
// get attitude as a quaternion. returns true on success.
// att_quat will be an earth-frame quaternion rotated such that
// yaw is in body-frame.
bool get_attitude_quaternion(uint8_t instance, Quaternion& att_quat);
// get mount's current attitude in euler angles in degrees. yaw angle is in body-frame // get mount's current attitude in euler angles in degrees. yaw angle is in body-frame
// returns true on success // returns true on success
bool get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg); bool get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg);