Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

vmount: support absolute angles #10525

Merged
merged 6 commits into from
Jan 6, 2020
Merged

vmount: support absolute angles #10525

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
e9aafc1
vmount: first step to support absolute angles
julianoes Sep 19, 2018
1e11d55
vmount: set all DO_MOUNT_CONFIGURE params
julianoes Dec 9, 2019
2cf8ee5
vmount: fix implicit double promotion
julianoes Dec 11, 2019
f747d08
vmount: fix input for test command
julianoes Dec 11, 2019
801ec18
vmount: properly initialize last_active
julianoes Dec 11, 2019
e187491
vmount: fix float casting, simplify loop
julianoes Dec 16, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 7 additions & 2 deletions src/modules/vmount/common.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
40 changes: 34 additions & 6 deletions src/modules/vmount/input_mavlink.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -312,9 +312,37 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
_ack_vehicle_command(&vehicle_command);

} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
_stabilize[0] = (uint8_t) vehicle_command.param2 == 1;
_stabilize[1] = (uint8_t) vehicle_command.param3 == 1;
_stabilize[2] = (uint8_t) vehicle_command.param4 == 1;
_stabilize[0] = (int)(vehicle_command.param2 + 0.5f) == 1;
_stabilize[1] = (int)(vehicle_command.param3 + 0.5f) == 1;
_stabilize[2] = (int)(vehicle_command.param4 + 0.5f) == 1;

const int params[] = {
(int)((float)vehicle_command.param5 + 0.5f),
(int)((float)vehicle_command.param6 + 0.5f),
(int)(vehicle_command.param7 + 0.5f)
};

for (int i = 0; i < 3; ++i) {

if (params[i] == 0) {
_control_data.type_data.angle.frames[i] =
ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;

} else if (params[i] == 1) {
_control_data.type_data.angle.frames[i] =
ControlData::TypeData::TypeAngle::Frame::AngularRate;

} else if (params[i] == 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;
Expand Down
2 changes: 1 addition & 1 deletion src/modules/vmount/input_rc.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
7 changes: 5 additions & 2 deletions src/modules/vmount/input_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.is_speed[i] = false;
_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;
Expand Down
2 changes: 1 addition & 1 deletion src/modules/vmount/output.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
12 changes: 12 additions & 0 deletions src/modules/vmount/output_mavlink.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,10 +71,22 @@ 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]);
}

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);
}

Expand Down
4 changes: 2 additions & 2 deletions src/modules/vmount/vmount.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -220,7 +220,7 @@ static int vmount_thread_main(int argc, char *argv[])
ControlData *control_data = nullptr;
g_thread_data = &thread_data;

int last_active = 0;
int last_active = -1;

while (!thread_should_exit) {

Expand Down Expand Up @@ -395,7 +395,7 @@ static int vmount_thread_main(int argc, char *argv[])

thread_data.input_objs_len = 0;

last_active = 0;
last_active = -1;

if (thread_data.output_obj) {
delete (thread_data.output_obj);
Expand Down