Skip to content

Commit

Permalink
[Spinal][Config] Additional features and new configuration for Spinal (…
Browse files Browse the repository at this point in the history 
…#630)

* [Spinal][Dynamixel] transplanted dynamixel interface to spinal

* [Spinal][Encoder] modified encorder interface imitating neuron one

* [Spinal] change usart3 bardrate as 1Mbps

* [Spinal][Servo][WIP] create a new universal interface to control servos

* [Spinal][Servo] fix complile erors in servo interface (stil not work)

* [Spinal][Servo] add implimentation to call servo interface from main.cpp

* [Spinal][Servo][WIP] modifed some details and add MPU setting

* [Spinal][Servo] add new task to treat servos directly

* [Spinal][Servo] change servo flag name

* [Spinal][Servo][WIP] workaround send command to dynamixel

* [Spinal][Servo] create a new service to set configuration of direct control servos

* [Spinal][Servo] workaround to set configuration of direct servos to flashmemory

* [Spinal][Servo] add interface to set target angle directly

* [Spinal][Servo] fix some bugs

* [Spinal][Servo] change the priority of servotask from idle to realtime

* [Spinal][Servo] fixed bug in a function of toruqe enabling

* [Spinal][Dynamixel] limit the situation in which offset and torque parameter can be set to only during torqe off state.

* [Spinal][Dynamixel] modified maximum servo number from 4 to 8.

* [Spinal][servo] Create README for servo interface

* [Spinal][Servo] resize the servo message as same as maximum servo num.

* [ServoBridge] workaroud to use servo profiles in spinal firmware.

* [Spinal][Servo] use servo index to indentify servo

* [Spinal][Servo] workaround to use rqt to monitor and setup direct servos.

* [Spinal][Servo] debug around reading servo from rqt

* [Spinal][Servo] debug around boad configurator

* [Spinal][Servo] change the port for servo control from usart3 to usart2

* [Spinal][Servo] workaround to use servo and gps simultaneously

* [Spinal][DShot] add telemetry function for the ESCs that support. The code can be flashed into mcu. However, the telemetry has not been tested yet, since the DShot function should be implemented first.

* [Spinal][DShot] add DShot code from https://github.com/mokhwasomssi/stm32_hal_dshot

* [Spinal][DShot] add DMA function for DShot PWM. only set the CubeMX and generate code, not tested.

* [Spinal][DShot]: remove redundant code for esc_telem

* [Spinal][DShot]: create the new dshot class

* [Spinal][DShot]: change the PWM setting in CubeMX for DShot; fix a type error in dshot.cpp; fix a 'static' keyword error and add dma address for STM32H7 in dshot.h. When building, some include errors occur.

* [Spinal][DShot]: try to change pwm to dshot. Not tested.

* [Spinal][DShot]: fix bugs, now the code can be built without errors.

* [Spinal][DShot]: change the memory size for dma buffer. The original size is too small. However, error still occur when accessing the memory.

* [Spinal][DShot]: only one motor can rotate. do not know why

* [Spinal][DShot]: [SUCCESS] change settings to use NPU. NPU is critical for DMA in stm32H7. change a permission setting to allow NPU access. pwm_test works fine on real machine!

* [Spinal][DShot]: add one flag to decide if using telemetry or not.

* [Spinal][DShot]: change the UART for receiving dshot telemetry to RX only.

* [Spinal][DShot]: add msg for telemetry

* [Spinal][DShot]: achieve dshot telemetry of one motor. test using debug. the received data is checked in a clever way.

* [Spinal][DShot]: add code to communicate with ros. tested successfully!

* [Spinal][DShot]: refactor dshot class to triggering one by one. tested successfully!

* [Spinal][DShot]: refactor esc_telem class to measure one by one. Unsuccessful. Still problem

* Revert "[Spinal][DShot]: refactor esc_telem class to measure one by one. Unsuccessful. Still problem"

This reverts commit 699c1d1.

* Revert "[Spinal][DShot]: refactor dshot class to triggering one by one. tested successfully!"

This reverts commit 5d1518e.

* [Spinal][DShot]: add time stamp to ESCTelemetryArray.msg

* [Spinal][DShot]: add esc_reader support for different esc_msgs

* [Spinal][DShot]: send telem flag to different motors. one telem for one motor during one round.  [tested using Oscilloscope]

* [Spinal][DShot]: finish the eRPM measurement of different motors. Tested! Now the freq. is limited to 125hz. 250 hz will cause strange behavior.

* [Spinal][DShot]: polish the dshot code for rotor measurement

* [Spinal][DShot]: add crc to ESCTelemetry.msg for debugging

* [Spinal][DShot]: refactor some names; add constraints for dshot value

* [Spinal][DShot]: add support to reverse rotation directions. tested!

* [Spinal][DShot]: change telemetry info from erpm to real rpm!

* [Spinal]: fix typo

* [Spinal][DShot]: refactor rx_buf_ to esc_telem_rx_, in case of unambiguous

* [Spinal][DShot]: remove code for dshot config, since unsuccessful

* [Spinal][Battery]: use dshot telemetry to update the battery voltage

* [Spinal][Dynamixel] change the Dynamixel reading procedure from send-receive to receive-send. Not working.

* [Spinal][Dynamixel] make the send freq. the same as INST_GET_PRESENT_POS

* [Spinal][Dynamixel] refactor code to support quick reading for Dynamixel. The current freq. for servo is 100hz, and the current freq. for dshot is 125hz. Tested on real machine.

* [Spinal][Dynamixel] fix a bug after cherry-pick

* [Spinal][DShot][WIP] workaround to use dshot in Spinal 4.1

* [Spinal][DShot] workaround to use Dshot and ESC telemetry in Spinal version 4.1

* [Spinal][DShot] modified volatage sensing update procudure

* [Spinal4.1][DShot] restore the procedure of reading esc telem as previous version

* [Spinal][Config][ver4.1] workaround to enable servo control in spinal 4.1

* [Spinal][Config][ver4.1] add roslib build instruction for stm32h7_v2

* [Spinal][Dynamixel] modified servo task priority

* [Spinal][Dynamixel] modified max servo number

* [Spinal][Servo] modified way of gain transmitting to avoid write error

* [Spinal][Servo] debug around the method to distinguish invalid servo ID.

* [Spinal][H7_V1] revert ping uart2 and 3 config as master

* [Spinal][Config] add new flag to distinguish special board version

* [Spinal][DirectServo] choose correct MPU region according to board version.

* [Spinal][DirectServo] dynamically configure Pin setting for servo.

* [Spinal][Config][WIP] create separated config files corresponding to each Spinal board.

* [Spinal][Config][F7] configure CubeIDE project for SpinalF7 based on its own config.

* [Spinal][Config][H7] configure CubeIDE project for SpinalH7 based on its own config.

* [Spinal][Config][H7_v2] configure CubeIDE project for SpinalH7_v2 based on its own config.

* [Spinal][F7] update the way to call magencoder

* [Spinal][H7&H7_V2] modified imu select condition

* [Spinal][config] delete previous common config file.

* [Spinal][Dshot] deleted unused include path.

* [Spinal][Controller] add a dynamic board configurator for the method of thrust signal transmission(PWM or DSHOT).

* [Spinal][Config] add new flag to determine whether Spinal uses dshot or not.

* [Spinal][ACD] Rvert adc channel from 18 to 15 to revive previous voltage sensing way.

* [Spinal][Servo] Debug of infinity loop in sevoTask function when direct servo mode is disenabled.

* [Spinal] Initialize battery instance according to Dshot falg.

* [Spinal] remove the exectuable attribute for config files

* Revert "[Spinal][Encoder] modified encorder interface imitating neuron one"

This reverts commit af3f910.

* Revert "[Spinal][F7] update the way to call magencoder"

This reverts commit 366b3d1.

* [Spinal][Config] add new flag to determine Spinal or Neuron.

* [Spinal][Dynamixel] Add build macro abount the part related to external encoder.

---------

Co-authored-by: Kaneko <[email protected]>
Co-authored-by: hisaaki <[email protected]>
Co-authored-by: LI, Jinjie <[email protected]>
Co-authored-by: Jinjie Li <[email protected]>
Co-authored-by: Moju Zhao <[email protected]>
  • Loading branch information
@sugihara-16 @Li-Jinjie @tongtybj
6 people authored Jan 16, 2025
1 parent d072459 commit d421b9b
Show file tree
Hide file tree
Showing 422 changed files with 313,716 additions and 170 deletions.
5 changes: 5 additions & 0 deletions aerial_robot_model/include/aerial_robot_model/servo_bridge.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,8 @@
#include <spinal/ServoStates.h>
#include <spinal/ServoControlCmd.h>
#include <spinal/ServoTorqueCmd.h>
#include <spinal/JointProfiles.h>
#include <spinal/UavInfo.h>
#include <controller_manager_msgs/LoadController.h>
#include <controller_manager_msgs/SwitchController.h>
#include <urdf/model.h>
Expand Down Expand Up @@ -200,6 +202,8 @@ class ServoBridge

ros::Publisher servo_states_pub_;
ros::Publisher mujoco_control_input_pub_;
ros::Publisher joint_profile_pub_;
ros::Subscriber uav_info_sub_;
map<string, ros::Subscriber> servo_states_subs_;
map<string, ros::Subscriber> servo_ctrl_subs_;
map<string, bool> no_real_state_flags_;
Expand All @@ -219,6 +223,7 @@ class ServoBridge
void servoStatesCallback(const spinal::ServoStatesConstPtr& state_msg, const std::string& servo_group_name);
void servoCtrlCallback(const sensor_msgs::JointStateConstPtr& joints_ctrl_msg, const std::string& servo_group_name);
bool servoTorqueCtrlCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res, const std::string& servo_group_name);
void uavInfoCallback(const spinal::UavInfoConstPtr& uav_msg);

