├── msg ├── Corrections.msg ├── Gains.msg ├── StatusData.msg ├── TRPYCommand.msg ├── AuxCommand.msg ├── SO3Command.msg ├── Odometry.msg ├── OutputData.msg ├── PPROutputData.msg ├── PositionCommand.msg~ ├── Serial.msg ├── PositionCommand.msg └── PolynomialTrajectory.msg ├── include └── quadrotor_msgs │ ├── encode_msgs.h │ ├── decode_msgs.h │ └── comm_types.h ├── package.xml ├── src ├── encode_msgs.cpp └── decode_msgs.cpp └── CMakeLists.txt /msg/Corrections.msg: -------------------------------------------------------------------------------- 1 | float64 kf_correction 2 | float64[2] angle_corrections 3 | -------------------------------------------------------------------------------- /msg/Gains.msg: -------------------------------------------------------------------------------- 1 | float64 Kp 2 | float64 Kd 3 | float64 Kp_yaw 4 | float64 Kd_yaw 5 | -------------------------------------------------------------------------------- /msg/StatusData.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint16 loop_rate 3 | float64 voltage 4 | uint8 seq 5 | -------------------------------------------------------------------------------- /msg/TRPYCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 thrust 3 | float32 roll 4 | float32 pitch 5 | float32 yaw 6 | quadrotor_msgs/AuxCommand aux 7 | -------------------------------------------------------------------------------- /msg/AuxCommand.msg: -------------------------------------------------------------------------------- 1 | float64 current_yaw 2 | float64 kf_correction 3 | float64[2] angle_corrections# Trims for roll, pitch 4 | bool enable_motors 5 | bool use_external_yaw 6 | -------------------------------------------------------------------------------- /msg/SO3Command.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Vector3 force 3 | geometry_msgs/Quaternion orientation 4 | float64[3] kR 5 | float64[3] kOm 6 | quadrotor_msgs/AuxCommand aux 7 | -------------------------------------------------------------------------------- /msg/Odometry.msg: -------------------------------------------------------------------------------- 1 | uint8 STATUS_ODOM_VALID=0 2 | uint8 STATUS_ODOM_INVALID=1 3 | uint8 STATUS_ODOM_LOOPCLOSURE=2 4 | 5 | nav_msgs/Odometry curodom 6 | nav_msgs/Odometry kfodom 7 | uint32 kfid 8 | uint8 status 9 | -------------------------------------------------------------------------------- /msg/OutputData.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint16 loop_rate 3 | float64 voltage 4 | geometry_msgs/Quaternion orientation 5 | geometry_msgs/Vector3 angular_velocity 6 | geometry_msgs/Vector3 linear_acceleration 7 | float64 pressure_dheight 8 | float64 pressure_height 9 | geometry_msgs/Vector3 magnetic_field 10 | uint8[8] radio_channel 11 | #uint8[4] motor_rpm 12 | uint8 seq 13 | -------------------------------------------------------------------------------- /msg/PPROutputData.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint16 quad_time 3 | float64 des_thrust 4 | float64 des_roll 5 | float64 des_pitch 6 | float64 des_yaw 7 | float64 est_roll 8 | float64 est_pitch 9 | float64 est_yaw 10 | float64 est_angvel_x 11 | float64 est_angvel_y 12 | float64 est_angvel_z 13 | float64 est_acc_x 14 | float64 est_acc_y 15 | float64 est_acc_z 16 | uint16[4] pwm 17 | -------------------------------------------------------------------------------- /msg/PositionCommand.msg~: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Point position 3 | geometry_msgs/Vector3 velocity 4 | geometry_msgs/Vector3 acceleration 5 | float64 yaw 6 | float64 yaw_dot 7 | float64[3] kx 8 | float64[3] kv 9 | 10 | uint32 trajectory_id 11 | 12 | uint8 TRAJECTORY_STATUS_EMPTY = 0 13 | uint8 TRAJECTORY_STATUS_READY = 1 14 | uint8 TRAJECTORY_STATUS_COMPLETE = 3 15 | uint8 TRAJECTROY_STATUS_ABORT = 8 16 | 17 | uint8 trajectory_flag 18 | -------------------------------------------------------------------------------- /msg/Serial.msg: -------------------------------------------------------------------------------- 1 | # Note: These constants need to be kept in sync with the types 2 | # defined in include/quadrotor_msgs/comm_types.h 3 | uint8 SO3_CMD = 115 # 's' in base 10 4 | uint8 TRPY_CMD = 112 # 'p' in base 10 5 | uint8 STATUS_DATA = 99 # 'c' in base 10 6 | uint8 OUTPUT_DATA = 100 # 'd' in base 10 7 | uint8 PPR_OUTPUT_DATA = 116 # 't' in base 10 8 | uint8 PPR_GAINS = 103 # 'g' 9 | 10 | Header header 11 | uint8 channel 12 | uint8 type # One of the types listed above 13 | uint8[] data 14 | -------------------------------------------------------------------------------- /msg/PositionCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Point position 3 | geometry_msgs/Vector3 velocity 4 | geometry_msgs/Vector3 acceleration 5 | float64 yaw 6 | float64 yaw_dot 7 | float64[3] kx 8 | float64[3] kv 9 | 10 | uint32 trajectory_id 11 | 12 | uint8 TRAJECTORY_STATUS_EMPTY = 0 13 | uint8 TRAJECTORY_STATUS_READY = 1 14 | uint8 TRAJECTORY_STATUS_COMPLETED = 3 15 | uint8 TRAJECTROY_STATUS_ABORT = 4 16 | uint8 TRAJECTORY_STATUS_ILLEGAL_START = 5 17 | uint8 TRAJECTORY_STATUS_ILLEGAL_FINAL = 6 18 | uint8 TRAJECTORY_STATUS_IMPOSSIBLE = 7 19 | 20 | # Its ID number will start from 1, allowing you comparing it with 0. 21 | uint8 trajectory_flag 22 | -------------------------------------------------------------------------------- /msg/PolynomialTrajectory.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # the trajectory id, starts from "1". 4 | uint32 trajectory_id 5 | 6 | # the action command for trajectory server. 7 | uint32 ACTION_ADD = 1 8 | uint32 ACTION_ABORT = 2 9 | uint32 ACTION_WARN_START = 3 10 | uint32 ACTION_WARN_FINAL = 4 11 | uint32 ACTION_WARN_IMPOSSIBLE = 5 12 | uint32 action 13 | 14 | # the order of trajectory. 15 | uint32 num_order 16 | uint32 num_segment 17 | 18 | # the polynomial coecfficients of the trajectory. 19 | float64 start_yaw 20 | float64 final_yaw 21 | float64[] coef_x 22 | float64[] coef_y 23 | float64[] coef_z 24 | float64[] time 25 | float64 mag_coeff 26 | 27 | string debug_info 28 | -------------------------------------------------------------------------------- /include/quadrotor_msgs/encode_msgs.h: -------------------------------------------------------------------------------- 1 | #ifndef __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ 2 | #define __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace quadrotor_msgs 11 | { 12 | 13 | void encodeSO3Command(const quadrotor_msgs::SO3Command &so3_command, 14 | std::vector &output); 15 | void encodeTRPYCommand(const quadrotor_msgs::TRPYCommand &trpy_command, 16 | std::vector &output); 17 | 18 | void encodePPRGains(const quadrotor_msgs::Gains &gains, 19 | std::vector &output); 20 | } 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /include/quadrotor_msgs/decode_msgs.h: -------------------------------------------------------------------------------- 1 | #ifndef __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ 2 | #define __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace quadrotor_msgs 11 | { 12 | 13 | bool decodeOutputData(const std::vector &data, 14 | quadrotor_msgs::OutputData &output); 15 | 16 | bool decodeStatusData(const std::vector &data, 17 | quadrotor_msgs::StatusData &status); 18 | 19 | bool decodePPROutputData(const std::vector &data, 20 | quadrotor_msgs::PPROutputData &output); 21 | } 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | quadrotor_msgs 3 | 0.0.0 4 | quadrotor_msgs 5 | Kartik Mohta 6 | http://ros.org/wiki/quadrotor_msgs 7 | BSD 8 | catkin 9 | geometry_msgs 10 | nav_msgs 11 | message_generation 12 | geometry_msgs 13 | nav_msgs 14 | message_runtime 15 | 16 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /include/quadrotor_msgs/comm_types.h: -------------------------------------------------------------------------------- 1 | #ifndef __QUADROTOR_MSGS_COMM_TYPES_H__ 2 | #define __QUADROTOR_MSGS_COMM_TYPES_H__ 3 | 4 | #define TYPE_SO3_CMD 's' 5 | struct SO3_CMD_INPUT 6 | { 7 | // Scaling factors when decoding 8 | int16_t force[3]; // /500 9 | int8_t des_qx, des_qy, des_qz, des_qw; // /125 10 | uint8_t kR[3]; // /50 11 | uint8_t kOm[3]; // /100 12 | int16_t cur_yaw; // /1e4 13 | int16_t kf_correction; // /1e11; 14 | uint8_t angle_corrections[2]; // roll,pitch /2500 15 | uint8_t enable_motors:1; 16 | uint8_t use_external_yaw:1; 17 | uint8_t seq; 18 | }; 19 | 20 | #define TYPE_STATUS_DATA 'c' 21 | struct STATUS_DATA 22 | { 23 | uint16_t loop_rate; 24 | uint16_t voltage; 25 | uint8_t seq; 26 | }; 27 | 28 | #define TYPE_OUTPUT_DATA 'd' 29 | struct OUTPUT_DATA 30 | { 31 | uint16_t loop_rate; 32 | uint16_t voltage; 33 | int16_t roll, pitch, yaw; 34 | int16_t ang_vel[3]; 35 | int16_t acc[3]; 36 | int16_t dheight; 37 | int32_t height; 38 | int16_t mag[3]; 39 | uint8_t radio[8]; 40 | //uint8_t rpm[4]; 41 | uint8_t seq; 42 | }; 43 | 44 | #define TYPE_TRPY_CMD 'p' 45 | struct TRPY_CMD 46 | { 47 | int16_t thrust; 48 | int16_t roll; 49 | int16_t pitch; 50 | int16_t yaw; 51 | int16_t current_yaw; 52 | uint8_t enable_motors:1; 53 | uint8_t use_external_yaw:1; 54 | }; 55 | 56 | #define TYPE_PPR_OUTPUT_DATA 't' 57 | struct PPR_OUTPUT_DATA 58 | { 59 | uint16_t time; 60 | int16_t des_thrust; 61 | int16_t des_roll; 62 | int16_t des_pitch; 63 | int16_t des_yaw; 64 | int16_t est_roll; 65 | int16_t est_pitch; 66 | int16_t est_yaw; 67 | int16_t est_angvel_x; 68 | int16_t est_angvel_y; 69 | int16_t est_angvel_z; 70 | int16_t est_acc_x; 71 | int16_t est_acc_y; 72 | int16_t est_acc_z; 73 | uint16_t pwm1; 74 | uint16_t pwm2; 75 | uint16_t pwm3; 76 | uint16_t pwm4; 77 | }; 78 | 79 | #define TYPE_PPR_GAINS 'g' 80 | struct PPR_GAINS 81 | { 82 | int16_t Kp; 83 | int16_t Kd; 84 | int16_t Kp_yaw; 85 | int16_t Kd_yaw; 86 | }; 87 | #endif 88 | -------------------------------------------------------------------------------- /src/encode_msgs.cpp: -------------------------------------------------------------------------------- 1 | #include "quadrotor_msgs/encode_msgs.h" 2 | #include 3 | 4 | namespace quadrotor_msgs 5 | { 6 | 7 | void encodeSO3Command(const quadrotor_msgs::SO3Command &so3_command, 8 | std::vector &output) 9 | { 10 | struct SO3_CMD_INPUT so3_cmd_input; 11 | 12 | so3_cmd_input.force[0] = so3_command.force.x*500; 13 | so3_cmd_input.force[1] = so3_command.force.y*500; 14 | so3_cmd_input.force[2] = so3_command.force.z*500; 15 | 16 | so3_cmd_input.des_qx = so3_command.orientation.x*125; 17 | so3_cmd_input.des_qy = so3_command.orientation.y*125; 18 | so3_cmd_input.des_qz = so3_command.orientation.z*125; 19 | so3_cmd_input.des_qw = so3_command.orientation.w*125; 20 | 21 | so3_cmd_input.kR[0] = so3_command.kR[0]*50; 22 | so3_cmd_input.kR[1] = so3_command.kR[1]*50; 23 | so3_cmd_input.kR[2] = so3_command.kR[2]*50; 24 | 25 | so3_cmd_input.kOm[0] = so3_command.kOm[0]*100; 26 | so3_cmd_input.kOm[1] = so3_command.kOm[1]*100; 27 | so3_cmd_input.kOm[2] = so3_command.kOm[2]*100; 28 | 29 | so3_cmd_input.cur_yaw = so3_command.aux.current_yaw*1e4; 30 | 31 | so3_cmd_input.kf_correction = so3_command.aux.kf_correction*1e11; 32 | so3_cmd_input.angle_corrections[0] = so3_command.aux.angle_corrections[0]*2500; 33 | so3_cmd_input.angle_corrections[1] = so3_command.aux.angle_corrections[1]*2500; 34 | 35 | so3_cmd_input.enable_motors = so3_command.aux.enable_motors; 36 | so3_cmd_input.use_external_yaw = so3_command.aux.use_external_yaw; 37 | 38 | so3_cmd_input.seq = so3_command.header.seq % 255; 39 | 40 | output.resize(sizeof(so3_cmd_input)); 41 | memcpy(&output[0], &so3_cmd_input, sizeof(so3_cmd_input)); 42 | } 43 | 44 | void encodeTRPYCommand(const quadrotor_msgs::TRPYCommand &trpy_command, 45 | std::vector &output) 46 | { 47 | struct TRPY_CMD trpy_cmd_input; 48 | trpy_cmd_input.thrust = trpy_command.thrust*1e4; 49 | trpy_cmd_input.roll = trpy_command.roll*1e4; 50 | trpy_cmd_input.pitch = trpy_command.pitch*1e4; 51 | trpy_cmd_input.yaw = trpy_command.yaw*1e4; 52 | trpy_cmd_input.current_yaw = trpy_command.aux.current_yaw*1e4; 53 | trpy_cmd_input.enable_motors = trpy_command.aux.enable_motors; 54 | trpy_cmd_input.use_external_yaw = trpy_command.aux.use_external_yaw; 55 | 56 | output.resize(sizeof(trpy_cmd_input)); 57 | memcpy(&output[0], &trpy_cmd_input, sizeof(trpy_cmd_input)); 58 | } 59 | 60 | void encodePPRGains(const quadrotor_msgs::Gains &gains, 61 | std::vector &output) 62 | { 63 | struct PPR_GAINS ppr_gains; 64 | ppr_gains.Kp = gains.Kp; 65 | ppr_gains.Kd = gains.Kd; 66 | ppr_gains.Kp_yaw = gains.Kp_yaw; 67 | ppr_gains.Kd_yaw = gains.Kd_yaw; 68 | 69 | output.resize(sizeof(ppr_gains)); 70 | memcpy(&output[0], &ppr_gains, sizeof(ppr_gains)); 71 | } 72 | 73 | } 74 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #-cmake_minimum_required(VERSION 2.4.6) 2 | #-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | #-rosbuild_init() 13 | 14 | #set the default path for built executables to the "bin" directory 15 | #-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | #-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | #uncomment if you have defined messages 20 | #rosbuild_genmsg() 21 | #uncomment if you have defined services 22 | #rosbuild_gensrv() 23 | 24 | #common commands for building c++ executables and libraries 25 | #-\rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 26 | #target_link_libraries(${PROJECT_NAME} another_library) 27 | #rosbuild_add_boost_directories() 28 | #rosbuild_link_boost(${PROJECT_NAME} thread) 29 | #rosbuild_add_executable(example examples/example.cpp) 30 | #target_link_libraries(example ${PROJECT_NAME}) 31 | 32 | #-rosbuild_add_library(encode_msgs src/encode_msgs.cpp) 33 | #-rosbuild_add_link_flags(encode_msgs -Wl,--as-needed) 34 | 35 | #list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 36 | #find_package(Eigen3 REQUIRED) 37 | 38 | #include_directories(${EIGEN3_INCLUDE_DIR}) 39 | #-rosbuild_add_library(decode_msgs src/decode_msgs.cpp) 40 | #-rosbuild_add_link_flags(decode_msgs -Wl,--as-needed) 41 | 42 | #------------------------------------------------------------------ 43 | cmake_minimum_required(VERSION 2.8.3) 44 | project(quadrotor_msgs) 45 | 46 | find_package(catkin REQUIRED COMPONENTS 47 | #armadillo 48 | roscpp 49 | nav_msgs 50 | geometry_msgs 51 | message_generation 52 | ) 53 | 54 | include_directories( 55 | ${catkin_INCLUDE_DIRS} 56 | include 57 | ) 58 | 59 | 60 | add_message_files( 61 | FILES 62 | AuxCommand.msg 63 | Corrections.msg 64 | Gains.msg 65 | OutputData.msg 66 | PositionCommand.msg 67 | PPROutputData.msg 68 | Serial.msg 69 | SO3Command.msg 70 | StatusData.msg 71 | TRPYCommand.msg 72 | Odometry.msg 73 | PolynomialTrajectory.msg 74 | ) 75 | 76 | generate_messages( 77 | DEPENDENCIES 78 | geometry_msgs 79 | nav_msgs 80 | ) 81 | 82 | catkin_package( 83 | #INCLUDE_DIRS include 84 | LIBRARIES encode_msgs decode_msgs 85 | #CATKIN_DEPENDS geometry_msgs nav_msgs 86 | #DEPENDS system_lib 87 | CATKIN_DEPENDS message_runtime 88 | ) 89 | 90 | # add_executable(test_exe src/test_exe.cpp) 91 | add_library(decode_msgs src/decode_msgs.cpp) 92 | add_library(encode_msgs src/encode_msgs.cpp) 93 | 94 | # add_dependencies(test_exe quadrotor_msgs_generate_messages_cpp) 95 | add_dependencies(encode_msgs quadrotor_msgs_generate_messages_cpp) 96 | add_dependencies(decode_msgs quadrotor_msgs_generate_messages_cpp) 97 | 98 | find_package(Eigen3 REQUIRED) 99 | include_directories(${EIGEN3_INCLUDE_DIR}) 100 | 101 | 102 | # target_link_libraries(test_exe 103 | # decode_msgs 104 | # encode_msgs 105 | # ) 106 | 107 | -------------------------------------------------------------------------------- /src/decode_msgs.cpp: -------------------------------------------------------------------------------- 1 | #include "quadrotor_msgs/decode_msgs.h" 2 | #include 3 | #include 4 | 5 | namespace quadrotor_msgs 6 | { 7 | 8 | bool decodeOutputData(const std::vector &data, 9 | quadrotor_msgs::OutputData &output) 10 | { 11 | struct OUTPUT_DATA output_data; 12 | if(data.size() != sizeof(output_data)) 13 | return false; 14 | 15 | memcpy(&output_data, &data[0], sizeof(output_data)); 16 | output.loop_rate = output_data.loop_rate; 17 | output.voltage = output_data.voltage/1e3; 18 | 19 | const double roll = output_data.roll/1e2 * M_PI/180; 20 | const double pitch = output_data.pitch/1e2 * M_PI/180; 21 | const double yaw = output_data.yaw/1e2 * M_PI/180; 22 | // Asctec (2012 firmware) uses Z-Y-X convention 23 | Eigen::Quaterniond q = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * 24 | Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * 25 | Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()); 26 | output.orientation.w = q.w(); 27 | output.orientation.x = q.x(); 28 | output.orientation.y = q.y(); 29 | output.orientation.z = q.z(); 30 | 31 | output.angular_velocity.x = output_data.ang_vel[0]*0.0154*M_PI/180; 32 | output.angular_velocity.y = output_data.ang_vel[1]*0.0154*M_PI/180; 33 | output.angular_velocity.z = output_data.ang_vel[2]*0.0154*M_PI/180; 34 | 35 | output.linear_acceleration.x = output_data.acc[0]/1e3 * 9.81; 36 | output.linear_acceleration.y = output_data.acc[1]/1e3 * 9.81; 37 | output.linear_acceleration.z = output_data.acc[2]/1e3 * 9.81; 38 | 39 | output.pressure_dheight = output_data.dheight/1e3; 40 | output.pressure_height = output_data.height/1e3; 41 | 42 | output.magnetic_field.x = output_data.mag[0]/2500.0; 43 | output.magnetic_field.y = output_data.mag[1]/2500.0; 44 | output.magnetic_field.z = output_data.mag[2]/2500.0; 45 | 46 | for(int i = 0; i < 8; i++) 47 | { 48 | output.radio_channel[i] = output_data.radio[i]; 49 | } 50 | //for(int i = 0; i < 4; i++) 51 | // output.motor_rpm[i] = output_data.rpm[i]; 52 | 53 | output.seq = output_data.seq; 54 | 55 | return true; 56 | } 57 | 58 | bool decodeStatusData(const std::vector &data, 59 | quadrotor_msgs::StatusData &status) 60 | { 61 | struct STATUS_DATA status_data; 62 | if(data.size() != sizeof(status_data)) 63 | return false; 64 | memcpy(&status_data, &data[0], sizeof(status_data)); 65 | 66 | status.loop_rate = status_data.loop_rate; 67 | status.voltage = status_data.voltage/1e3; 68 | status.seq = status_data.seq; 69 | 70 | return true; 71 | } 72 | 73 | bool decodePPROutputData(const std::vector &data, 74 | quadrotor_msgs::PPROutputData &output) 75 | { 76 | struct PPR_OUTPUT_DATA output_data; 77 | if(data.size() != sizeof(output_data)) 78 | return false; 79 | memcpy(&output_data, &data[0], sizeof(output_data)); 80 | 81 | output.quad_time = output_data.time; 82 | output.des_thrust = output_data.des_thrust*1e-4; 83 | output.des_roll = output_data.des_roll*1e-4; 84 | output.des_pitch = output_data.des_pitch*1e-4; 85 | output.des_yaw = output_data.des_yaw*1e-4; 86 | output.est_roll = output_data.est_roll*1e-4; 87 | output.est_pitch = output_data.est_pitch*1e-4; 88 | output.est_yaw = output_data.est_yaw*1e-4; 89 | output.est_angvel_x = output_data.est_angvel_x*1e-3; 90 | output.est_angvel_y = output_data.est_angvel_y*1e-3; 91 | output.est_angvel_z = output_data.est_angvel_z*1e-3; 92 | output.est_acc_x = output_data.est_acc_x*1e-4; 93 | output.est_acc_y = output_data.est_acc_y*1e-4; 94 | output.est_acc_z = output_data.est_acc_z*1e-4; 95 | output.pwm[0] = output_data.pwm1; 96 | output.pwm[1] = output_data.pwm2; 97 | output.pwm[2] = output_data.pwm3; 98 | output.pwm[3] = output_data.pwm4; 99 | 100 | return true; 101 | } 102 | 103 | } 104 | --------------------------------------------------------------------------------