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

vmount: only send CONFIGURE cmd if type changes

Browse Source 
We don't always have to send MOUNT_CONFIGURE but really only when the
control data type actually changes.
This commit is contained in:
Julian Oes
2022-01-10 23:04:10 -05:00
committed by Daniel Agar
parent 22180a2639
commit 73044c51f9
2 changed files with 27 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vmount/output_mavlink.cpp
+25 -22
View File
@@ -66,33 +66,36 @@ int OutputMavlinkV1::update(const ControlData *control_data)
//got new command
_set_angle_setpoints(control_data);
#if !defined(ALT_MANTIS_GIMBAL_HACKS)
// Don't send this command to ATL Mantis as it just spams the vehicle_command queue and the
// Mantis ignores it anyway.
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE;
vehicle_command.timestamp = hrt_absolute_time();
const bool configuration_changed =
(control_data->type != _previous_control_data_type);
_previous_control_data_type = control_data->type;
if (control_data->type == ControlData::Type::Neutral) {
vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
if (configuration_changed) {
vehicle_command.param5 = 0.0;
vehicle_command.param6 = 0.0;
vehicle_command.param7 = 0.0f;
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE;
vehicle_command.timestamp = hrt_absolute_time();
} else {
vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
if (control_data->type == ControlData::Type::Neutral) {
vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
vehicle_command.param5 = static_cast<double>(control_data->type_data.angle.frames[0]);
vehicle_command.param6 = static_cast<double>(control_data->type_data.angle.frames[1]);
vehicle_command.param7 = static_cast<float>(control_data->type_data.angle.frames[2]);
vehicle_command.param5 = 0.0;
vehicle_command.param6 = 0.0;
vehicle_command.param7 = 0.0f;
} else {
vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
vehicle_command.param5 = static_cast<double>(control_data->type_data.angle.frames[0]);
vehicle_command.param6 = static_cast<double>(control_data->type_data.angle.frames[1]);
vehicle_command.param7 = static_cast<float>(control_data->type_data.angle.frames[2]);
}
vehicle_command.param2 = _stabilize[0] ? 1.0f : 0.0f;
vehicle_command.param3 = _stabilize[1] ? 1.0f : 0.0f;
vehicle_command.param4 = _stabilize[2] ? 1.0f : 0.0f;
_vehicle_command_pub.publish(vehicle_command);
}
vehicle_command.param2 = _stabilize[0] ? 1.0f : 0.0f;
vehicle_command.param3 = _stabilize[1] ? 1.0f : 0.0f;
vehicle_command.param4 = _stabilize[2] ? 1.0f : 0.0f;
_vehicle_command_pub.publish(vehicle_command);
#endif
}
_handle_position_update();
src/modules/vmount/output_mavlink.h
+2
View File
@@ -68,6 +68,8 @@ private:
void _stream_device_attitude_status();
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
uORB::Publication <gimbal_device_attitude_status_s> _attitude_status_pub{ORB_ID(gimbal_device_attitude_status)};
ControlData::Type _previous_control_data_type {ControlData::Type::Neutral};
};
class OutputMavlinkV2 : public OutputBase