From 7a69b858508f300430d458559ca1136f1f16f740 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 11 Dec 2019 15:42:40 +0100 Subject: [PATCH] vmount: fix input for test command It is most convenient to use a yaw angle relative to body for testing. Also, we set stabilize_axis to true for testing. --- src/modules/vmount/input_test.cpp | 7 +++++-- src/modules/vmount/output_mavlink.cpp | 19 +++++++++++-------- 2 files changed, 16 insertions(+), 10 deletions(-) diff --git a/src/modules/vmount/input_test.cpp b/src/modules/vmount/input_test.cpp index 16b30cddae..2654b8500e 100644 --- a/src/modules/vmount/input_test.cpp +++ b/src/modules/vmount/input_test.cpp @@ -65,11 +65,14 @@ int InputTest::update(unsigned int timeout_ms, ControlData **control_data, bool _control_data.type = ControlData::Type::Angle; + _control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; + _control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; + _control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + for (int i = 0; i < 3; ++i) { - _control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; _control_data.type_data.angle.angles[i] = _angles[i] * M_DEG_TO_RAD_F; - _control_data.stabilize_axis[i] = false; + _control_data.stabilize_axis[i] = true; } _control_data.gimbal_shutter_retract = false; diff --git a/src/modules/vmount/output_mavlink.cpp b/src/modules/vmount/output_mavlink.cpp index f4f179381b..9babd00497 100644 --- a/src/modules/vmount/output_mavlink.cpp +++ b/src/modules/vmount/output_mavlink.cpp @@ -71,18 +71,21 @@ int OutputMavlink::update(const ControlData *control_data) if (control_data->type == ControlData::Type::Neutral) { vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL; + 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(control_data->type_data.angle.frames[0]); + vehicle_command.param6 = static_cast(control_data->type_data.angle.frames[1]); + vehicle_command.param7 = static_cast(control_data->type_data.angle.frames[2]); } - // We assume that a MAVLink gimbal does the stabilization. - vehicle_command.param2 = 0.0f; - vehicle_command.param3 = 0.0f; - vehicle_command.param4 = 0.0f; - - vehicle_command.param5 = static_cast(control_data->type_data.angle.frames[0]); - vehicle_command.param6 = static_cast(control_data->type_data.angle.frames[1]); - vehicle_command.param7 = static_cast(control_data->type_data.angle.frames[2]); + vehicle_command.param2 = control_data->stabilize_axis[0]; + vehicle_command.param3 = control_data->stabilize_axis[1]; + vehicle_command.param4 = control_data->stabilize_axis[2]; _vehicle_command_pub.publish(vehicle_command); }