fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 12:10: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/common.h
+7 -2
View File
@@ -61,13 +61,18 @@ struct ControlData {
//TODO: add more, like smooth curve, ... ?
};
Type type = Type::Neutral;
union TypeData {
struct TypeAngle {
float angles[3]; /**< attitude angles (roll, pitch, yaw) in rad, [-pi, +pi] if is_speed[i] == false */
float angles[3]; /**< attitude angles (roll, pitch, yaw) in rad, [-pi, +pi] or rad/s (if is angular rate) */
bool is_speed[3]; /**< if true, the angle is the angular speed in rad/s */
enum class Frame : uint8_t {
AngleBodyFrame = 0, /**< Angle in body frame. */
AngularRate, /**< Angular rate in rad/s. */
AngleAbsoluteFrame /**< Angle in absolute frame. */
} frames[3]; /**< Frame of given angle. */
} angle;
struct TypeLonLat {
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;
src/modules/vmount/input_rc.cpp
+1 -1
View File
@@ -132,7 +132,7 @@ bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bo
_first_time = false;
for (int i = 0; i < 3; ++i) {
control_data.type_data.angle.is_speed[i] = false;
control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
control_data.type_data.angle.angles[i] = new_aux_values[i] * M_PI_F;
control_data.stabilize_axis[i] = _do_stabilization;
src/modules/vmount/input_test.cpp
+1 -1
View File
@@ -66,7 +66,7 @@ int InputTest::update(unsigned int timeout_ms, ControlData **control_data, bool
_control_data.type = ControlData::Type::Angle;
for (int i = 0; i < 3; ++i) {
_control_data.type_data.angle.is_speed[i] = false;
_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;
src/modules/vmount/output.cpp
+1 -1
View File
@@ -103,7 +103,7 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data)
switch (control_data->type) {
case ControlData::Type::Angle:
for (int i = 0; i < 3; ++i) {
if (control_data->type_data.angle.is_speed[i]) {
if (control_data->type_data.angle.frames[i] == ControlData::TypeData::TypeAngle::Frame::AngularRate) {
_angle_speeds[i] = control_data->type_data.angle.angles[i];
} else {