From 3e0b8b7016556224aedb54f51d769d3288f0920e Mon Sep 17 00:00:00 2001 From: stmoon Date: Sat, 6 Jan 2018 22:48:04 +0900 Subject: [PATCH] remove unnecessary codes and follow code style --- src/modules/simulator/simulator.h | 1 - src/modules/simulator/simulator_mavlink.cpp | 82 +++++---------------- 2 files changed, 20 insertions(+), 63 deletions(-) diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 5f992a4f3c..c87b7dace6 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -392,7 +392,6 @@ private: void pollForMAVLinkMessages(bool publish, int udp_port); void pack_actuator_message(mavlink_hil_actuator_controls_t &actuator_msg, unsigned index); - //void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) void send_mavlink_message(const mavlink_message_t &aMsg); void update_sensors(mavlink_hil_sensor_t *imu); void update_gps(mavlink_hil_gps_t *gps_sim); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 1cb7439964..0996181c65 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -55,8 +55,6 @@ extern "C" __EXPORT hrt_abstime hrt_reset(void); #define PRESS_GROUND 101325.0f #define DENSITY 1.2041f -//static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; -//static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; #ifdef ENABLE_UART_RC_INPUT @@ -180,12 +178,11 @@ void Simulator::send_controls() continue; } - mavlink_hil_actuator_controls_t msg; - pack_actuator_message(msg, i); - //send_mavlink_message(MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS, &msg, 200); - mavlink_message_t message; - mavlink_msg_hil_actuator_controls_encode(0, 200, &message, &msg); - send_mavlink_message(message); + mavlink_hil_actuator_controls_t hil_act_control = {}; + mavlink_message_t message = {}; + pack_actuator_message(hil_act_control, i); + mavlink_msg_hil_actuator_controls_encode(0, 200, &message, &hil_act_control); + send_mavlink_message(message); } } @@ -536,56 +533,20 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) void Simulator::send_mavlink_message(const mavlink_message_t &aMsg) { - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - uint16_t bufLen = 0; + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + uint16_t bufLen = 0; - // convery mavlink message to raw data - bufLen = mavlink_msg_to_send_buffer(buf, &aMsg); + // convery mavlink message to raw data + bufLen = mavlink_msg_to_send_buffer(buf, &aMsg); - // send data - ssize_t len = sendto(_fd, buf, bufLen, 0, (struct sockaddr *)&_srcaddr, _addrlen); + // send data + ssize_t len = sendto(_fd, buf, bufLen, 0, (struct sockaddr *)&_srcaddr, _addrlen); - if (len <= 0) { - PX4_WARN("Failed sending mavlink message"); - } + if (len <= 0) { + PX4_WARN("Failed sending mavlink message"); + } } -//void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) -//{ -// component_ID = 0; -// uint8_t payload_len = mavlink_message_lengths[msgid]; -// unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - -// uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - -// /* header */ -// buf[0] = MAVLINK_STX; -// buf[1] = payload_len; -// /* no idea which numbers should be here*/ -// buf[2] = 100; -// buf[3] = 0; -// buf[4] = component_ID; -// buf[5] = msgid; - -// /* payload */ -// memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len); - -// /* checksum */ -// uint16_t checksum; -// crc_init(&checksum); -// crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); -// crc_accumulate(mavlink_message_crcs[msgid], &checksum); - -// buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); -// buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); - -// ssize_t len = sendto(_fd, buf, packet_len, 0, (struct sockaddr *)&_srcaddr, _addrlen); - -// if (len <= 0) { -// PX4_WARN("Failed sending mavlink message"); -// } -//} - void Simulator::poll_topics() { // copy new actuator data if available @@ -783,13 +744,11 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port) len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); // send hearbeat mavlink_heartbeat_t hb = {}; + mavlink_message_t message = {}; hb.autopilot = 12; hb.base_mode |= (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 128 : 0; - //send_mavlink_message(MAVLINK_MSG_ID_HEARTBEAT, &hb, 200); - mavlink_message_t message; - mavlink_msg_heartbeat_encode(0, 50, &message, &hb); - send_mavlink_message(message); - + mavlink_msg_heartbeat_encode(0, 50, &message, &hb); + send_mavlink_message(message); if (len > 0) { mavlink_message_t msg; @@ -836,13 +795,12 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port) //send MAV_CMD_SET_MESSAGE_INTERVAL for HIL_STATE_QUATERNION ground truth mavlink_command_long_t cmd_long = {}; + mavlink_message_t message = {}; cmd_long.command = MAV_CMD_SET_MESSAGE_INTERVAL; cmd_long.param1 = MAVLINK_MSG_ID_HIL_STATE_QUATERNION; cmd_long.param2 = 5e3; - //send_mavlink_message(MAVLINK_MSG_ID_COMMAND_LONG, &cmd_long, 200); - mavlink_message_t message; - mavlink_msg_command_long_encode(0, 50, &message, &cmd_long); - send_mavlink_message(message); + mavlink_msg_command_long_encode(0, 50, &message, &cmd_long); + send_mavlink_message(message); _initialized = true;