diff --git a/src/modules/vmount/common.h b/src/modules/vmount/common.h index 956a639269..a1b3a6eece 100644 --- a/src/modules/vmount/common.h +++ b/src/modules/vmount/common.h @@ -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 { diff --git a/src/modules/vmount/input_mavlink.cpp b/src/modules/vmount/input_mavlink.cpp index 4b38dba3f8..5ae8c4a29d 100644 --- a/src/modules/vmount/input_mavlink.cpp +++ b/src/modules/vmount/input_mavlink.cpp @@ -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; diff --git a/src/modules/vmount/input_rc.cpp b/src/modules/vmount/input_rc.cpp index 702ca6ea36..b917914807 100644 --- a/src/modules/vmount/input_rc.cpp +++ b/src/modules/vmount/input_rc.cpp @@ -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; diff --git a/src/modules/vmount/input_test.cpp b/src/modules/vmount/input_test.cpp index 1acac19625..16b30cddae 100644 --- a/src/modules/vmount/input_test.cpp +++ b/src/modules/vmount/input_test.cpp @@ -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; diff --git a/src/modules/vmount/output.cpp b/src/modules/vmount/output.cpp index 5ad4bf8a25..29816e2d68 100644 --- a/src/modules/vmount/output.cpp +++ b/src/modules/vmount/output.cpp @@ -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 {