From 89aeea7e8e31b3c9e6e8aef04d97d6af2087fca3 Mon Sep 17 00:00:00 2001 From: Nicolas de Palezieux Date: Thu, 10 Aug 2017 13:45:29 +0200 Subject: [PATCH] vmount: publish mount angle commands in proper order and units --- src/drivers/vmount/output_mavlink.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/drivers/vmount/output_mavlink.cpp b/src/drivers/vmount/output_mavlink.cpp index 3c30bc3895..9897a29da7 100644 --- a/src/drivers/vmount/output_mavlink.cpp +++ b/src/drivers/vmount/output_mavlink.cpp @@ -104,9 +104,11 @@ int OutputMavlink::update(const ControlData *control_data) vehicle_command.timestamp = t; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL; - vehicle_command.param1 = _angle_outputs[0]; - vehicle_command.param2 = _angle_outputs[1]; - vehicle_command.param3 = _angle_outputs[2]; + // vmount spec has roll, pitch on channels 0, 1, respectively; MAVLink spec has roll, pitch on channels 1, 0, respectively + // vmount uses radians, MAVLink uses degrees + vehicle_command.param1 = _angle_outputs[1] * M_RAD_TO_DEG_F; + vehicle_command.param2 = _angle_outputs[0] * M_RAD_TO_DEG_F; + vehicle_command.param3 = _angle_outputs[2] * M_RAD_TO_DEG_F; orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command);