Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[SYSID] [Motor Test] add dshot telemetry support for motor_test #592

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

[SYSID] [Motor Test] add dshot telemetry support for motor_test #592

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
f9ea80e
[Motor Test][DShot]: add support for dshot and telemetry
Li-Jinjie Jan 31, 2024
6a0932f
[Motor Test][DShot]: set a parameter to choose has_telemetry or not
Li-Jinjie Jan 31, 2024
0d482f5
[Motor Test][DShot]: fix a bug in the launch file. Tested!
Li-Jinjie Jan 31, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion aerial_robot_nerve/motor_test/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,12 +9,13 @@ find_package(catkin REQUIRED COMPONENTS
std_srvs
geometry_msgs
takasako_sps
spinal
)

find_package(Boost REQUIRED COMPONENTS system)

catkin_package(
CATKIN_DEPENDS roscpp std_msgs std_srvs geometry_msgs takasako_sps
CATKIN_DEPENDS roscpp std_msgs std_srvs geometry_msgs takasako_sps spinal
)

include_directories(
Expand Down
11 changes: 7 additions & 4 deletions aerial_robot_nerve/motor_test/launch/test.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,8 @@
<!-- parameters for oneshot -->
<arg name="raise_duration" default="2.0" />
<arg name="brake_duration" default="4.0" />
<arg name="has_dshot_telemetry" default="false" />

<arg name="force_sensor" default="CFS034CA301U" /> <!-- other sensor: PFS055YA501U6 -->

<node name="power_node" pkg="takasako_sps" type="scpi_tcp_client_node" output="screen" />
Expand All @@ -17,10 +19,10 @@
<rosparam file="$(find cfs_sensor)/config/$(arg force_sensor).yaml" command="load" />
</node>

<include file="$(find rosserial_server)/launch/serial.launch" >
<arg name="port" value="/dev/ttyUSB0" />
<arg name="baud" value="921600" />
</include>
<!-- <include file="$(find rosserial_server)/launch/serial.launch" >-->
<!-- <arg name="port" value="/dev/ttyUSB0" />-->
<!-- <arg name="baud" value="921600" />-->
<!-- </include>-->

<node name="motor_test_node" pkg="motor_test" type="motor_test_node" output="screen">
<param name="force_sensor_sub_name" value="/cfs/data" />
Expand All @@ -33,5 +35,6 @@
<param name="max_pwm_value" value="$(arg max_pwm_value)" />
<param name="raise_duration" value="$(arg raise_duration)" />
<param name="brake_duration" value="$(arg brake_duration)" />
<param name="has_dshot_telemetry" value="$(arg has_dshot_telemetry)" />
</node>
</launch>
2 changes: 2 additions & 0 deletions aerial_robot_nerve/motor_test/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,11 +14,13 @@
<build_depend>std_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>takasako_sps</build_depend>
<build_depend>spinal</build_depend>

<run_depend>geometry_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>std_srvs</run_depend>
<run_depend>takasako_sps</run_depend>
<run_depend>spinal</run_depend>

</package>
59 changes: 50 additions & 9 deletions aerial_robot_nerve/motor_test/src/motor_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
#include <std_msgs/Empty.h>
#include <std_srvs/Empty.h>
#include <takasako_sps/PowerInfo.h>
#include <spinal/ESCTelemetryArray.h>

namespace Mode
{
Expand Down Expand Up @@ -44,6 +45,14 @@ class MotorTest
start_cmd_sub_ = nh_.subscribe("start_log_cmd", 1, &MotorTest::startCallback, this, ros::TransportHints().tcpNoDelay());
sps_on_pub_ = nh.advertise<std_msgs::Empty>("/power_on_cmd", 1);

/* dshot telemetry */
nhp_.param("has_dshot_telemetry", has_dshot_telemetry_, false);

if (has_dshot_telemetry_)
{
esc_telem_sub_ = nh_.subscribe("/esc_telem", 1, &MotorTest::escTelemCallback, this, ros::TransportHints().tcpNoDelay());
}

ROS_WARN("run: %f, raise: %f, brake: %f", run_duration_, raise_duration_, brake_duration_);

ros::ServiceClient calib_client = nh_.serviceClient<std_srvs::Empty>("/cfs_sensor_calib");
Expand Down Expand Up @@ -72,6 +81,7 @@ class MotorTest
ros::Subscriber force_snesor_sub_;
ros::Subscriber power_info_sub_;
ros::Subscriber start_cmd_sub_;
ros::Subscriber esc_telem_sub_;
ros::Publisher motor_pwm_pub_;
ros::Publisher sps_on_pub_;

Expand All @@ -93,6 +103,12 @@ class MotorTest

std::ofstream ofs_;

/* dshot measurement */
bool has_dshot_telemetry_;
uint32_t rpm_;
int temperature_;
float voltage_;

void startCallback(const std_msgs::EmptyConstPtr & msg)
{
std::string file_name = std::string("motor_test_") + std::to_string((int)ros::Time::now().toSec()) + std::string(".txt");
Expand All @@ -108,6 +124,13 @@ class MotorTest
start_flag_ = true;
}

void escTelemCallback(const spinal::ESCTelemetryArrayConstPtr & msg)
{
rpm_ = msg->esc_telemetry_1.rpm;
temperature_ = msg->esc_telemetry_1.temperature;
voltage_ = (float)(msg->esc_telemetry_1.voltage) / 100;
}

void powerInfoCallback(const takasako_sps::PowerInfoConstPtr& msg)
{
currency_ = msg->currency;
Expand All @@ -120,16 +143,34 @@ class MotorTest
double force_norm = sqrt(msg->wrench.force.x * msg->wrench.force.x +
msg->wrench.force.y * msg->wrench.force.y +
msg->wrench.force.z * msg->wrench.force.z);
if (has_dshot_telemetry_)
{
ofs_ << pwm_value_ << " "
<< msg->wrench.force.x << " "
<< msg->wrench.force.y << " "
<< msg->wrench.force.z << " "
<< force_norm << " "
<< msg->wrench.torque.x << " "
<< msg->wrench.torque.y << " "
<< msg->wrench.torque.z << " "
<< currency_ << " "
<< rpm_ << " "
<< temperature_ << " "
<< voltage_;
}
else
{
ofs_ << pwm_value_ << " "
<< msg->wrench.force.x << " "
<< msg->wrench.force.y << " "
<< msg->wrench.force.z << " "
<< force_norm << " "
<< msg->wrench.torque.x << " "
<< msg->wrench.torque.y << " "
<< msg->wrench.torque.z << " "
<< currency_;
}

ofs_ << pwm_value_ << " "
<< msg->wrench.force.x << " "
<< msg->wrench.force.y << " "
<< msg->wrench.force.z << " "
<< force_norm << " "
<< msg->wrench.torque.x << " "
<< msg->wrench.torque.y << " "
<< msg->wrench.torque.z << " "
<< currency_;

if(test_mode_ == Mode::ONESHOT)
{
Expand Down
Loading