fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 03:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

vmount mavlink input: continue to poll for vehicle_command until reaching the timeout

Browse Source 
or receiving a command that we need to handle. This makes sure that we
don't drop commands, when the output mode is set to mavlink (in that case
the input topic is rate-limited, and each input change results in a
vehicle_command publication, which results in queueing up orb items if we
get vehicle_commands from other sources too).
This commit is contained in:
Beat Küng
2018-01-21 15:41:30 +01:00
committed by Lorenz Meier
parent 5a189f3c4a
commit 99c9e09624
1 changed files with 97 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/vmount/input_mavlink.cpp
+97 -80
View File
@@ -221,92 +221,109 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
polls[0].fd = _vehicle_command_sub;
polls[0].events = POLLIN;
int ret = px4_poll(polls, num_poll, timeout_ms);
int poll_timeout = (int)timeout_ms;
if (ret < 0) {
return -errno;
}
bool exit_loop = false;
if (ret == 0) {
// Timeout control_data already null.
while (!exit_loop && poll_timeout >= 0) {
hrt_abstime poll_start = hrt_absolute_time();
} else {
if (polls[0].revents & POLLIN) {
vehicle_command_s vehicle_command;
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command);
int ret = px4_poll(polls, num_poll, poll_timeout);
// Process only if the command is for us or for anyone (component id 0).
const bool sysid_correct = (vehicle_command.target_system == _mav_sys_id);
const bool compid_correct = ((vehicle_command.target_component == _mav_comp_id) ||
(vehicle_command.target_component == 0));
if (!sysid_correct || !compid_correct) {
return 0;
}
for (int i = 0; i < 3; ++i) {
_control_data.stabilize_axis[i] = _stabilize[i];
}
_control_data.gimbal_shutter_retract = false;
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) {
switch ((int)vehicle_command.param7) {
case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
_control_data.gimbal_shutter_retract = true;
/* FALLTHROUGH */
case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
_control_data.type = ControlData::Type::Neutral;
*control_data = &_control_data;
break;
case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
_control_data.type = ControlData::Type::Angle;
_control_data.type_data.angle.is_speed[0] = false;
_control_data.type_data.angle.is_speed[1] = false;
_control_data.type_data.angle.is_speed[2] = false;
// vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0
_control_data.type_data.angle.angles[0] = vehicle_command.param2 * M_DEG_TO_RAD_F;
// vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1
_control_data.type_data.angle.angles[1] = vehicle_command.param1 * M_DEG_TO_RAD_F;
// both specs have yaw on channel 2
_control_data.type_data.angle.angles[2] = vehicle_command.param3 * M_DEG_TO_RAD_F;
// We expect angle of [-pi..+pi]. If the input range is [0..2pi] we can fix that.
if (_control_data.type_data.angle.angles[2] > M_PI_F) {
_control_data.type_data.angle.angles[2] -= 2 * M_PI_F;
}
*control_data = &_control_data;
break;
case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
break;
case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
control_data_set_lon_lat((double)vehicle_command.param2, (double)vehicle_command.param1, vehicle_command.param3);
*control_data = &_control_data;
break;
}
_ack_vehicle_command(&vehicle_command);
} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
_stabilize[0] = (uint8_t) vehicle_command.param2 == 1;
_stabilize[1] = (uint8_t) vehicle_command.param3 == 1;
_stabilize[2] = (uint8_t) vehicle_command.param4 == 1;
_control_data.type = ControlData::Type::Neutral; //always switch to neutral position
*control_data = &_control_data;
_ack_vehicle_command(&vehicle_command);
}
if (ret < 0) {
return -errno;
}
poll_timeout -= (hrt_absolute_time() - poll_start) / 1000;
// if we get a command that we need to handle, we exit the loop, otherwise we poll until we reach the timeout
exit_loop = true;
if (ret == 0) {
// Timeout control_data already null.
} else {
if (polls[0].revents & POLLIN) {
vehicle_command_s vehicle_command;
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command);
// Process only if the command is for us or for anyone (component id 0).
const bool sysid_correct = (vehicle_command.target_system == _mav_sys_id);
const bool compid_correct = ((vehicle_command.target_component == _mav_comp_id) ||
(vehicle_command.target_component == 0));
if (!sysid_correct || !compid_correct) {
exit_loop = false;
continue;
}
for (int i = 0; i < 3; ++i) {
_control_data.stabilize_axis[i] = _stabilize[i];
}
_control_data.gimbal_shutter_retract = false;
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) {
switch ((int)vehicle_command.param7) {
case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
_control_data.gimbal_shutter_retract = true;
/* FALLTHROUGH */
case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
_control_data.type = ControlData::Type::Neutral;
*control_data = &_control_data;
break;
case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
_control_data.type = ControlData::Type::Angle;
_control_data.type_data.angle.is_speed[0] = false;
_control_data.type_data.angle.is_speed[1] = false;
_control_data.type_data.angle.is_speed[2] = false;
// vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0
_control_data.type_data.angle.angles[0] = vehicle_command.param2 * M_DEG_TO_RAD_F;
// vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1
_control_data.type_data.angle.angles[1] = vehicle_command.param1 * M_DEG_TO_RAD_F;
// both specs have yaw on channel 2
_control_data.type_data.angle.angles[2] = vehicle_command.param3 * M_DEG_TO_RAD_F;
// We expect angle of [-pi..+pi]. If the input range is [0..2pi] we can fix that.
if (_control_data.type_data.angle.angles[2] > M_PI_F) {
_control_data.type_data.angle.angles[2] -= 2 * M_PI_F;
}
*control_data = &_control_data;
break;
case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
break;
case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
control_data_set_lon_lat((double)vehicle_command.param2, (double)vehicle_command.param1, vehicle_command.param3);
*control_data = &_control_data;
break;
}
_ack_vehicle_command(&vehicle_command);
} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
_stabilize[0] = (uint8_t) vehicle_command.param2 == 1;
_stabilize[1] = (uint8_t) vehicle_command.param3 == 1;
_stabilize[2] = (uint8_t) vehicle_command.param4 == 1;
_control_data.type = ControlData::Type::Neutral; //always switch to neutral position
*control_data = &_control_data;
_ack_vehicle_command(&vehicle_command);
} else {
exit_loop = false;
}
}
}
}
return 0;