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_DDS/AP_DDS_ExternalControl.cpp
Views: 1798
1
#include "AP_DDS_config.h"

2


3
#if AP_DDS_ENABLED

4


5
#include "AP_DDS_ExternalControl.h"

6
#include "AP_DDS_Frames.h"

7
#include <AP_AHRS/AP_AHRS.h>

8


9
#include <AP_ExternalControl/AP_ExternalControl.h>

10


11
// These are the Goal Interface constants. Because microxrceddsgen does not expose

12
// them in the generated code, they are manually maintained

13
// Position ignore flags

14
static constexpr uint16_t TYPE_MASK_IGNORE_LATITUDE = 1;

15
static constexpr uint16_t TYPE_MASK_IGNORE_LONGITUDE = 2;

16
static constexpr uint16_t TYPE_MASK_IGNORE_ALTITUDE = 4;

17


18
bool AP_DDS_External_Control::handle_global_position_control(ardupilot_msgs_msg_GlobalPosition& cmd_pos)

19
{

20
    auto *external_control = AP::externalcontrol();

21
    if (external_control == nullptr) {

22
        return false;

23
    }

24


25
    if (strcmp(cmd_pos.header.frame_id, MAP_FRAME) == 0) {

26
        // Narrow the altitude

27
        const int32_t alt_cm  = static_cast<int32_t>(cmd_pos.altitude * 100);

28


29
        Location::AltFrame alt_frame;

30
        if (!convert_alt_frame(cmd_pos.coordinate_frame, alt_frame)) {

31
            return false;

32
        }

33


34
        constexpr uint32_t MASK_POS_IGNORE =

35
            TYPE_MASK_IGNORE_LATITUDE |

36
            TYPE_MASK_IGNORE_LONGITUDE |

37
            TYPE_MASK_IGNORE_ALTITUDE;

38


39
        if (!(cmd_pos.type_mask & MASK_POS_IGNORE)) {

40
            Location loc(cmd_pos.latitude * 1E7, cmd_pos.longitude * 1E7, alt_cm, alt_frame);

41
            if (!external_control->set_global_position(loc)) {

42
                return false; // Don't try sending other commands if this fails

43
            }

44
        }

45


46
        // TODO add velocity and accel handling

47


48
        return true;

49
    }

50


51
    return false;

52
}

53


54
bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel)

55
{

56
    auto *external_control = AP::externalcontrol();

57
    if (external_control == nullptr) {

58
        return false;

59
    }

60


61
    if (strcmp(cmd_vel.header.frame_id, BASE_LINK_FRAME_ID) == 0) {

62
        // Convert commands from body frame (x-forward, y-left, z-up) to NED.

63
        Vector3f linear_velocity;

64
        Vector3f linear_velocity_base_link {

65
            float(cmd_vel.twist.linear.x),

66
            float(cmd_vel.twist.linear.y),

67
            float(-cmd_vel.twist.linear.z) };

68
        const float yaw_rate = -cmd_vel.twist.angular.z;

69


70
        auto &ahrs = AP::ahrs();

71
        linear_velocity = ahrs.body_to_earth(linear_velocity_base_link);

72
        return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate);

73
    }

74


75
    else if (strcmp(cmd_vel.header.frame_id, MAP_FRAME) == 0) {

76
        // Convert commands from ENU to NED frame

77
        Vector3f linear_velocity {

78
            float(cmd_vel.twist.linear.y),

79
            float(cmd_vel.twist.linear.x),

80
            float(-cmd_vel.twist.linear.z) };

81
        const float yaw_rate = -cmd_vel.twist.angular.z;

82
        return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate);

83
    }

84


85
    return false;

86
}

87


88
bool AP_DDS_External_Control::arm(AP_Arming::Method method, bool do_arming_checks)

89
{

90
    auto *external_control = AP::externalcontrol();

91
    if (external_control == nullptr) {

92
        return false;

93
    }

94


95
    return external_control->arm(method, do_arming_checks);

96
}

97


98
bool AP_DDS_External_Control::disarm(AP_Arming::Method method, bool do_disarm_checks)

99
{

100
    auto *external_control = AP::externalcontrol();

101
    if (external_control == nullptr) {

102
        return false;

103
    }

104


105
    return external_control->disarm(method, do_disarm_checks);

106
}

107


108
bool AP_DDS_External_Control::convert_alt_frame(const uint8_t frame_in,  Location::AltFrame& frame_out)

109
{

110


111
    // Specified in ROS REP-147; only some are supported.

112
    switch (frame_in) {

113
    case 5: // FRAME_GLOBAL_INT

114
        frame_out = Location::AltFrame::ABSOLUTE;

115
        break;

116
    case 6: // FRAME_GLOBAL_REL_ALT

117
        frame_out = Location::AltFrame::ABOVE_HOME;

118
        break;

119
    case 11: // FRAME_GLOBAL_TERRAIN_ALT

120
        frame_out = Location::AltFrame::ABOVE_TERRAIN;

121
        break;

122
    default:

123
        return false;

124
    }

125
    return true;

126
}

127


128


129
#endif // AP_DDS_ENABLED

130