zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

mavlink: always copy offboard setpoints, not just when control mode changed, also bugfixes in orb calls

sbg
Julian Oes 11 years ago
parent
c99683752c
commit
f5699e6490
1 changed files with 61 additions and 62 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 123
      src/modules/mavlink/mavlink_receiver.cpp

123
src/modules/mavlink/mavlink_receiver.cpp
Unescape View File

@ -407,11 +407,10 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message @@ -407,11 +407,10 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
/*Temporary hack to use set_quad_swarm_roll_pitch_yaw_thrust msg for position or velocity */
/* Converts INT16 centimeters to float meters */
offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / 100.0f;
offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / 100.0f;
offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / 100.0f;
offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / 100.0f;
offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / 1000.0f;
offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / 1000.0f;
offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / 1000.0f;
offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / 1000.0f;
}
if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
@ -434,83 +433,83 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message @@ -434,83 +433,83 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
orb_check(_control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
}
if (_control_mode.flag_control_offboard_enabled) {
if (_control_mode.flag_control_position_enabled) {
// TODO Use something else then quad_swarm_roll_pitch_yaw_thrust
struct vehicle_local_position_setpoint_s loc_pos_sp;
memset(&loc_pos_sp, 0, sizeof(loc_pos_sp));
loc_pos_sp.x = offboard_control_sp.p1;
loc_pos_sp.y = offboard_control_sp.p2;
loc_pos_sp.yaw = offboard_control_sp.p3;
loc_pos_sp.z = -offboard_control_sp.p4;
if (_control_mode.flag_control_offboard_enabled) {
if (_control_mode.flag_control_position_enabled) {
// TODO Use something else then quad_swarm_roll_pitch_yaw_thrust
struct vehicle_local_position_setpoint_s loc_pos_sp;
memset(&loc_pos_sp, 0, sizeof(loc_pos_sp));
if (_local_pos_sp_pub < 0) {
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp_pub);
loc_pos_sp.x = offboard_control_sp.p1;
loc_pos_sp.y = offboard_control_sp.p2;
loc_pos_sp.yaw = offboard_control_sp.p3;
loc_pos_sp.z = -offboard_control_sp.p4;
} else {
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &loc_pos_sp);
}
if (_local_pos_sp_pub < 0) {
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &loc_pos_sp);
} else if (_control_mode.flag_control_velocity_enabled) {
/* velocity control */
struct vehicle_global_velocity_setpoint_s global_vel_sp;
memset(&global_vel_sp, 0, sizeof(&global_vel_sp));
} else {
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &loc_pos_sp);
}
global_vel_sp.vx = offboard_control_sp.p1;
global_vel_sp.vy = offboard_control_sp.p2;
global_vel_sp.vz = offboard_control_sp.p3;
} else if (_control_mode.flag_control_velocity_enabled) {
/* velocity control */
struct vehicle_global_velocity_setpoint_s global_vel_sp;
memset(&global_vel_sp, 0, sizeof(&global_vel_sp));
if (_global_vel_sp_pub < 0) {
_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp_pub);
global_vel_sp.vx = offboard_control_sp.p1;
global_vel_sp.vy = offboard_control_sp.p2;
global_vel_sp.vz = offboard_control_sp.p3;
} else {
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &global_vel_sp);
}
if (_global_vel_sp_pub < 0) {
_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
} else if (_control_mode.flag_control_attitude_enabled) {
/* attitude control */
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
} else {
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &global_vel_sp);
}
att_sp.roll_body = offboard_control_sp.p1;
att_sp.pitch_body = offboard_control_sp.p2;
att_sp.yaw_body = offboard_control_sp.p3;
att_sp.thrust = offboard_control_sp.p4;
} else if (_control_mode.flag_control_attitude_enabled) {
/* attitude control */
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
att_sp.timestamp = hrt_absolute_time();
att_sp.roll_body = offboard_control_sp.p1;
att_sp.pitch_body = offboard_control_sp.p2;
att_sp.yaw_body = offboard_control_sp.p3;
att_sp.thrust = offboard_control_sp.p4;
if (_att_sp_pub < 0) {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
att_sp.timestamp = hrt_absolute_time();
} else {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp);
}
if (_att_sp_pub < 0) {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
} else if (_control_mode.flag_control_rates_enabled) {
/* rates control */
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
} else {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp);
}
rates_sp.roll = offboard_control_sp.p1;
rates_sp.pitch = offboard_control_sp.p2;
rates_sp.yaw = offboard_control_sp.p3;
rates_sp.thrust = offboard_control_sp.p4;
} else if (_control_mode.flag_control_rates_enabled) {
/* rates control */
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
rates_sp.timestamp = hrt_absolute_time();
rates_sp.roll = offboard_control_sp.p1;
rates_sp.pitch = offboard_control_sp.p2;
rates_sp.yaw = offboard_control_sp.p3;
rates_sp.thrust = offboard_control_sp.p4;
if (_rates_sp_pub < 0) {
_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
rates_sp.timestamp = hrt_absolute_time();
} else {
orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp);
}
if (_rates_sp_pub < 0) {
_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
} else {
/* direct control */
// TODO
orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp);
}
} else {
/* direct control */
// TODO
}
}
}

Write Preview
Loading…
Cancel
Save