Skip to content

Commit

Permalink
Merge pull request #68 from umdlife/hotfix/motors-stop
Browse files Browse the repository at this point in the history
Add esc data
  • Loading branch information
biancabnd authored Mar 26, 2024
2 parents 0443be2 + 88323e3 commit 58f6e2b
Show file tree
Hide file tree
Showing 8 changed files with 118 additions and 4 deletions.
2 changes: 2 additions & 0 deletions psdk_interfaces/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/HmsInfoMsg.msg"
"msg/HmsInfoTable.msg"
"msg/SingleBatteryInfo.msg"
"msg/EscData.msg"
"msg/EscStatusIndividual.msg"
"srv/CameraGetType.srv"
"srv/CameraSetExposureModeEV.srv"
"srv/CameraGetExposureModeEV.srv"
Expand Down
5 changes: 5 additions & 0 deletions psdk_interfaces/msg/EscData.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@


std_msgs/Header header

EscStatusIndividual[] esc # Array of ESC status
9 changes: 9 additions & 0 deletions psdk_interfaces/msg/EscStatusIndividual.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
int16 current # ESC current, unit: mA
int16 speed # ESC speed, unit: rpm
uint16 voltage # Input power from battery to ESC, unit: mV
int16 temperature # ESC temperature, unit: degree C
int16 stall # Motor is stall
int16 empty # Motor has no load
int16 unbalanced # Motor speed is unbalanced
int16 esc_disconnected # ESC is disconnected
int16 temperature_high # Temperature is high
1 change: 1 addition & 0 deletions psdk_wrapper/cfg/psdk_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -46,3 +46,4 @@
flight_status: 1
battery_level: 1
control_information: 1
esc_data_frequency: 1
19 changes: 19 additions & 0 deletions psdk_wrapper/include/psdk_wrapper/psdk_wrapper.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,7 @@
// PSDK wrapper interfaces
#include "psdk_interfaces/msg/control_mode.hpp"
#include "psdk_interfaces/msg/display_mode.hpp"
#include "psdk_interfaces/msg/esc_data.hpp"
#include "psdk_interfaces/msg/flight_anomaly.hpp"
#include "psdk_interfaces/msg/flight_status.hpp"
#include "psdk_interfaces/msg/gimbal_rotation.hpp"
Expand Down Expand Up @@ -254,6 +255,7 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
int flight_status_frequency;
int battery_level_frequency;
int control_information_frequency;
int esc_data_frequency;
};

std::map<::E_DjiLiveViewCameraPosition, DJICameraStreamDecoder*>
Expand Down Expand Up @@ -448,6 +450,8 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
const T_DjiDataTimestamp* timestamp);
friend T_DjiReturnCode c_rc_callback(const uint8_t* data, uint16_t data_size,
const T_DjiDataTimestamp* timestamp);
friend T_DjiReturnCode c_esc_callback(const uint8_t* data, uint16_t data_size,
const T_DjiDataTimestamp* timestamp);
friend T_DjiReturnCode c_rc_connection_status_callback(
const uint8_t* data, uint16_t data_size,
const T_DjiDataTimestamp* timestamp);
Expand Down Expand Up @@ -818,6 +822,19 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
* @return T_DjiReturnCode error code indicating if the subscription has been
* done correctly
*/

/**
* @brief Retrieves the ESC data provided by DJI PSDK lib and publishes it on
* a ROS 2 topic.This topic provides esc data
* @param data pointer to T_DjiFcSubscriptionEscData data
* @param data_size size of data. Unused parameter.
* @param timestamp timestamp provided by DJI
* @return T_DjiReturnCode error code indicating if the subscription has been
* done correctly
*/
T_DjiReturnCode esc_callback(const uint8_t* data, uint16_t data_size,
const T_DjiDataTimestamp* timestamp);

T_DjiReturnCode gimbal_angles_callback(const uint8_t* data,
uint16_t data_size,
const T_DjiDataTimestamp* timestamp);
Expand Down Expand Up @@ -1764,6 +1781,8 @@ class PSDKWrapper : public rclcpp_lifecycle::LifecycleNode
rclcpp_lifecycle::LifecyclePublisher<
psdk_interfaces::msg::RCConnectionStatus>::SharedPtr
rc_connection_status_pub_;
rclcpp_lifecycle::LifecyclePublisher<psdk_interfaces::msg::EscData>::SharedPtr
esc_pub_;
rclcpp_lifecycle::LifecyclePublisher<
geometry_msgs::msg::Vector3Stamped>::SharedPtr gimbal_angles_pub_;
rclcpp_lifecycle::LifecyclePublisher<
Expand Down
5 changes: 4 additions & 1 deletion psdk_wrapper/include/psdk_wrapper/psdk_wrapper_utils.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,8 @@
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>

