CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/libraries/AP_CustomRotations/AP_CustomRotations.cpp
Views: 1798
1
#include "AP_CustomRotations_config.h"

2


3
#if AP_CUSTOMROTATIONS_ENABLED

4


5
#include "AP_CustomRotations.h"

6


7
#include <AP_InternalError/AP_InternalError.h>

8
#include <AP_Vehicle/AP_Vehicle_Type.h>

9
#include <AP_HAL/AP_HAL_Boards.h>

10
#include <AP_BoardConfig/AP_BoardConfig.h>

11


12
const AP_Param::GroupInfo AP_CustomRotations::var_info[] = {

13


14
    // @Param: _ENABLE

15
    // @DisplayName: Enable Custom rotations

16
    // @Values: 0:Disable, 1:Enable

17
    // @Description: This enables custom rotations

18
    // @User: Standard

19
    // @RebootRequired: True

20
    AP_GROUPINFO_FLAGS("_ENABLE", 1, AP_CustomRotations, enable, 0, AP_PARAM_FLAG_ENABLE),

21


22
    // @Group: 1_

23
    // @Path: AP_CustomRotations_params.cpp

24
    AP_SUBGROUPINFO(params[0], "1_", 2, AP_CustomRotations, AP_CustomRotation_params),

25


26
    // @Group: 2_

27
    // @Path: AP_CustomRotations_params.cpp

28
    AP_SUBGROUPINFO(params[1], "2_", 3, AP_CustomRotations, AP_CustomRotation_params),

29


30
    AP_GROUPEND

31
};

32


33
AP_CustomRotation::AP_CustomRotation(AP_CustomRotation_params &_params):

34
    params(_params)

35
{

36
    init();

37
}

38


39
void AP_CustomRotation::init()

40
{

41
    m.from_euler(radians(params.roll),radians(params.pitch),radians(params.yaw));

42
    q.from_rotation_matrix(m);

43
}

44


45
AP_CustomRotations::AP_CustomRotations()

46
{

47
    AP_Param::setup_object_defaults(this, var_info);

48


49
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL

50
    if (singleton != nullptr) {

51
        AP_HAL::panic("AP_CustomRotations must be singleton");

52
    }

53
#endif

54
    singleton = this;

55
}

56


57
void AP_CustomRotations::init()

58
{

59
    if (enable == 0) {

60
        return;

61
    }

62


63
    // make sure all custom rotations are allocated

64
    for (uint8_t i = 0; i < NUM_CUST_ROT; i++) {

65
        AP_CustomRotation* rot = get_rotation(Rotation(i + ROTATION_CUSTOM_1));

66
        if (rot == nullptr) {

67
            AP_BoardConfig::allocation_error("Custom Rotations");

68
        }

69
    }

70
}

71


72
void AP_CustomRotations::convert(Rotation r, float roll, float pitch, float yaw)

73
{

74
    AP_CustomRotation* rot = get_rotation(r);

75
    if (rot == nullptr) {

76
        return;

77
    }

78
    if (!rot->params.roll.configured() && !rot->params.pitch.configured() && !rot->params.yaw.configured()) {

79
        rot->params.roll.set_and_save(roll);

80
        rot->params.pitch.set_and_save(pitch);

81
        rot->params.yaw.set_and_save(yaw);

82
        rot->init();

83
    }

84
}

85


86
void AP_CustomRotations::set(Rotation r, float roll, float pitch, float yaw)

87
{

88
    AP_CustomRotation* rot = get_rotation(r);

89
    if (rot == nullptr) {

90
        return;

91
    }

92
    rot->params.roll.set(roll);

93
    rot->params.pitch.set(pitch);

94
    rot->params.yaw.set(yaw);

95
    rot->init();

96
}

97


98
void AP_CustomRotations::from_rotation(Rotation r, QuaternionD& q)

99
{

100
    AP_CustomRotation* rot = get_rotation(r);

101
    if (rot == nullptr) {

102
        return;

103
    }

104
    q = rot->q.todouble();

105
}

106


107
void AP_CustomRotations::from_rotation(Rotation r, Quaternion& q)

108
{

109
    AP_CustomRotation* rot = get_rotation(r);

110
    if (rot == nullptr) {

111
        return;

112
    }

113
    q = rot->q;

114
}

115


116
void AP_CustomRotations::rotate(Rotation r, Vector3d& v)

117
{

118
    AP_CustomRotation* rot = get_rotation(r);

119
    if (rot == nullptr) {

120
        return;

121
    }

122
    v = (rot->m * v.tofloat()).todouble();

123
}

124


125
void AP_CustomRotations::rotate(Rotation r, Vector3f& v)

126
{

127
    AP_CustomRotation* rot = get_rotation(r);

128
    if (rot == nullptr) {

129
        return;

130
    }

131
    v = rot->m * v;

132
}

133


134
AP_CustomRotation* AP_CustomRotations::get_rotation(Rotation r)

135
{

136
    if (r < ROTATION_CUSTOM_1 || r >= ROTATION_CUSTOM_END) {

137
        INTERNAL_ERROR(AP_InternalError::error_t::bad_rotation);

138
        return nullptr;

139
    }

140
    const uint8_t index = r - ROTATION_CUSTOM_1;

141
    if (rotations[index] == nullptr) {

142
        rotations[index] = NEW_NOTHROW AP_CustomRotation(params[index]);

143
         // make sure param is enabled if custom rotation is used

144
        enable.set_and_save_ifchanged(1);

145
    }

146
    return rotations[index];

147
}

148


149
// singleton instance

150
AP_CustomRotations *AP_CustomRotations::singleton;

151


152
namespace AP {

153


154
AP_CustomRotations &custom_rotations()

155
{

156
    return *AP_CustomRotations::get_singleton();

157
}

158


159
}

160


161
#endif  // AP_CUSTOMROTATIONS_ENABLED

162


163