yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

ROS: Fix formatting

Browse Source
This commit is contained in:
Lorenz Meier
2015-10-19 13:36:00 +02:00
parent d60f201dc8
commit 6c8755106e
7 changed files with 100 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/platforms/ros/geo.cpp
View File

@@ -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;

40
src/platforms/ros/nodes/commander/commander.cpp
View File

@@ -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);

6
src/platforms/ros/nodes/commander/commander.h
View File

@@ -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;

7
src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp
View File

@@ -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;
}

5
src/platforms/ros/nodes/manual_input/manual_input.cpp
View File

@@ -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;

92
src/platforms/ros/nodes/mavlink/mavlink.cpp
View File

@@ -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<std::string>("mavlink_fcu_url", mavlink_fcu_url, std::string("udp://localhost:14565@localhost:14560"));
privateNh.param<std::string>("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);

5
src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
View File

@@ -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];
}