#include <map>
#include <string>
#include <vector>
#define IMU_TOPIC_MAX_FREQ 400
#define ATTITUDE_TOPICS_MAX_FREQ 100
Expand All @@ -39,6 +41,7 @@
#define FLIGHT_STATUS_TOPICS_MAX_FREQ 50
#define BATTERY_STATUS_TOPICS_MAX_FREQ 50
#define CONTROL_DATA_TOPICS_MAX_FREQ 50
#define ESC_DATA_TOPICS_FREQ 50

#define GOOD_GPS_SIGNAL_LEVEL 5

Expand Down Expand Up @@ -183,7 +186,7 @@ const std::vector<DJITopic> topics_to_subscribe{
CONTROL_DATA_TOPICS_MAX_FREQ},
DJITopic{DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT,
CONTROL_DATA_TOPICS_MAX_FREQ},
};
DJITopic{DJI_FC_SUBSCRIPTION_TOPIC_ESC_DATA, ESC_DATA_TOPICS_FREQ}};

const std::map<E_DjiCameraType, std::string> camera_type_str = {
{DJI_CAMERA_TYPE_UNKNOWN, "Unkown"},
Expand Down
59 changes: 56 additions & 3 deletions psdk_wrapper/src/modules/telemetry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -198,6 +198,13 @@ c_rc_callback(const uint8_t *data, uint16_t data_size,
return global_ptr_->rc_callback(data, data_size, timestamp);
}

T_DjiReturnCode
c_esc_callback(const uint8_t *data, uint16_t data_size,
const T_DjiDataTimestamp *timestamp)
{
return global_ptr_->esc_callback(data, data_size, timestamp);
}

T_DjiReturnCode
c_rc_connection_status_callback(const uint8_t *data, uint16_t data_size,
const T_DjiDataTimestamp *timestamp)
Expand Down Expand Up @@ -834,6 +841,36 @@ PSDKWrapper::rc_callback(const uint8_t *data, uint16_t data_size,
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

T_DjiReturnCode
PSDKWrapper::esc_callback(const uint8_t *data, uint16_t data_size,
const T_DjiDataTimestamp *timestamp)
{
(void)data_size;
(void)timestamp;
std::unique_ptr<T_DjiFcSubscriptionEscData> esc_data =
std::make_unique<T_DjiFcSubscriptionEscData>(
*reinterpret_cast<const T_DjiFcSubscriptionEscData *>(data));
psdk_interfaces::msg::EscData esc_msg;
esc_msg.header.stamp = this->get_clock()->now();
// Populate the message with ESC data
for (int i = 0; i < 8; ++i)
{
psdk_interfaces::msg::EscStatusIndividual esc_individual_msg;
esc_individual_msg.current = esc_data->esc[i].current;
esc_individual_msg.speed = esc_data->esc[i].speed;
esc_individual_msg.voltage = esc_data->esc[i].voltage;
esc_individual_msg.temperature = esc_data->esc[i].temperature;
esc_individual_msg.stall = esc_data->esc[i].stall;
esc_individual_msg.empty = esc_data->esc[i].empty;
esc_individual_msg.unbalanced = esc_data->esc[i].unbalanced;
esc_individual_msg.esc_disconnected = esc_data->esc[i].escDisconnected;
esc_individual_msg.temperature_high = esc_data->esc[i].temperatureHigh;
esc_msg.esc.push_back(esc_individual_msg);
}
esc_pub_->publish(esc_msg);
return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

T_DjiReturnCode
PSDKWrapper::rc_connection_status_callback(const uint8_t *data,
uint16_t data_size,
Expand Down Expand Up @@ -1389,7 +1426,7 @@ PSDKWrapper::home_point_altitude_callback(const uint8_t *data,

void
PSDKWrapper::subscribe_psdk_topics()
{
{ // NOLINT(readability/fn_size)
T_DjiReturnCode return_code;
if (params_.imu_frequency > 0)
{
Expand Down Expand Up @@ -1419,6 +1456,7 @@ PSDKWrapper::subscribe_psdk_topics()
return_code);
}
}

if (params_.acceleration_frequency > 0)
{
return_code = DjiFcSubscription_SubscribeTopic(
Expand Down Expand Up @@ -1698,6 +1736,7 @@ PSDKWrapper::subscribe_psdk_topics()
return_code);
}
}

if (params_.magnetometer_frequency > 0)
{
return_code = DjiFcSubscription_SubscribeTopic(
Expand All @@ -1712,6 +1751,7 @@ PSDKWrapper::subscribe_psdk_topics()
return_code);
}
}

if (params_.rc_channels_data_frequency > 0)
{
return_code = DjiFcSubscription_SubscribeTopic(
Expand All @@ -1738,6 +1778,20 @@ PSDKWrapper::subscribe_psdk_topics()
return_code);
}
}
if (params_.esc_data_frequency > 0)
{
return_code = DjiFcSubscription_SubscribeTopic(
DJI_FC_SUBSCRIPTION_TOPIC_ESC_DATA,
get_frequency(params_.esc_data_frequency), c_esc_callback);

if (return_code != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
{
RCLCPP_ERROR(get_logger(),
"Could not subscribe successfully to topic "
"DJI_FC_SUBSCRIPTION_TOPIC_ESC_DATA, error %ld",
return_code);
}
}
if (params_.gimbal_data_frequency > 0)
{
return_code = DjiFcSubscription_SubscribeTopic(
Expand Down Expand Up @@ -1870,7 +1924,6 @@ PSDKWrapper::subscribe_psdk_topics()
return_code);
}
}

if (params_.control_information_frequency > 0)
{
return_code = DjiFcSubscription_SubscribeTopic(
Expand Down Expand Up @@ -1947,7 +2000,7 @@ PSDKWrapper::subscribe_psdk_topics()
return_code);
}
}
}
} // NOLINT(readability/fn_size)

