diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h index ed2b7e7f6f..f7c1bed8e6 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h @@ -79,7 +79,7 @@ typedef enum MAV_CMD MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_MOTOR_TEST=209, /* Mission command to perform motor test |motor sequence number (a number from 1 to max number of motors on the vehicle)| throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)| throttle| timeout (in seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_INVERTED_FLIGHT=210, /* Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty| */ MAV_CMD_DO_GUIDED_MASTER=221, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_GUIDED_LIMITS=222, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, WGS84) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, WGS84) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ @@ -162,13 +162,13 @@ typedef enum MOTOR_TEST_THROTTLE_TYPE #define HAVE_ENUM_CAMERA_STATUS_TYPES typedef enum CAMERA_STATUS_TYPES { - HEARTBEAT=0, /* Camera heartbeat, announce camera component ID at 1hz | */ - TRIGGER=1, /* Camera image triggered | */ - DISCONNECT=2, /* Camera connection lost | */ - ERROR=3, /* Camera unknown error | */ - LOWBATT=4, /* Camera battery low. Parameter p1 shows reported voltage | */ - LOWSTORE=5, /* Camera storage low. Parameter p1 shows reported shots remaining | */ - LOWSTOREV=6, /* Camera storage low. Parameter p1 shows reported video minutes remaining | */ + CAMERA_STATUS_TYPE_HEARTBEAT=0, /* Camera heartbeat, announce camera component ID at 1hz | */ + CAMERA_STATUS_TYPE_TRIGGER=1, /* Camera image triggered | */ + CAMERA_STATUS_TYPE_DISCONNECT=2, /* Camera connection lost | */ + CAMERA_STATUS_TYPE_ERROR=3, /* Camera unknown error | */ + CAMERA_STATUS_TYPE_LOWBATT=4, /* Camera battery low. Parameter p1 shows reported voltage | */ + CAMERA_STATUS_TYPE_LOWSTORE=5, /* Camera storage low. Parameter p1 shows reported shots remaining | */ + CAMERA_STATUS_TYPE_LOWSTOREV=6, /* Camera storage low. Parameter p1 shows reported video minutes remaining | */ CAMERA_STATUS_TYPES_ENUM_END=7, /* | */ } CAMERA_STATUS_TYPES; #endif diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/version.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/version.h index cf34271644..6f1843a48a 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/version.h +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Fri Jul 25 11:24:10 2014" +#define MAVLINK_BUILD_DATE "Thu Aug 7 12:21:20 2014" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h index 3b97b40d0d..35170a71ab 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h @@ -5,10 +5,10 @@ typedef struct __mavlink_attitude_quaternion_t { uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - float q1; ///< Quaternion component 1 - float q2; ///< Quaternion component 2 - float q3; ///< Quaternion component 3 - float q4; ///< Quaternion component 4 + float q1; ///< Quaternion component 1, w (1 in null-rotation) + float q2; ///< Quaternion component 2, x (0 in null-rotation) + float q3; ///< Quaternion component 3, y (0 in null-rotation) + float q4; ///< Quaternion component 4, z (0 in null-rotation) float rollspeed; ///< Roll angular speed (rad/s) float pitchspeed; ///< Pitch angular speed (rad/s) float yawspeed; ///< Yaw angular speed (rad/s) @@ -44,10 +44,10 @@ typedef struct __mavlink_attitude_quaternion_t * @param msg The MAVLink message to compress the data into * * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param q1 Quaternion component 1 - * @param q2 Quaternion component 2 - * @param q3 Quaternion component 3 - * @param q4 Quaternion component 4 + * @param q1 Quaternion component 1, w (1 in null-rotation) + * @param q2 Quaternion component 2, x (0 in null-rotation) + * @param q3 Quaternion component 3, y (0 in null-rotation) + * @param q4 Quaternion component 4, z (0 in null-rotation) * @param rollspeed Roll angular speed (rad/s) * @param pitchspeed Pitch angular speed (rad/s) * @param yawspeed Yaw angular speed (rad/s) @@ -97,10 +97,10 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, u * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param q1 Quaternion component 1 - * @param q2 Quaternion component 2 - * @param q3 Quaternion component 3 - * @param q4 Quaternion component 4 + * @param q1 Quaternion component 1, w (1 in null-rotation) + * @param q2 Quaternion component 2, x (0 in null-rotation) + * @param q3 Quaternion component 3, y (0 in null-rotation) + * @param q4 Quaternion component 4, z (0 in null-rotation) * @param rollspeed Roll angular speed (rad/s) * @param pitchspeed Pitch angular speed (rad/s) * @param yawspeed Yaw angular speed (rad/s) @@ -176,10 +176,10 @@ static inline uint16_t mavlink_msg_attitude_quaternion_encode_chan(uint8_t syste * @param chan MAVLink channel to send the message * * @param time_boot_ms Timestamp (milliseconds since system boot) - * @param q1 Quaternion component 1 - * @param q2 Quaternion component 2 - * @param q3 Quaternion component 3 - * @param q4 Quaternion component 4 + * @param q1 Quaternion component 1, w (1 in null-rotation) + * @param q2 Quaternion component 2, x (0 in null-rotation) + * @param q3 Quaternion component 3, y (0 in null-rotation) + * @param q4 Quaternion component 4, z (0 in null-rotation) * @param rollspeed Roll angular speed (rad/s) * @param pitchspeed Pitch angular speed (rad/s) * @param yawspeed Yaw angular speed (rad/s) @@ -287,7 +287,7 @@ static inline uint32_t mavlink_msg_attitude_quaternion_get_time_boot_ms(const ma /** * @brief Get field q1 from attitude_quaternion message * - * @return Quaternion component 1 + * @return Quaternion component 1, w (1 in null-rotation) */ static inline float mavlink_msg_attitude_quaternion_get_q1(const mavlink_message_t* msg) { @@ -297,7 +297,7 @@ static inline float mavlink_msg_attitude_quaternion_get_q1(const mavlink_message /** * @brief Get field q2 from attitude_quaternion message * - * @return Quaternion component 2 + * @return Quaternion component 2, x (0 in null-rotation) */ static inline float mavlink_msg_attitude_quaternion_get_q2(const mavlink_message_t* msg) { @@ -307,7 +307,7 @@ static inline float mavlink_msg_attitude_quaternion_get_q2(const mavlink_message /** * @brief Get field q3 from attitude_quaternion message * - * @return Quaternion component 3 + * @return Quaternion component 3, y (0 in null-rotation) */ static inline float mavlink_msg_attitude_quaternion_get_q3(const mavlink_message_t* msg) { @@ -317,7 +317,7 @@ static inline float mavlink_msg_attitude_quaternion_get_q3(const mavlink_message /** * @brief Get field q4 from attitude_quaternion message * - * @return Quaternion component 4 + * @return Quaternion component 4, z (0 in null-rotation) */ static inline float mavlink_msg_attitude_quaternion_get_q4(const mavlink_message_t* msg) { diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h index 1a028bdf99..344a19e183 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h @@ -5,7 +5,7 @@ typedef struct __mavlink_hil_state_quaternion_t { uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float attitude_quaternion[4]; ///< Vehicle attitude expressed as normalized quaternion + float attitude_quaternion[4]; ///< Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation) float rollspeed; ///< Body frame roll / phi angular speed (rad/s) float pitchspeed; ///< Body frame pitch / theta angular speed (rad/s) float yawspeed; ///< Body frame yaw / psi angular speed (rad/s) @@ -60,7 +60,7 @@ typedef struct __mavlink_hil_state_quaternion_t * @param msg The MAVLink message to compress the data into * * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion + * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation) * @param rollspeed Body frame roll / phi angular speed (rad/s) * @param pitchspeed Body frame pitch / theta angular speed (rad/s) * @param yawspeed Body frame yaw / psi angular speed (rad/s) @@ -135,7 +135,7 @@ static inline uint16_t mavlink_msg_hil_state_quaternion_pack(uint8_t system_id, * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion + * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation) * @param rollspeed Body frame roll / phi angular speed (rad/s) * @param pitchspeed Body frame pitch / theta angular speed (rad/s) * @param yawspeed Body frame yaw / psi angular speed (rad/s) @@ -236,7 +236,7 @@ static inline uint16_t mavlink_msg_hil_state_quaternion_encode_chan(uint8_t syst * @param chan MAVLink channel to send the message * * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion + * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation) * @param rollspeed Body frame roll / phi angular speed (rad/s) * @param pitchspeed Body frame pitch / theta angular speed (rad/s) * @param yawspeed Body frame yaw / psi angular speed (rad/s) @@ -383,7 +383,7 @@ static inline uint64_t mavlink_msg_hil_state_quaternion_get_time_usec(const mavl /** * @brief Get field attitude_quaternion from hil_state_quaternion message * - * @return Vehicle attitude expressed as normalized quaternion + * @return Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation) */ static inline uint16_t mavlink_msg_hil_state_quaternion_get_attitude_quaternion(const mavlink_message_t* msg, float *attitude_quaternion) { diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h index db3a926424..c47fc8c0c7 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h @@ -4,10 +4,10 @@ typedef struct __mavlink_sim_state_t { - float q1; ///< True attitude quaternion component 1 - float q2; ///< True attitude quaternion component 2 - float q3; ///< True attitude quaternion component 3 - float q4; ///< True attitude quaternion component 4 + float q1; ///< True attitude quaternion component 1, w (1 in null-rotation) + float q2; ///< True attitude quaternion component 2, x (0 in null-rotation) + float q3; ///< True attitude quaternion component 3, y (0 in null-rotation) + float q4; ///< True attitude quaternion component 4, z (0 in null-rotation) float roll; ///< Attitude roll expressed as Euler angles, not recommended except for human-readable outputs float pitch; ///< Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs float yaw; ///< Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs @@ -69,10 +69,10 @@ typedef struct __mavlink_sim_state_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param q1 True attitude quaternion component 1 - * @param q2 True attitude quaternion component 2 - * @param q3 True attitude quaternion component 3 - * @param q4 True attitude quaternion component 4 + * @param q1 True attitude quaternion component 1, w (1 in null-rotation) + * @param q2 True attitude quaternion component 2, x (0 in null-rotation) + * @param q3 True attitude quaternion component 3, y (0 in null-rotation) + * @param q4 True attitude quaternion component 4, z (0 in null-rotation) * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs @@ -161,10 +161,10 @@ static inline uint16_t mavlink_msg_sim_state_pack(uint8_t system_id, uint8_t com * @param component_id ID of this component (e.g. 200 for IMU) * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into - * @param q1 True attitude quaternion component 1 - * @param q2 True attitude quaternion component 2 - * @param q3 True attitude quaternion component 3 - * @param q4 True attitude quaternion component 4 + * @param q1 True attitude quaternion component 1, w (1 in null-rotation) + * @param q2 True attitude quaternion component 2, x (0 in null-rotation) + * @param q3 True attitude quaternion component 3, y (0 in null-rotation) + * @param q4 True attitude quaternion component 4, z (0 in null-rotation) * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs @@ -279,10 +279,10 @@ static inline uint16_t mavlink_msg_sim_state_encode_chan(uint8_t system_id, uint * @brief Send a sim_state message * @param chan MAVLink channel to send the message * - * @param q1 True attitude quaternion component 1 - * @param q2 True attitude quaternion component 2 - * @param q3 True attitude quaternion component 3 - * @param q4 True attitude quaternion component 4 + * @param q1 True attitude quaternion component 1, w (1 in null-rotation) + * @param q2 True attitude quaternion component 2, x (0 in null-rotation) + * @param q3 True attitude quaternion component 3, y (0 in null-rotation) + * @param q4 True attitude quaternion component 4, z (0 in null-rotation) * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs @@ -446,7 +446,7 @@ static inline void mavlink_msg_sim_state_send_buf(mavlink_message_t *msgbuf, mav /** * @brief Get field q1 from sim_state message * - * @return True attitude quaternion component 1 + * @return True attitude quaternion component 1, w (1 in null-rotation) */ static inline float mavlink_msg_sim_state_get_q1(const mavlink_message_t* msg) { @@ -456,7 +456,7 @@ static inline float mavlink_msg_sim_state_get_q1(const mavlink_message_t* msg) /** * @brief Get field q2 from sim_state message * - * @return True attitude quaternion component 2 + * @return True attitude quaternion component 2, x (0 in null-rotation) */ static inline float mavlink_msg_sim_state_get_q2(const mavlink_message_t* msg) { @@ -466,7 +466,7 @@ static inline float mavlink_msg_sim_state_get_q2(const mavlink_message_t* msg) /** * @brief Get field q3 from sim_state message * - * @return True attitude quaternion component 3 + * @return True attitude quaternion component 3, y (0 in null-rotation) */ static inline float mavlink_msg_sim_state_get_q3(const mavlink_message_t* msg) { @@ -476,7 +476,7 @@ static inline float mavlink_msg_sim_state_get_q3(const mavlink_message_t* msg) /** * @brief Get field q4 from sim_state message * - * @return True attitude quaternion component 4 + * @return True attitude quaternion component 4, z (0 in null-rotation) */ static inline float mavlink_msg_sim_state_get_q4(const mavlink_message_t* msg) { diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/common/version.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/common/version.h index 89f3d08287..b234661ce9 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/common/version.h +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/common/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Fri Jul 25 11:24:10 2014" +#define MAVLINK_BUILD_DATE "Thu Aug 7 12:21:20 2014" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/mavlink_conversions.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/mavlink_conversions.h index a5da98ab12..beac630a3d 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/mavlink_conversions.h +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/mavlink_conversions.h @@ -27,6 +27,13 @@ * @author James Goppert */ + +/** + * Converts a quaternion to a rotation matrix + * + * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) + * @param dcm a 3x3 rotation matrix + */ MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float dcm[3][3]) { double a = quaternion[0]; @@ -48,6 +55,15 @@ MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float d dcm[2][2] = aSq - bSq - cSq + dSq; } + +/** + * Converts a rotation matrix to euler angles + * + * @param dcm a 3x3 rotation matrix + * @param roll the roll angle in radians + * @param pitch the pitch angle in radians + * @param yaw the yaw angle in radians + */ MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, float* pitch, float* yaw) { float phi, theta, psi; @@ -73,6 +89,15 @@ MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, flo *yaw = psi; } + +/** + * Converts a quaternion to euler angles + * + * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) + * @param roll the roll angle in radians + * @param pitch the pitch angle in radians + * @param yaw the yaw angle in radians + */ MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float* roll, float* pitch, float* yaw) { float dcm[3][3]; @@ -80,6 +105,15 @@ MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float mavlink_dcm_to_euler((const float(*)[3])dcm, roll, pitch, yaw); } + +/** + * Converts euler angles to a quaternion + * + * @param roll the roll angle in radians + * @param pitch the pitch angle in radians + * @param yaw the yaw angle in radians + * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) + */ MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4]) { double cosPhi_2 = cos((double)roll / 2.0); @@ -98,6 +132,13 @@ MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float y sinPhi_2 * sinTheta_2 * cosPsi_2); } + +/** + * Converts a rotation matrix to a quaternion + * + * @param dcm a 3x3 rotation matrix + * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0) + */ MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4]) { quaternion[0] = (0.5 * sqrt(1.0 + @@ -110,6 +151,15 @@ MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quate (double)(-dcm[0][0] - dcm[1][1] + dcm[2][2]))); } + +/** + * Converts euler angles to a rotation matrix + * + * @param roll the roll angle in radians + * @param pitch the pitch angle in radians + * @param yaw the yaw angle in radians + * @param dcm a 3x3 rotation matrix + */ MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3]) { double cosPhi = cos(roll);