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

AP_DDS: Accept body-frame velocity inputs

This commit is contained in:
pedro-fuoco 2023-09-03 21:04:58 -03:00 committed by Peter Barker
parent 198268e03c
commit 62b15a8af0
1 changed files with 25 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_DDS/AP_DDS_ExternalControl.cpp
View File

@ -2,6 +2,7 @@
#include "AP_DDS_ExternalControl.h"
#include "AP_DDS_Frames.h"
#include <AP_AHRS/AP_AHRS.h>
#include <AP_ExternalControl/AP_ExternalControl.h>
@ -11,21 +12,32 @@ bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistSta
if (external_control == nullptr) {
return false;
}
if (strcmp(cmd_vel.header.frame_id, MAP_FRAME) != 0) {
// Although REP-147 says cmd_vel should be in body frame, all the AP math is done in earth frame.
// This is because accounting for the gravity vector.
// Although the ROS 2 interface could support body-frame velocity control in the future,
// it is currently not supported.
return false;
if (strcmp(cmd_vel.header.frame_id, BASE_LINK_FRAME_ID) == 0) {
// Convert commands from body frame (x-forward, y-left, z-up) to NED.
Vector3f linear_velocity;
Vector3f linear_velocity_base_link {
float(cmd_vel.twist.linear.x),
float(cmd_vel.twist.linear.y),
float(-cmd_vel.twist.linear.z) };
const float yaw_rate = -cmd_vel.twist.angular.z;
auto &ahrs = AP::ahrs();
linear_velocity = ahrs.body_to_earth(linear_velocity_base_link);
return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate);
}
// Convert commands from ENU to NED frame
Vector3f linear_velocity {
float(cmd_vel.twist.linear.y),
float(cmd_vel.twist.linear.x),
float(-cmd_vel.twist.linear.z) };
const float yaw_rate = -cmd_vel.twist.angular.z;
return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate);
else if (strcmp(cmd_vel.header.frame_id, MAP_FRAME) == 0) {
// Convert commands from ENU to NED frame
Vector3f linear_velocity {
float(cmd_vel.twist.linear.y),
float(cmd_vel.twist.linear.x),
float(-cmd_vel.twist.linear.z) };
const float yaw_rate = -cmd_vel.twist.angular.z;
return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate);
}
return false;
}