void
PSDKWrapper::unsubscribe_psdk_topics()
Expand Down
22 changes: 22 additions & 0 deletions psdk_wrapper/src/psdk_wrapper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,7 @@ PSDKWrapper::PSDKWrapper(const std::string &node_name)
declare_parameter("data_frequency.flight_status", 1);
declare_parameter("data_frequency.battery_level", 1);
declare_parameter("data_frequency.control_information", 1);
declare_parameter("data_frequency.esc_data_frequency", 1);

declare_parameter("num_of_initialization_retries", 1);
}
Expand Down Expand Up @@ -622,6 +623,22 @@ PSDKWrapper::load_parameters()
params_.rc_channels_data_frequency = RC_CHANNELS_TOPICS_MAX_FREQ;
}

if (!get_parameter("data_frequency.esc_data_frequency",
params_.esc_data_frequency))
{
RCLCPP_ERROR(get_logger(), "esc_data_frequency param not defined");
exit(-1);
}
if (params_.esc_data_frequency > ESC_DATA_TOPICS_FREQ)
{
RCLCPP_WARN(get_logger(),
"Frequency defined for the ESC channel topics is higher than "
"the maximum "
"allowed %d. Tha maximum value is set",
ESC_DATA_TOPICS_FREQ);
params_.esc_data_frequency = ESC_DATA_TOPICS_FREQ;
}

if (!get_parameter("data_frequency.gimbal_data",
params_.gimbal_data_frequency))
{
Expand Down Expand Up @@ -831,6 +848,8 @@ PSDKWrapper::initialize_ros_elements()
rc_connection_status_pub_ =
create_publisher<psdk_interfaces::msg::RCConnectionStatus>(
"psdk_ros2/rc_connection_status", 10);
esc_pub_ =
create_publisher<psdk_interfaces::msg::EscData>("psdk_ros2/esc_data", 1);
gimbal_angles_pub_ = create_publisher<geometry_msgs::msg::Vector3Stamped>(
"psdk_ros2/gimbal_angles", 10);
gimbal_status_pub_ = create_publisher<psdk_interfaces::msg::GimbalStatus>(
Expand Down Expand Up @@ -1164,6 +1183,7 @@ PSDKWrapper::activate_ros_elements()
rtk_connection_status_pub_->on_activate();
magnetic_field_pub_->on_activate();
rc_pub_->on_activate();
esc_pub_->on_activate();
rc_connection_status_pub_->on_activate();
gimbal_angles_pub_->on_activate();
gimbal_status_pub_->on_activate();
Expand Down Expand Up @@ -1215,6 +1235,7 @@ PSDKWrapper::deactivate_ros_elements()
rtk_connection_status_pub_->on_deactivate();
magnetic_field_pub_->on_deactivate();
rc_pub_->on_deactivate();
esc_pub_->on_deactivate();
rc_connection_status_pub_->on_deactivate();
gimbal_angles_pub_->on_deactivate();
gimbal_status_pub_->on_deactivate();
Expand Down Expand Up @@ -1339,6 +1360,7 @@ PSDKWrapper::clean_ros_elements()
rtk_connection_status_pub_.reset();
magnetic_field_pub_.reset();
rc_pub_.reset();
esc_pub_.reset();
rc_connection_status_pub_.reset();
gimbal_angles_pub_.reset();
gimbal_status_pub_.reset();
Expand Down

0 comments on commit 58f6e2b

Please sign in to comment.