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

vmount: fix input for test command

Browse Source 
It is most convenient to use a yaw angle relative to body for testing.
Also, we set stabilize_axis to true for testing.
This commit is contained in:
Julian Oes
2020-01-06 16:24:36 +01:00
committed by Beat Küng
parent 4753e02364
commit 7a69b85850
2 changed files with 16 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vmount/input_test.cpp
+5 -2
View File
@@ -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;
src/modules/vmount/output_mavlink.cpp
+11 -8
View File
@@ -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<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]);
}
// 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<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 = 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);
}