mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-16 09:00:36 +08:00
mavlink: always copy offboard setpoints, not just when control mode changed, also bugfixes in orb calls
This commit is contained in:
Julian Oes
@@ -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
|
||||
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));
|
||||
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;
|
||||
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 (_local_pos_sp_pub < 0) {
|
||||
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp_pub);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &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));
|
||||
|
||||
global_vel_sp.vx = offboard_control_sp.p1;
|
||||
global_vel_sp.vy = offboard_control_sp.p2;
|
||||
global_vel_sp.vz = offboard_control_sp.p3;
|
||||
|
||||
if (_global_vel_sp_pub < 0) {
|
||||
_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp_pub);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &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));
|
||||
|
||||
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;
|
||||
|
||||
att_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
if (_att_sp_pub < 0) {
|
||||
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &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));
|
||||
|
||||
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;
|
||||
|
||||
rates_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
if (_rates_sp_pub < 0) {
|
||||
_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp);
|
||||
}
|
||||
if (_local_pos_sp_pub < 0) {
|
||||
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &loc_pos_sp);
|
||||
|
||||
} else {
|
||||
/* direct control */
|
||||
// TODO
|
||||
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &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));
|
||||
|
||||
global_vel_sp.vx = offboard_control_sp.p1;
|
||||
global_vel_sp.vy = offboard_control_sp.p2;
|
||||
global_vel_sp.vz = offboard_control_sp.p3;
|
||||
|
||||
if (_global_vel_sp_pub < 0) {
|
||||
_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &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));
|
||||
|
||||
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;
|
||||
|
||||
att_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
if (_att_sp_pub < 0) {
|
||||
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &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));
|
||||
|
||||
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;
|
||||
|
||||
rates_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
if (_rates_sp_pub < 0) {
|
||||
_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp);
|
||||
}
|
||||
|
||||
} else {
|
||||
/* direct control */
|
||||
// TODO
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user