mavlink MOUNT_ORIENTATION use math::degrees

This commit is contained in:
Daniel Agar 2018-07-13 09:13:18 -04:00 committed by Lorenz Meier
parent f26c3ac014
commit 1ea63e4955

View File

@ -1061,7 +1061,6 @@ protected:
actuator_armed_s armed = {};
airspeed_s airspeed = {};
bool updated = false;
updated |= _pos_sub->update(&_pos_time, &pos);
updated |= _armed_sub->update(&_armed_time, &armed);
@ -4068,9 +4067,9 @@ protected:
mavlink_mount_orientation_t msg = {};
msg.roll = 180.0f / M_PI_F * mount_orientation.attitude_euler_angle[0];
msg.pitch = 180.0f / M_PI_F * mount_orientation.attitude_euler_angle[1];
msg.yaw = 180.0f / M_PI_F * mount_orientation.attitude_euler_angle[2];
msg.roll = math::degrees(mount_orientation.attitude_euler_angle[0]);
msg.pitch = math::degrees(mount_orientation.attitude_euler_angle[1]);
msg.yaw = math::degrees(mount_orientation.attitude_euler_angle[2]);
mavlink_msg_mount_orientation_send_struct(_mavlink->get_channel(), &msg);