From 6c8755106e4e33b876bb887a18a5e9b571423e71 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:36:00 +0200 Subject: [PATCH] ROS: Fix formatting --- src/platforms/ros/geo.cpp | 15 ++- .../ros/nodes/commander/commander.cpp | 40 ++++---- src/platforms/ros/nodes/commander/commander.h | 6 +- .../demo_offboard_attitude_setpoints.cpp | 7 +- .../ros/nodes/manual_input/manual_input.cpp | 5 +- src/platforms/ros/nodes/mavlink/mavlink.cpp | 92 +++++++++++-------- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 5 + 7 files changed, 100 insertions(+), 70 deletions(-) diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp index 2f277d7f16..d13a5e277a 100644 --- a/src/platforms/ros/geo.cpp +++ b/src/platforms/ros/geo.cpp @@ -182,7 +182,8 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference return ref->timestamp; } -__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, + uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { ref->lat_rad = lat_0 * M_DEG_TO_RAD; @@ -196,12 +197,14 @@ __EXPORT int map_projection_init_timestamped(struct map_projection_reference_s * return 0; } -__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, + double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { return map_projection_init_timestamped(ref, lat_0, lon_0, px4::get_time_micros()); } -__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad) +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, + double *ref_lon_rad) { if (!map_projection_initialized(ref)) { return -1; @@ -213,7 +216,8 @@ __EXPORT int map_projection_reference(const struct map_projection_reference_s *r return 0; } -__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, + float *y) { if (!map_projection_initialized(ref)) { return -1; @@ -235,7 +239,8 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref return 0; } -__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, + double *lon) { if (!map_projection_initialized(ref)) { return -1; diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index 54086cfd4b..38db5dad7f 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -109,45 +109,45 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon } void Commander::SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode, - px4::vehicle_control_mode &msg_vehicle_control_mode) + px4::vehicle_control_mode &msg_vehicle_control_mode) { msg_vehicle_control_mode.flag_control_manual_enabled = false; msg_vehicle_control_mode.flag_control_offboard_enabled = true; msg_vehicle_control_mode.flag_control_auto_enabled = false; msg_vehicle_control_mode.flag_control_rates_enabled = !msg_offboard_control_mode.ignore_bodyrate || - !msg_offboard_control_mode.ignore_attitude || - !msg_offboard_control_mode.ignore_position || - !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_acceleration_force; + !msg_offboard_control_mode.ignore_attitude || + !msg_offboard_control_mode.ignore_position || + !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_acceleration_force; msg_vehicle_control_mode.flag_control_attitude_enabled = !msg_offboard_control_mode.ignore_attitude || - !msg_offboard_control_mode.ignore_position || - !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_acceleration_force; + !msg_offboard_control_mode.ignore_position || + !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_acceleration_force; msg_vehicle_control_mode.flag_control_velocity_enabled = !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_position; + !msg_offboard_control_mode.ignore_position; msg_vehicle_control_mode.flag_control_climb_rate_enabled = !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_position; + !msg_offboard_control_mode.ignore_position; msg_vehicle_control_mode.flag_control_position_enabled = !msg_offboard_control_mode.ignore_position; msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_position; + !msg_offboard_control_mode.ignore_position; } void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, - px4::vehicle_status &msg_vehicle_status, - px4::vehicle_control_mode &msg_vehicle_control_mode) { + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode) +{ // XXX this is a minimal implementation. If more advanced functionalities are // needed consider a full port of the commander - if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON) - { + if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON) { SetOffboardControl(_msg_offboard_control_mode, msg_vehicle_control_mode); return; } @@ -155,7 +155,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, msg_vehicle_control_mode.flag_control_offboard_enabled = false; switch (msg->mode_switch) { - case px4::manual_control_setpoint::SWITCH_POS_NONE: + case px4::manual_control_setpoint::SWITCH_POS_NONE: ROS_WARN("Joystick button mapping error, main mode not set"); break; @@ -183,6 +183,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, msg_vehicle_control_mode.flag_control_climb_rate_enabled = true; msg_vehicle_control_mode.flag_control_position_enabled = true; msg_vehicle_control_mode.flag_control_velocity_enabled = true; + } else { msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_ALTCTL; msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_ALTCTL; @@ -194,6 +195,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, msg_vehicle_control_mode.flag_control_position_enabled = false; msg_vehicle_control_mode.flag_control_velocity_enabled = false; } + break; } @@ -206,20 +208,20 @@ void Commander::OffboardControlModeCallback(const px4::offboard_control_modeCons /* Force system into offboard control mode */ if (!_got_manual_control) { SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode); - + _msg_vehicle_status.timestamp = px4::get_time_micros(); _msg_vehicle_status.hil_state = _msg_vehicle_status.HIL_STATE_OFF; _msg_vehicle_status.hil_state = _msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; _msg_vehicle_status.is_rotary_wing = true; _msg_vehicle_status.arming_state = _msg_vehicle_status.ARMING_STATE_ARMED; - + _msg_actuator_armed.armed = true; _msg_actuator_armed.timestamp = px4::get_time_micros(); _msg_vehicle_control_mode.timestamp = px4::get_time_micros(); _msg_vehicle_control_mode.flag_armed = true; - + _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); _actuator_armed_pub.publish(_msg_actuator_armed); diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index 856144389d..67bee544ab 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -68,14 +68,14 @@ protected: * Set control mode flags based on stick positions (equiv to code in px4 commander) */ void EvalSwitches(const px4::manual_control_setpointConstPtr &msg, - px4::vehicle_status &msg_vehicle_status, - px4::vehicle_control_mode &msg_vehicle_control_mode); + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode); /** * Sets offboard controll flags in msg_vehicle_control_mode */ void SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode, - px4::vehicle_control_mode &msg_vehicle_control_mode); + px4::vehicle_control_mode &msg_vehicle_control_mode); ros::NodeHandle _n; ros::Subscriber _man_ctrl_sp_sub; diff --git a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp index 328f545c6b..7b02a6f07e 100644 --- a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp +++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp @@ -65,15 +65,18 @@ int DemoOffboardAttitudeSetpoints::main() /* Publish example offboard attitude setpoint */ geometry_msgs::PoseStamped pose; - tf::Quaternion q = tf::createQuaternionFromRPY(0.0, 0.1 * (sinf(0.5 * (float)px4::get_time_micros() / 1000000.0f)) , 0.0); + tf::Quaternion q = tf::createQuaternionFromRPY(0.0, 0.1 * (sinf(0.5 * (float)px4::get_time_micros() / 1000000.0f)) , + 0.0); quaternionTFToMsg(q, pose.pose.orientation); _attitude_sp_pub.publish(pose); std_msgs::Float64 thrust; - thrust.data = 0.4f + 0.25 * (sinf((float)px4::get_time_micros() / 1000000.0f)); // just some example throttle input that makes the quad 'jump' + thrust.data = 0.4f + 0.25 * (sinf((float)px4::get_time_micros() / + 1000000.0f)); // just some example throttle input that makes the quad 'jump' _thrust_sp_pub.publish(thrust); } + return 0; } diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 8488c518f5..fc8db220c3 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -116,12 +116,14 @@ void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, do if (val + offset > deadzone || val + offset < -deadzone) { out = (float)((val + offset)) * scale; + } else { out = 0.0f; } } -void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) { +void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) +{ msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; if (_buttons_state[MAIN_STATE_MANUAL] != msg->buttons[_param_buttons_map[MAIN_STATE_MANUAL]] == true) { @@ -150,6 +152,7 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; return; + } else if (_buttons_state[MAIN_STATE_OFFBOARD] != msg->buttons[_param_buttons_map[MAIN_STATE_OFFBOARD]] == true) { msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE; msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 567acc200e..e015266809 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -66,7 +66,8 @@ int main(int argc, char **argv) ros::init(argc, argv, "mavlink"); ros::NodeHandle privateNh("~"); std::string mavlink_fcu_url; - privateNh.param("mavlink_fcu_url", mavlink_fcu_url, std::string("udp://localhost:14565@localhost:14560")); + privateNh.param("mavlink_fcu_url", mavlink_fcu_url, + std::string("udp://localhost:14565@localhost:14560")); Mavlink m(mavlink_fcu_url); ros::spin(); return 0; @@ -76,18 +77,18 @@ void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg) { mavlink_message_t msg_m; mavlink_msg_attitude_quaternion_pack_chan( - _link->get_system_id(), - _link->get_component_id(), - _link->get_channel(), - &msg_m, - get_time_micros() / 1000, - msg->q[0], - msg->q[1], - msg->q[2], - msg->q[3], - msg->rollspeed, - msg->pitchspeed, - msg->yawspeed); + _link->get_system_id(), + _link->get_component_id(), + _link->get_channel(), + &msg_m, + get_time_micros() / 1000, + msg->q[0], + msg->q[1], + msg->q[2], + msg->q[3], + msg->rollspeed, + msg->pitchspeed, + msg->yawspeed); _link->send_message(&msg_m); } @@ -95,33 +96,36 @@ void Mavlink::VehicleLocalPositionCallback(const vehicle_local_positionConstPtr { mavlink_message_t msg_m; mavlink_msg_local_position_ned_pack_chan( - _link->get_system_id(), - _link->get_component_id(), - _link->get_channel(), - &msg_m, - get_time_micros() / 1000, - msg->x, - msg->y, - msg->z, - msg->vx, - msg->vy, - msg->vz); + _link->get_system_id(), + _link->get_component_id(), + _link->get_channel(), + &msg_m, + get_time_micros() / 1000, + msg->x, + msg->y, + msg->z, + msg->vx, + msg->vy, + msg->vz); _link->send_message(&msg_m); } -void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) { +void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) +{ (void)sysid; (void)compid; - switch(mmsg->msgid) { - case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: - handle_msg_set_attitude_target(mmsg); - break; - case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: - handle_msg_set_position_target_local_ned(mmsg); - break; - default: - break; + switch (mmsg->msgid) { + case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: + handle_msg_set_attitude_target(mmsg); + break; + + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + handle_msg_set_position_target_local_ned(mmsg); + break; + + default: + break; } } @@ -146,10 +150,12 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ _offboard_control_mode.ignore_bodyrate = ignore_bodyrate && _offboard_control_mode.ignore_bodyrate; _offboard_control_mode.ignore_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude; + } else { _offboard_control_mode.ignore_bodyrate = ignore_bodyrate; _offboard_control_mode.ignore_attitude = ignore_attitude; } + _offboard_control_mode.ignore_position = true; _offboard_control_mode.ignore_velocity = true; _offboard_control_mode.ignore_acceleration_force = true; @@ -162,13 +168,14 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) */ _att_sp.timestamp = get_time_micros(); + if (!ignore_attitude) { mavlink_quaternion_to_euler(set_attitude_target.q, &_att_sp.roll_body, &_att_sp.pitch_body, - &_att_sp.yaw_body); + &_att_sp.yaw_body); mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])_att_sp.R_body.data()); _att_sp.R_valid = true; } - + if (!_offboard_control_mode.ignore_thrust) { _att_sp.thrust = set_attitude_target.thrust; @@ -178,6 +185,7 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) for (ssize_t i = 0; i < 4; i++) { _att_sp.q_d[i] = set_attitude_target.q[i]; } + _att_sp.q_d_valid = true; } @@ -200,9 +208,9 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * /* Only accept messages which are intended for this system */ // XXX removed for sitl, makes maybe sense to re-introduce at some point // if ((mavlink_system.sysid == set_position_target_local_ned.target_system || - // set_position_target_local_ned.target_system == 0) && - // (mavlink_system.compid == set_position_target_local_ned.target_component || - // set_position_target_local_ned.target_component == 0)) { + // set_position_target_local_ned.target_system == 0) && + // (mavlink_system.compid == set_position_target_local_ned.target_component || + // set_position_target_local_ned.target_component == 0)) { /* convert mavlink type (local, NED) to uORB offboard control struct */ offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7); @@ -223,7 +231,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * * gets published only if in offboard mode. We leave that out for now. */ if (is_force_sp && offboard_control_mode.ignore_position && - offboard_control_mode.ignore_velocity) { + offboard_control_mode.ignore_velocity) { /* The offboard setpoint is a force setpoint only, directly writing to the force * setpoint topic and not publishing the setpoint triplet topic */ vehicle_force_setpoint force_sp; @@ -233,6 +241,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * //XXX: yaw _force_sp_pub.publish(force_sp); + } else { /* It's not a pure force setpoint: publish to setpoint triplet topic */ position_setpoint_triplet pos_sp_triplet; @@ -247,6 +256,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * pos_sp_triplet.current.x = set_position_target_local_ned.x; pos_sp_triplet.current.y = set_position_target_local_ned.y; pos_sp_triplet.current.z = set_position_target_local_ned.z; + } else { pos_sp_triplet.current.position_valid = false; } @@ -257,6 +267,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * pos_sp_triplet.current.vx = set_position_target_local_ned.vx; pos_sp_triplet.current.vy = set_position_target_local_ned.vy; pos_sp_triplet.current.vz = set_position_target_local_ned.vz; + } else { pos_sp_triplet.current.velocity_valid = false; } @@ -292,6 +303,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * } else { pos_sp_triplet.current.yawspeed_valid = false; } + //XXX handle global pos setpoints (different MAV frames) _pos_sp_triplet_pub.publish(pos_sp_triplet); diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 002a112b69..67084c64bb 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -221,14 +221,19 @@ void MultirotorMixer::actuatorControlsCallback(const px4::actuator_controls_0 &m // switch mixer if necessary std::string mixer_name; _n.getParamCached("mixer", mixer_name); + if (mixer_name == "x") { _rotors = _config_index[0]; + } else if (mixer_name == "+") { _rotors = _config_index[1]; + } else if (mixer_name == "e") { _rotors = _config_index[2]; + } else if (mixer_name == "w") { _rotors = _config_index[3]; + } else if (mixer_name == "i") { _rotors = _config_index[4]; }