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

vmount: first step to support absolute angles

Browse Source 
The mavlink spec now supports absolute angles as well, see:
https://github.com/mavlink/mavlink/pull/944
This commit is contained in:
Julian Oes
2018-09-19 14:30:22 +02:00
committed by Beat Küng
parent 1484dfed6a
commit 5836edc586
5 changed files with 46 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vmount/input_mavlink.cpp
+36 -3
View File
@@ -281,9 +281,9 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
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;
_control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
_control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
// 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
@@ -315,6 +315,39 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
_stabilize[0] = (uint8_t) vehicle_command.param2 == 1;
_stabilize[1] = (uint8_t) vehicle_command.param3 == 1;
_stabilize[2] = (uint8_t) vehicle_command.param4 == 1;
for (int i = 0; i < 3; ++i) {
float param;
if (i == 0) {
param = vehicle_command.param5;
} else if (i == 1) {
param = vehicle_command.param6;
} else {
param = vehicle_command.param7;
}
if (int(param) == 0) {
_control_data.type_data.angle.frames[i] =
ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
} else if (int(param) == 1) {
_control_data.type_data.angle.frames[i] =
ControlData::TypeData::TypeAngle::Frame::AngularRate;
} else if (int(param) == 2) {
_control_data.type_data.angle.frames[i] =
ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
} else {
// Not supported, fallback to body angle.
_control_data.type_data.angle.frames[i] =
ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
}
}
_control_data.type = ControlData::Type::Neutral; //always switch to neutral position
*control_data = &_control_data;