public:
ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp);
Expand Down
35 changes: 33 additions & 2 deletions aerial_robot_model/src/servo_bridge/servo_bridge.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,12 +73,16 @@ ServoBridge::ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh),nhp_(
mujoco_control_input_pub_ = nh_.advertise<sensor_msgs::JointState>("mujoco/ctrl_input", 1);
/* common publisher: torque on/off command */
servo_torque_ctrl_pubs_.insert(make_pair("common", nh_.advertise<spinal::ServoTorqueCmd>(torque_pub_topic, 1)));
/* common publisher: joint profiles */
joint_profile_pub_ = nh_.advertise<spinal::JointProfiles>("joint_profiles",1);
/* subscriber: uav info */
uav_info_sub_ = nh_.subscribe<spinal::UavInfo>("uav_info", 1, &ServoBridge::uavInfoCallback, this);


/* get additional config for servos from ros parameters */
XmlRpc::XmlRpcValue all_servos_params;
nh_.getParam("servo_controller", all_servos_params);

spinal::JointProfiles joint_profiles_msg;
for(auto servo_group_params: all_servos_params)
{
if (servo_group_params.second.getType() != XmlRpc::XmlRpcValue::TypeStruct)
Expand Down Expand Up @@ -126,7 +130,8 @@ ServoBridge::ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh),nhp_(
double lower_limit = urdf_model.getJoint(servo_params.second["name"])->limits->lower;

/* get parameters from rosparam */
int angle_sgn = servo_params.second.hasMember("angle_sgn")?
int servo_id = servo_params.second["id"];
int angle_sgn = servo_params.second.hasMember("angle_sgn")?
servo_params.second["angle_sgn"]:servo_group_params.second["angle_sgn"];
int zero_point_offset = servo_params.second.hasMember("zero_point_offset")?
servo_params.second["zero_point_offset"]:servo_group_params.second["zero_point_offset"];
Expand Down Expand Up @@ -391,3 +396,29 @@ bool ServoBridge::servoTorqueCtrlCallback(std_srvs::SetBool::Request &req, std_s
return true;
}

void ServoBridge::uavInfoCallback(const spinal::UavInfoConstPtr& uav_msg)
{
/* Send servo profiles to Spinal*/
spinal::JointProfiles joint_profiles_msg;
for(auto servo_group : servos_handler_){
for(auto servo : servo_group.second){
spinal::JointProfile joint_profile;
if(servo_group.first == "joints"){
joint_profile.type = spinal::JointProfile::JOINT;
}
else if(servo_group.first == "gimbals"){
joint_profile.type = spinal::JointProfile::GIMBAL;
}
else{
ROS_ERROR("Invalid servo type. Please define 'joints' or 'gimbals'.");
continue;
}
joint_profile.servo_id = servo->getId();
joint_profile.angle_sgn = servo->getAngleSgn();
joint_profile.angle_scale = servo->getAngleScale();
joint_profile.zero_point_offset = servo->getZeroPointOffset();
joint_profiles_msg.joints.push_back(joint_profile);
}
}
joint_profile_pub_.publish(joint_profiles_msg);
}
9 changes: 9 additions & 0 deletions aerial_robot_nerve/spinal/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,10 @@ add_message_files(
FlightConfigCmd.msg
Vector3Int16.msg
TorqueAllocationMatrixInv.msg
ESCTelemetry.msg
ESCTelemetryArray.msg
JointProfile.msg
JointProfiles.msg
)

add_service_files(
Expand All @@ -53,6 +57,7 @@ add_service_files(
SetAttitudeGains.srv
ImuCalib.srv
MagDeclination.srv
SetDirectServoConfig.srv
)

generate_messages(
Expand Down Expand Up @@ -97,6 +102,10 @@ add_dependencies(${PROJECT_NAME}_generate_stm32f7_ros_lib ${catkin_EXPORTED_TARG
add_custom_target(${PROJECT_NAME}_generate_stm32h7_ros_lib ALL COMMAND ${CATKIN_DEVEL_PREFIX}/env.sh ${PYTHON_EXECUTABLE} ${PROJECT_SOURCE_DIR}/scripts/make_libraries.py --save_path ${PROJECT_SOURCE_DIR}/mcu_project/boards/stm32H7 --support_rtos --support_ethernet)
add_dependencies(${PROJECT_NAME}_generate_stm32h7_ros_lib ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)

# generate ros_lib for STM32H7_v2
add_custom_target(${PROJECT_NAME}_generate_stm32h7_v2_ros_lib ALL COMMAND ${CATKIN_DEVEL_PREFIX}/env.sh ${PYTHON_EXECUTABLE} ${PROJECT_SOURCE_DIR}/scripts/make_libraries.py --save_path ${PROJECT_SOURCE_DIR}/mcu_project/boards/stm32H7_v2 --support_rtos --support_ethernet)
add_dependencies(${PROJECT_NAME}_generate_stm32h7_v2_ros_lib ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)


install(DIRECTORY ${SPINAL_DIRS}
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
Expand Down
Loading

0 comments on commit d421b9b

Please sign in to comment.