diff --git a/aerial_robot_base/bin/install_igmp_report.sh b/aerial_robot_base/bin/install_igmp_report.sh new file mode 100755 index 000000000..67d303f0e --- /dev/null +++ b/aerial_robot_base/bin/install_igmp_report.sh @@ -0,0 +1,23 @@ +#!/bin/bash + +# Define the path for the temporary file +TEMP_CRON_FILE=$(mktemp) + +# Copy python file to the home directory +FILE_NAME="send_igmp_report.py" +cp `rospack find aerial_robot_base`/scripts/$FILE_NAME $HOME/$FILE_NAME + +# Retrieve the current crontab and save it to the temporary file +sudo crontab -l 2>/dev/null > "$TEMP_CRON_FILE" + +# Add a new job +CRON_COMMAND="*/2 * * * * /usr/bin/python3 $HOME/$FILE_NAME >> /var/log/force_send_GIMP_report.log 2>&1" +grep -qxF "$CRON_COMMAND" "$TEMP_CRON_FILE" || echo "$CRON_COMMAND" >> "$TEMP_CRON_FILE" + +# Update the crontab +sudo crontab "$TEMP_CRON_FILE" + +# Delete the temporary file +rm -f "$TEMP_CRON_FILE" + +echo "Crontab has been updated." diff --git a/aerial_robot_base/scripts/send_igmp_report.py b/aerial_robot_base/scripts/send_igmp_report.py new file mode 100755 index 000000000..0283b8b7c --- /dev/null +++ b/aerial_robot_base/scripts/send_igmp_report.py @@ -0,0 +1,72 @@ +import socket +import struct +import netifaces +import datetime + +def get_wifi_ip(): + # Get the default gateway interface + gateway_interface = netifaces.gateways().get('default') + if gateway_interface: + gateway_interface_name = gateway_interface[2][-1] + + # Get the IP address of the Wi-Fi interface + wifi_ip = netifaces.ifaddresses(gateway_interface_name).get(netifaces.AF_INET) + if wifi_ip: + return wifi_ip[0]['addr'] + return None # Return None if Wi-Fi interface is not found + +def send_igmp_v2_membership_report(group_ip, interface_ip): + # IGMPv2 Membership Report packet format + # Type (1 byte), Max Resp Time (1 byte), Checksum (2 bytes), Group Address (4 bytes) + + IGMP_MEMBERSHIP_REPORT_TYPE = 0x16 # IGMPv2 Membership Report + MAX_RESP_TIME = 0 # Not used in Membership Report + CHECKSUM_PLACEHOLDER = 0 + + # Convert group IP to bytes + group_bytes = socket.inet_aton(group_ip) + + # Create IGMP packet with a placeholder checksum + igmp_packet = struct.pack('!BBH4s', + IGMP_MEMBERSHIP_REPORT_TYPE, + MAX_RESP_TIME, + CHECKSUM_PLACEHOLDER, + group_bytes) + + # Calculate checksum + def calculate_checksum(data): + if len(data) % 2: + data += b'\x00' + checksum = sum(struct.unpack('!%dH' % (len(data) // 2), data)) + checksum = (checksum >> 16) + (checksum & 0xFFFF) + checksum += checksum >> 16 + return ~checksum & 0xFFFF + + checksum = calculate_checksum(igmp_packet) + + # Rebuild the packet with the correct checksum + igmp_packet = struct.pack('!BBH4s', + IGMP_MEMBERSHIP_REPORT_TYPE, + MAX_RESP_TIME, + checksum, + group_bytes) + + # Create a raw socket + sock = socket.socket(socket.AF_INET, socket.SOCK_RAW, socket.IPPROTO_IGMP) + + # Bind to the interface + sock.setsockopt(socket.SOL_IP, socket.IP_MULTICAST_IF, socket.inet_aton(interface_ip)) + + # Send the IGMP packet + sock.sendto(igmp_packet, (group_ip, 0)) + dt_now = datetime.datetime.now() + print("[{}] IGMPv2 Membership Report sent to {} from {}".format(dt_now, group_ip, interface_ip)) + +# Example usage +group_ip = "239.255.42.99" # Multicast group address +interface_ip = get_wifi_ip() # Automatically detect the IP address of the Wi-Fi interface +if interface_ip: + send_igmp_v2_membership_report(group_ip, interface_ip) +else: + dt_now = datetime.datetime.now() + print("[{}] Wi-Fi interface not found.".format(dt_now)) diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp index 4f198071d..187f4cb4d 100644 --- a/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp @@ -57,6 +57,7 @@ class PolynomialTrajectory : public ReferenceBase { PolyType y_; PolyType z_; PolyType yaw_; + QuadState prev_constraint_; std::vector states_; diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp index 5fe268808..1e2910339 100644 --- a/aerial_robot_control/src/flight_navigation.cpp +++ b/aerial_robot_control/src/flight_navigation.cpp @@ -696,57 +696,60 @@ void BaseNavigator::update() /* update the target pos and velocity */ if (trajectory_mode_) { - if (traj_generator_ptr_.get() == nullptr) + if(getNaviState() == HOVER_STATE) { - if (ros::Time::now().toSec() > trajectory_reset_time_) + if (traj_generator_ptr_.get() == nullptr) { - setTargetZeroVel(); - setTargetZeroAcc(); + if (ros::Time::now().toSec() > trajectory_reset_time_) + { + setTargetZeroVel(); + setTargetZeroAcc(); - setTargetZeroOmega(); - setTargetZeroAngAcc(); + setTargetZeroOmega(); + setTargetZeroAngAcc(); - trajectory_mode_ = false; + trajectory_mode_ = false; - ROS_INFO("[Flight nav] stop trajectory mode in POS-VEL mode"); - } - } - else - { - // trajectory following mode - double t = ros::Time::now().toSec(); - double end_t = traj_generator_ptr_->getEndSetpoint().state.t; - if (t > end_t) - { - ROS_INFO("[Nav] reach the end of trajectory"); - - setTargetZeroVel(); - setTargetZeroAcc(); - - setTargetZeroOmega(); - setTargetZeroAngAcc(); - - trajectory_mode_ = false; - - traj_generator_ptr_.reset(); + ROS_INFO("[Flight nav] stop trajectory mode in POS-VEL mode"); + } } else { - agi::QuadState target_state = traj_generator_ptr_->getState(t); - setTargetPos(tf::Vector3(target_state.p(0), target_state.p(1), target_state.p(2))); - setTargetVel(tf::Vector3(target_state.v(0), target_state.v(1), target_state.v(2))); - setTargetAcc(tf::Vector3(target_state.a(0), target_state.a(1), target_state.a(2))); - - double target_yaw = target_state.getYaw(); - double target_omega_z = target_state.w(2); - double target_ang_acc_z = target_state.tau(2); - setTargetYaw(target_yaw); - setTargetOmegaZ(target_omega_z); - setTargetAngAccZ(target_ang_acc_z); - - tf::Vector3 curr_pos = estimator_->getPos(Frame::COG, estimate_mode_); - double yaw_angle = estimator_->getState(State::YAW_COG, estimate_mode_)[0]; - ROS_INFO_THROTTLE(0.5, "[Nav] trajectory mode, target pos&yaw: [%f, %f, %f, %f], curr pos&yaw: [%f, %f, %f, %f]", target_state.p(0), target_state.p(1), target_state.p(2), target_yaw, curr_pos.x(), curr_pos.y(), curr_pos.z(), yaw_angle); + // trajectory following mode + double t = ros::Time::now().toSec(); + double end_t = traj_generator_ptr_->getEndSetpoint().state.t; + if (t > end_t) + { + ROS_INFO("[Nav] reach the end of trajectory"); + + setTargetZeroVel(); + setTargetZeroAcc(); + + setTargetZeroOmega(); + setTargetZeroAngAcc(); + + trajectory_mode_ = false; + + traj_generator_ptr_.reset(); + } + else + { + agi::QuadState target_state = traj_generator_ptr_->getState(t); + setTargetPos(tf::Vector3(target_state.p(0), target_state.p(1), target_state.p(2))); + setTargetVel(tf::Vector3(target_state.v(0), target_state.v(1), target_state.v(2))); + setTargetAcc(tf::Vector3(target_state.a(0), target_state.a(1), target_state.a(2))); + + double target_yaw = target_state.getYaw(); + double target_omega_z = target_state.w(2); + double target_ang_acc_z = target_state.tau(2); + setTargetYaw(target_yaw); + setTargetOmegaZ(target_omega_z); + setTargetAngAccZ(target_ang_acc_z); + + tf::Vector3 curr_pos = estimator_->getPos(Frame::COG, estimate_mode_); + double yaw_angle = estimator_->getState(State::YAW_COG, estimate_mode_)[0]; + ROS_INFO_THROTTLE(0.5, "[Nav] trajectory mode, target pos&yaw: [%f, %f, %f, %f], curr pos&yaw: [%f, %f, %f, %f]", target_state.p(0), target_state.p(1), target_state.p(2), target_yaw, curr_pos.x(), curr_pos.y(), curr_pos.z(), yaw_angle); + } } } } @@ -1012,7 +1015,10 @@ void BaseNavigator::generateNewTrajectory(geometry_msgs::PoseStamped pose) } double du_tran = (end_state.p - start_state.p).norm() / trajectory_mean_vel_; - double du_rot = fabs(end_state.getYaw() - start_state.getYaw()) / trajectory_mean_yaw_rate_; + double delta_yaw = end_state.getYaw() - start_state.getYaw(); + if (delta_yaw > M_PI) delta_yaw -= 2 * M_PI; + if (delta_yaw < -M_PI) delta_yaw += 2 * M_PI; + double du_rot = fabs(delta_yaw) / trajectory_mean_yaw_rate_; double du = std::max(du_tran, trajectory_min_du_); if (!enable_latch_yaw_trajectory_) { diff --git a/aerial_robot_control/src/trajectory/trajectory_reference/polynomial_trajectory.cpp b/aerial_robot_control/src/trajectory/trajectory_reference/polynomial_trajectory.cpp index 2b1112ef3..92af636bd 100644 --- a/aerial_robot_control/src/trajectory/trajectory_reference/polynomial_trajectory.cpp +++ b/aerial_robot_control/src/trajectory/trajectory_reference/polynomial_trajectory.cpp @@ -75,12 +75,10 @@ PolynomialTrajectory::PolynomialTrajectory( yaw_.scale(start_state_.t, duration_); addStateConstraint(start_state_); - addStateConstraint(end_state_); - for (size_t i = 1; i < states_.size() - 1; i++) { addStateConstraint(states_.at(i), 0); // only consider the position constraints for the intermediate points } - + addStateConstraint(end_state_); if (!x_.solve()) std::cout << "Could not solve x-axis!" << std::endl; if (!y_.solve()) std::cout << "Could not solve y-axis!" << std::endl; @@ -116,9 +114,21 @@ bool PolynomialTrajectory::addStateConstraint(const QuadState& state, z_.addConstraint(state.t, constraints.row(2).transpose()); // ToDo: Yaw relative to initial yaw. - const Scalar yaw_angle = state.getYaw(); + Scalar yaw_angle = state.getYaw(); if (std::isfinite(yaw_angle)) - yaw_.addConstraint(state.t, Vector<3>(yaw_angle, state.w.z(), 0)); + { + if (std::isfinite(prev_constraint_.t)) + { + Scalar prev_yaw_angle = prev_constraint_.getYaw(); + Scalar diff = yaw_angle - prev_yaw_angle; + if (diff > M_PI) yaw_angle -= 2 * M_PI; + if (diff < -M_PI) yaw_angle += 2 * M_PI; + } + yaw_.addConstraint(state.t, Vector<3>(yaw_angle, state.w.z(), 0)); + } + + + prev_constraint_ = state; return true; } diff --git a/aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp b/aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp index e5425f915..47d7549c5 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp +++ b/aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp @@ -62,6 +62,7 @@ void Initializer::receiveDataCallback(uint8_t message_id, uint32_t DLC, uint8_t* case CAN::MESSAGEID_RECEIVE_INITIAL_CONFIG_REQUEST: { sendBoardConfig(); + servo_.setConnect(true); } break; case CAN::MESSAGEID_RECEIVE_BOARD_CONFIG_REQUEST: diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp b/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp index 56ea7ed65..4839fee9c 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp @@ -272,6 +272,21 @@ void DynamixelSerial::update() } else { instruction_buffer_.push(std::make_pair(INST_GET_HARDWARE_ERROR_STATUS, 0)); } + + // check the latest error status + for (unsigned int i = 0; i < servo_num_; i++) { + if (servo_[i].hardware_error_status_ != 0) { + servo_[i].force_servo_off_ = true; + + if (servo_[i].torque_enable_) { + servo_[i].torque_enable_= false; + setTorque(i); // servo off + } + } + else { + servo_[i].force_servo_off_ = false; + } + } } diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.h b/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.h index 50aeff4e3..87a7f77d7 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.h +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.h @@ -278,7 +278,14 @@ class RingBuffer class ServoData { public: ServoData(){} - ServoData(uint8_t id): id_(id), torque_enable_(false), first_get_pos_flag_(true), internal_offset_(0){} + ServoData(uint8_t id): + id_(id), + internal_offset_(0), + hardware_error_status_(0), + torque_enable_(false), + force_servo_off_(false), + first_get_pos_flag_(true) + {} uint8_t id_; int32_t present_position_; @@ -301,6 +308,7 @@ class ServoData { float resolution_ratio_; bool led_; bool torque_enable_; + bool force_servo_off_; bool first_get_pos_flag_; void updateHomingOffset() { homing_offset_ = calib_value_ - present_position_;} diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp index 63fa3b5cd..575c30478 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp @@ -11,6 +11,7 @@ void Servo::init(UART_HandleTypeDef* huart, I2C_HandleTypeDef* hi2c, osMutexId* mutex = NULL) { servo_handler_.init(huart, hi2c, mutex); + connect_ = false; } void Servo::update() @@ -26,6 +27,7 @@ void Servo::sendData() CANServoData data(static_cast(s.getPresentPosition()), s.present_temp_, s.moving_, + !connect_ || s.force_servo_off_, s.present_current_, s.hardware_error_status_); sendMessage(CAN::MESSAGEID_SEND_SERVO_LIST[i], m_slave_id, 8, reinterpret_cast(&data), 1); @@ -35,6 +37,8 @@ void Servo::sendData() void Servo::receiveDataCallback(uint8_t message_id, uint32_t DLC, uint8_t* data) { + if (!connect_) return; + switch (message_id) { case CAN::MESSAGEID_RECEIVE_SERVO_ANGLE: { @@ -48,10 +52,12 @@ void Servo::receiveDataCallback(uint8_t message_id, uint32_t DLC, uint8_t* data) } s.setGoalPosition(goal_pos); bool torque_enable = (((data[i * 2 + 1] >> 7) & 0x01) != 0) ? true : false; - if (s.torque_enable_ != torque_enable) { - s.torque_enable_ = torque_enable; - servo_handler_.setTorque(i); - } + if (!s.force_servo_off_) { + if (s.torque_enable_ != torque_enable) { + s.torque_enable_ = torque_enable; + servo_handler_.setTorque(i); + } + } } break; } diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h index eabd39e46..29cba4257 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h @@ -24,17 +24,24 @@ class Servo : public CANDevice void sendData() override; void receiveDataCallback(uint8_t message_id, uint32_t DLC, uint8_t* data) override; + bool getConnect() {return connect_;} + void setConnect(bool connect) {connect_ = connect;} + private: struct CANServoData{ int16_t angle; uint8_t temperature; - uint8_t moving; + uint8_t status; int16_t current; uint8_t error; - CANServoData(uint16_t angle, uint8_t temperature, uint8_t moving, int16_t current, uint8_t error) - :angle(angle), temperature(temperature), moving(moving), current(current), error(error){} + CANServoData(uint16_t angle, uint8_t temperature, uint8_t moving, bool force_servo_disable, int16_t current, uint8_t error) + :angle(angle), temperature(temperature), current(current), error(error) + { + status = (force_servo_disable? 1 : 0) | moving << 1; + } }; + bool connect_; DynamixelSerial servo_handler_; friend class Initializer; }; diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.cpp index c4f1e8af6..7c8816dc6 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.cpp @@ -9,7 +9,16 @@ void CANInitializer::sendData() { - return; + // reconnection with reboot neuron + if (reboot_id_ > 0) { + + // wait for the neuron completely reboot, 5000ms + if (HAL_GetTick() - reboot_time_ > 5000) { + requestConfig(reboot_id_); + reboot_time_ = 0; + reboot_id_ = 0; + } + } } void CANInitializer::initDevices() @@ -18,11 +27,17 @@ void CANInitializer::initDevices() HAL_Delay(200); std::sort(neuron_.begin(), neuron_.end()); for (unsigned int i = 0; i < neuron_.size(); i++) { - sendMessage(CAN::MESSAGEID_RECEIVE_INITIAL_CONFIG_REQUEST, neuron_.at(i).getSlaveId(), 0, nullptr, 1); + uint8_t slave_id = neuron_.at(i).getSlaveId(); + requestConfig(slave_id); HAL_Delay(200); } } +void CANInitializer::requestConfig(uint8_t slave_id) +{ + sendMessage(CAN::MESSAGEID_RECEIVE_INITIAL_CONFIG_REQUEST, slave_id, 0, nullptr, 1); +} + void CANInitializer::configDevice(const spinal::SetBoardConfig::Request& req) { uint8_t slave_id = static_cast(req.data[0]); @@ -144,6 +159,8 @@ void CANInitializer::configDevice(const spinal::SetBoardConfig::Request& req) uint8_t send_data[1]; send_data[0] = CAN::BOARD_CONFIG_REBOOT; sendMessage(CAN::MESSAGEID_RECEIVE_BOARD_CONFIG_REQUEST, slave_id, 1, send_data, 1); + reboot_id_ = slave_id; + reboot_time_ = HAL_GetTick(); break; } case spinal::SetBoardConfig::Request::SET_DYNAMIXEL_TTL_RS485_MIXED: diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.h b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.h index 70b17278e..864ad0941 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.h +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.h @@ -18,9 +18,13 @@ class CANInitializer : public CANDevice { private: std::vector& neuron_; + uint8_t reboot_id_; + uint32_t reboot_time_; + public: - CANInitializer(std::vector& neuron):CANDevice(CAN::DEVICEID_INITIALIZER, CAN::MASTER_ID), neuron_(neuron){} + CANInitializer(std::vector& neuron):CANDevice(CAN::DEVICEID_INITIALIZER, CAN::MASTER_ID), neuron_(neuron), reboot_id_(0), reboot_time_(0){} void initDevices(); + void requestConfig(uint8_t slave_id); void configDevice(const spinal::SetBoardConfig::Request& req); void sendData() override; void receiveDataCallback(uint8_t slave_id, uint8_t message_id, uint32_t DLC, uint8_t* data) override; diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.cpp index bfaf9b0a1..d0989306c 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.cpp @@ -7,10 +7,11 @@ #include "can_servo.h" -bool Servo::getTorqueEnable() +void Servo::setTorqueEnable(bool torque_enable) { - if(error_ != 0) torque_enable_ = false; - return torque_enable_; + if (force_servo_off_) return; + + torque_enable_ = torque_enable; } void CANServo::sendData() @@ -27,7 +28,12 @@ void CANServo::receiveDataCallback(uint8_t slave_id, uint8_t message_id, uint32_ CANServoData servo_data = *(reinterpret_cast(data)); servo_[message_id].present_position_ = servo_data.angle; servo_[message_id].present_temperature_ = servo_data.temperature; - servo_[message_id].moving_ = servo_data.moving; + servo_[message_id].moving_ = (servo_data.status >> 1) & 0x01 ; + servo_[message_id].force_servo_off_ = ((servo_data.status & 0x01) != 0)? true: false; servo_[message_id].present_current_ = servo_data.current; servo_[message_id].error_ = servo_data.error; + + if (servo_[message_id].force_servo_off_) { + servo_[message_id].torque_enable_ = false; + } } diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.h b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.h index 060dbcb77..6f9ff5ec0 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.h +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.h @@ -26,6 +26,7 @@ class Servo uint8_t error_; bool send_data_flag_; bool torque_enable_; + bool force_servo_off_; bool external_encoder_flag_; uint16_t joint_resolution_; uint16_t servo_resolution_; @@ -50,10 +51,11 @@ class Servo uint8_t getPresentTemperature() const {return present_temperature_;} uint8_t getMoving() const {return moving_;} uint8_t getError() const {return error_;} - bool getTorqueEnable(); + bool getTorqueEnable() const {return torque_enable_;} + bool getForceServoOffFlag() const {return force_servo_off_;} void setIndex(uint8_t index) {index_ = index;} void setGoalPosition(int16_t goal_position) {goal_position_ = goal_position;} - void setTorqueEnable(bool torque_enable) {torque_enable_ = torque_enable;} + void setTorqueEnable(bool torque_enable); }; class CANServo : public CANDevice @@ -73,7 +75,7 @@ class CANServo : public CANDevice struct CANServoData{ int16_t angle; uint8_t temperature; - uint8_t moving; + uint8_t status; int16_t current; uint8_t error; }; diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/Spine/spine.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/Spine/spine.cpp index fafcf577f..8410f4ecf 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/Spine/spine.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/Spine/spine.cpp @@ -228,6 +228,8 @@ namespace Spine if (send_board_index == slave_num_) send_board_index = 0; } } + + can_initializer_.sendData(); // if necessary } void update(void) diff --git a/robots/ninja/urdf/ninja.urdf.xacro b/robots/ninja/urdf/ninja.urdf.xacro index ae8ecee73..7a7b9dc2a 100644 --- a/robots/ninja/urdf/ninja.urdf.xacro +++ b/robots/ninja/urdf/ninja.urdf.xacro @@ -123,12 +123,12 @@ - - + + + ixx = "0.00047120" ixy = "-0.00000696" ixz = "-0.00002605" + iyx = "-0.00000696" iyy = "0.00089048" iyz = "0.00000963" + izx = "-0.00002605" izy = "0.00000963" izz = "0.00082655"/> @@ -326,12 +326,12 @@ - - + + + ixx = "0.00035938" ixy = "-0.00001594" ixz = "0.00000012" + iyx = "-0.00001594" iyy = "0.00038919" iyz = "0.00006568" + izx = "0.00000012" izy = "0.00006568" izz = "0.00032884"/>