Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

cleanup

Browse Source
This commit is contained in:
Roman Bapst
2014-12-30 11:41:28 +01:00
parent fa1f09b850
commit bfc3984426
1 changed files with 15 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
+11 -19
View File
@@ -48,13 +48,11 @@ public:
MultirotorMixer(); MultirotorMixer();
struct Rotor { struct Rotor {
float roll_scale; float roll_scale;
float pitch_scale; float pitch_scale;
float yaw_scale; float yaw_scale;
};
};
void mix();
void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg); void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg);
private: private:
@@ -75,21 +73,18 @@ private:
float control[6]; float control[6];
}outputs; }outputs;
void mix();
}; };
const MultirotorMixer::Rotor _config_x[] = { const MultirotorMixer::Rotor _config_x[] = {
{ 0.000000, -1.000000, -1.00 }, { 0.000000, -1.000000, -1.00 },
{ -0.000000, 1.000000, -1.00 }, { -0.000000, 1.000000, -1.00 },
{ 1.000000, 0.000000, 1.00 }, { 1.000000, 0.000000, 1.00 },
{ -1.000000, 0.000000, 1.00 }, { -1.000000, 0.000000, 1.00 },
}; };
const MultirotorMixer::Rotor *_config_index = { &_config_x[0] const MultirotorMixer::Rotor *_config_index = { &_config_x[0]
}; };
MultirotorMixer::MultirotorMixer(): MultirotorMixer::MultirotorMixer():
@@ -97,10 +92,9 @@ MultirotorMixer::MultirotorMixer():
_rotors(_config_index) _rotors(_config_index)
{ {
_sub = _n.subscribe("actuator_controls_0", 1000, &MultirotorMixer::actuatorControlsCallback,this); _sub = _n.subscribe("actuator_controls_0", 1000, &MultirotorMixer::actuatorControlsCallback,this);
_pub = _n.advertise<mav_msgs::MotorSpeed>("mixed_motor_commands",10); _pub = _n.advertise<mav_msgs::MotorSpeed>("mixed_motor_commands",10);
} }
void MultirotorMixer::mix() { void MultirotorMixer::mix() {
float roll = math::constrain(inputs.control[0], -1.0f, 1.0f); float roll = math::constrain(inputs.control[0], -1.0f, 1.0f);
float pitch = math::constrain(inputs.control[1], -1.0f, 1.0f); float pitch = math::constrain(inputs.control[1], -1.0f, 1.0f);
@@ -164,7 +158,7 @@ void MultirotorMixer::mix() {
void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg) void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg)
{ {
// read message // read message
for(int i = 0;i < msg.NUM_ACTUATOR_CONTROLS;i++) { for(int i = 0;i < msg.NUM_ACTUATOR_CONTROLS;i++) {
inputs.control[i] = msg.control[i]; inputs.control[i] = msg.control[i];
} }
@@ -175,17 +169,15 @@ void MultirotorMixer::mix() {
// publish message // publish message
mav_msgs::MotorSpeed rotor_vel_msg; mav_msgs::MotorSpeed rotor_vel_msg;
for (int i = 0; i < _rotor_count; i++) { for (int i = 0; i < _rotor_count; i++) {
rotor_vel_msg.motor_speed.push_back(outputs.control[i]); rotor_vel_msg.motor_speed.push_back(outputs.control[i]);
} }
_pub.publish(rotor_vel_msg); _pub.publish(rotor_vel_msg);
} }
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
ros::init(argc, argv, "mc_mixer"); ros::init(argc, argv, "mc_mixer");
MultirotorMixer mixer; MultirotorMixer mixer;
ros::spin(); ros::spin();
return 0; return 0;