8258243043

Committed 13 Mar 2024 12:47AM UTC coverage: 36.094% (-0.3%) from 36.352%

Build # 8258243043

Build Type

push

github

Committed by

web-flow

Commit Message

gimbal: add attitude (#2244)

It's confusing to have this as part of telemetry, especially once we
support more than one gimbal.

Signed-off-by: Julian Oes <julian@oes.ch>

Run Details

32 of 289 new or added lines in 7 files covered. (11.07%)

17 existing lines in 6 files now uncovered.

10089 of 27952 relevant lines covered (36.09%)

124.23 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

85.0

/src/mavsdk/core/math_conversions.cpp

#include "mavsdk_math.h"
#include "math_conversions.h"
#include <cmath>

namespace mavsdk {

bool operator==(const Quaternion& lhs, const Quaternion& rhs)
{
    return ((std::isnan(rhs.w) && std::isnan(lhs.w)) || rhs.w == lhs.w) &&
           ((std::isnan(rhs.x) && std::isnan(lhs.x)) || rhs.x == lhs.x) &&
           ((std::isnan(rhs.y) && std::isnan(lhs.y)) || rhs.y == lhs.y) &&
           ((std::isnan(rhs.z) && std::isnan(lhs.z)) || rhs.z == lhs.z);
}

bool operator==(const EulerAngle& lhs, const EulerAngle& rhs)
{
    return ((std::isnan(rhs.roll_deg) && std::isnan(lhs.roll_deg)) ||
            rhs.roll_deg == lhs.roll_deg) &&
           ((std::isnan(rhs.pitch_deg) && std::isnan(lhs.pitch_deg)) ||
            rhs.pitch_deg == lhs.pitch_deg) &&
           ((std::isnan(rhs.yaw_deg) && std::isnan(lhs.yaw_deg)) || rhs.yaw_deg == lhs.yaw_deg);
}

EulerAngle to_euler_angle_from_quaternion(Quaternion quaternion)
{
    auto& q = quaternion;

    EulerAngle euler_angle;
    euler_angle.roll_deg = to_deg_from_rad(
        atan2f(2.0f * (q.w * q.x + q.y * q.z), 1.0f - 2.0f * (q.x * q.x + q.y * q.y)));
    euler_angle.pitch_deg = to_deg_from_rad(asinf(2.0f * (q.w * q.y - q.z * q.x)));
    euler_angle.yaw_deg = to_deg_from_rad(
        atan2f(2.0f * (q.w * q.z + q.x * q.y), 1.0f - 2.0f * (q.y * q.y + q.z * q.z)));

    return euler_angle;
}

Quaternion to_quaternion_from_euler_angle(EulerAngle euler_angle)
{
    const double cos_phi_2 = cos(double(to_rad_from_deg(euler_angle.roll_deg)) / 2.0);
    const double sin_phi_2 = sin(double(to_rad_from_deg(euler_angle.roll_deg)) / 2.0);
    const double cos_theta_2 = cos(double(to_rad_from_deg(euler_angle.pitch_deg)) / 2.0);
    const double sin_theta_2 = sin(double(to_rad_from_deg(euler_angle.pitch_deg)) / 2.0);
    const double cos_psi_2 = cos(double(to_rad_from_deg(euler_angle.yaw_deg)) / 2.0);
    const double sin_psi_2 = sin(double(to_rad_from_deg(euler_angle.yaw_deg)) / 2.0);

    Quaternion quaternion;
    quaternion.w = float(cos_phi_2 * cos_theta_2 * cos_psi_2 + sin_phi_2 * sin_theta_2 * sin_psi_2);
    quaternion.x = float(sin_phi_2 * cos_theta_2 * cos_psi_2 - cos_phi_2 * sin_theta_2 * sin_psi_2);
    quaternion.y = float(cos_phi_2 * sin_theta_2 * cos_psi_2 + sin_phi_2 * cos_theta_2 * sin_psi_2);
    quaternion.z = float(cos_phi_2 * cos_theta_2 * sin_psi_2 - sin_phi_2 * sin_theta_2 * cos_psi_2);

    return quaternion;
}

Quaternion operator*(const Quaternion& lhs, const Quaternion& rhs)
{
    Quaternion result;
    result.w = lhs.w * rhs.w - lhs.x * rhs.x - lhs.y * rhs.y - lhs.z * rhs.z;
    result.x = lhs.w * rhs.x + lhs.x * rhs.w + lhs.y * rhs.z - lhs.z * rhs.y;
    result.y = lhs.w * rhs.y + lhs.y * rhs.w - lhs.x * rhs.z + lhs.z * rhs.x;
    result.z = lhs.w * rhs.z + lhs.z * rhs.w + lhs.x * rhs.y + lhs.y * rhs.x;

    return result;
}

} // namespace mavsdk

1	#include "mavsdk_math.h"
2	#include "math_conversions.h"
3	#include <cmath>
4
5	namespace mavsdk {
6
7	bool operator==(const Quaternion& lhs, const Quaternion& rhs)	1✔
8	{
9	return ((std::isnan(rhs.w) && std::isnan(lhs.w)) \|\| rhs.w == lhs.w) &&	3✔
10	((std::isnan(rhs.x) && std::isnan(lhs.x)) \|\| rhs.x == lhs.x) &&	3✔
11	((std::isnan(rhs.y) && std::isnan(lhs.y)) \|\| rhs.y == lhs.y) &&	5✔
12	((std::isnan(rhs.z) && std::isnan(lhs.z)) \|\| rhs.z == lhs.z);	3✔
13	}
14
NEW 15	bool operator==(const EulerAngle& lhs, const EulerAngle& rhs)	×
16	{
NEW 17	return ((std::isnan(rhs.roll_deg) && std::isnan(lhs.roll_deg)) \|\|	×
NEW 18	rhs.roll_deg == lhs.roll_deg) &&	×
NEW 19	((std::isnan(rhs.pitch_deg) && std::isnan(lhs.pitch_deg)) \|\|	×
NEW 20	rhs.pitch_deg == lhs.pitch_deg) &&	×
NEW 21	((std::isnan(rhs.yaw_deg) && std::isnan(lhs.yaw_deg)) \|\| rhs.yaw_deg == lhs.yaw_deg);	×
22	}
23
24	EulerAngle to_euler_angle_from_quaternion(Quaternion quaternion)	2✔
25	{
26	auto& q = quaternion;	2✔
27
28	EulerAngle euler_angle;	2✔
29	euler_angle.roll_deg = to_deg_from_rad(	4✔
30	atan2f(2.0f * (q.w * q.x + q.y * q.z), 1.0f - 2.0f * (q.x * q.x + q.y * q.y)));	2✔
31	euler_angle.pitch_deg = to_deg_from_rad(asinf(2.0f * (q.w * q.y - q.z * q.x)));	2✔
32	euler_angle.yaw_deg = to_deg_from_rad(	4✔
33	atan2f(2.0f * (q.w * q.z + q.x * q.y), 1.0f - 2.0f * (q.y * q.y + q.z * q.z)));	2✔
34
35	return euler_angle;	2✔
36	}
37
38	Quaternion to_quaternion_from_euler_angle(EulerAngle euler_angle)	5✔
39	{
40	const double cos_phi_2 = cos(double(to_rad_from_deg(euler_angle.roll_deg)) / 2.0);	5✔
41	const double sin_phi_2 = sin(double(to_rad_from_deg(euler_angle.roll_deg)) / 2.0);	5✔
42	const double cos_theta_2 = cos(double(to_rad_from_deg(euler_angle.pitch_deg)) / 2.0);	5✔
43	const double sin_theta_2 = sin(double(to_rad_from_deg(euler_angle.pitch_deg)) / 2.0);	5✔
44	const double cos_psi_2 = cos(double(to_rad_from_deg(euler_angle.yaw_deg)) / 2.0);	5✔
45	const double sin_psi_2 = sin(double(to_rad_from_deg(euler_angle.yaw_deg)) / 2.0);	5✔
46
47	Quaternion quaternion;	5✔
48	quaternion.w = float(cos_phi_2 * cos_theta_2 * cos_psi_2 + sin_phi_2 * sin_theta_2 * sin_psi_2);	5✔
49	quaternion.x = float(sin_phi_2 * cos_theta_2 * cos_psi_2 - cos_phi_2 * sin_theta_2 * sin_psi_2);	5✔
50	quaternion.y = float(cos_phi_2 * sin_theta_2 * cos_psi_2 + sin_phi_2 * cos_theta_2 * sin_psi_2);	5✔
51	quaternion.z = float(cos_phi_2 * cos_theta_2 * sin_psi_2 - sin_phi_2 * sin_theta_2 * cos_psi_2);	5✔
52
53	return quaternion;	5✔
54	}
55
56	Quaternion operator*(const Quaternion& lhs, const Quaternion& rhs)	1✔
57	{
58	Quaternion result;	1✔
59	result.w = lhs.w * rhs.w - lhs.x * rhs.x - lhs.y * rhs.y - lhs.z * rhs.z;	1✔
60	result.x = lhs.w * rhs.x + lhs.x * rhs.w + lhs.y * rhs.z - lhs.z * rhs.y;	1✔
61	result.y = lhs.w * rhs.y + lhs.y * rhs.w - lhs.x * rhs.z + lhs.z * rhs.x;	1✔
62	result.z = lhs.w * rhs.z + lhs.z * rhs.w + lhs.x * rhs.y + lhs.y * rhs.x;	1✔
63
64	return result;	1✔
65	}
66
67	} // namespace mavsdk

mavlink / MAVSDK / 8258243043

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous