From c9956e25b4d632fef49aac11f5cba770bccce03f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 4 Jan 2017 17:45:32 -0500 Subject: [PATCH] mavlink fix code style --- Tools/files_to_check_code_style.sh | 3 +- src/modules/mavlink/mavlink_main.h | 11 +++-- src/modules/mavlink/mavlink_messages.cpp | 12 ++++-- src/modules/mavlink/mavlink_mission.cpp | 15 +++++-- .../mavlink/mavlink_orb_subscription.cpp | 5 +++ src/modules/mavlink/mavlink_parameters.cpp | 3 +- src/modules/mavlink/mavlink_parameters.h | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 16 ++++++-- src/modules/mavlink/mavlink_ulog.cpp | 40 +++++++++++++++---- 9 files changed, 80 insertions(+), 27 deletions(-) diff --git a/Tools/files_to_check_code_style.sh b/Tools/files_to_check_code_style.sh index e49e3784ae..e26d0a3f92 100755 --- a/Tools/files_to_check_code_style.sh +++ b/Tools/files_to_check_code_style.sh @@ -9,6 +9,7 @@ then fi exec find src \ + -path src/drivers/bootloaders -o \ -path src/examples/attitude_estimator_ekf -prune -o \ -path src/examples/ekf_att_pos_estimator -prune -o \ -path src/lib/DriverFramework -prune -o \ @@ -16,9 +17,7 @@ exec find src \ -path src/lib/external_lgpl -prune -o \ -path src/lib/mathlib -prune -o \ -path src/lib/matrix -prune -o \ - -path src/drivers/bootloaders -o \ -path src/modules/commander -prune -o \ - -path src/modules/mavlink -prune -o \ -path src/modules/sdlog2 -prune -o \ -path src/modules/systemlib/uthash -prune -o \ -path src/modules/uavcan -prune -o \ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 05261a8010..a3a709f9bf 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -172,10 +172,11 @@ public: BROADCAST_MODE_ON }; - static const char *mavlink_mode_str(enum MAVLINK_MODE mode) { + static const char *mavlink_mode_str(enum MAVLINK_MODE mode) + { switch (mode) { case MAVLINK_MODE_NORMAL: - return "Normal"; + return "Normal"; case MAVLINK_MODE_CUSTOM: return "Custom"; @@ -441,12 +442,14 @@ public: /** get ulog streaming if active, nullptr otherwise */ MavlinkULog *get_ulog_streaming() { return _mavlink_ulog; } - void try_start_ulog_streaming(uint8_t target_system, uint8_t target_component) { + void try_start_ulog_streaming(uint8_t target_system, uint8_t target_component) + { if (_mavlink_ulog) { return; } _mavlink_ulog = MavlinkULog::try_start(_datarate, 0.7f, target_system, target_component); } - void request_stop_ulog_streaming() { + void request_stop_ulog_streaming() + { if (_mavlink_ulog) { _mavlink_ulog_stop_requested = true; } } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 450f0f28c4..c6dcc3d3ab 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2656,8 +2656,10 @@ protected: mavlink_attitude_target_t msg{}; msg.time_boot_ms = att_sp.timestamp / 1000; + if (att_sp.q_d_valid) { memcpy(&msg.q[0], &att_sp.q_d[0], sizeof(msg.q)); + } else { mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q); } @@ -3284,7 +3286,7 @@ protected: if (!status.in_transition_mode && status.is_rotary_wing) { _msg.vtol_state = MAV_VTOL_STATE_MC; - } else if (!status.in_transition_mode){ + } else if (!status.in_transition_mode) { _msg.vtol_state = MAV_VTOL_STATE_FW; } else if (status.in_transition_mode && status.in_transition_to_fw) { @@ -3384,9 +3386,11 @@ protected: { struct vehicle_global_position_s global_pos; updated |= _global_pos_sub->update(&_global_pos_time, &global_pos); + if (_global_pos_time != 0) { msg.altitude_amsl = global_pos.alt; global_alt = global_pos.alt; + } else { msg.altitude_amsl = NAN; } @@ -3680,7 +3684,7 @@ private: /* do not allow top copying this class */ MavlinkStreamHighLatency(MavlinkStreamHighLatency &); - MavlinkStreamHighLatency& operator = (const MavlinkStreamHighLatency &); + MavlinkStreamHighLatency &operator = (const MavlinkStreamHighLatency &); protected: explicit MavlinkStreamHighLatency(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -3769,6 +3773,7 @@ protected: if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { msg.throttle = actuator.control[actuator_controls_s::INDEX_THROTTLE] * 100; + } else { msg.throttle = 0; } @@ -3835,7 +3840,7 @@ public: unsigned get_size() { return (_att_time > 0 || _gpos_time > 0) ? MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN + - MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: @@ -3892,6 +3897,7 @@ protected: msg.yacc = 0; msg.zacc = 0; } + mavlink_msg_hil_state_quaternion_send_struct(_mavlink->get_channel(), &msg); } } diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index e7147f70f7..298fd50b03 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -309,6 +309,7 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 if (_verbose) { PX4_INFO("WPM: Send MISSION_REQUEST_INT seq %u to ID %u", wpr.seq, wpr.target_system); } + } else { mavlink_mission_request_t wpr; @@ -362,6 +363,7 @@ MavlinkMissionManager::send(const hrt_abstime now) _time_last_reached = now; _last_reached = mission_result.seq_reached; send_mission_item_reached((uint16_t)mission_result.seq_reached); + } else { _last_reached = -1; } @@ -572,6 +574,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) if (_int_mode) { _int_mode = false; } + handle_mission_request_both(msg); } @@ -582,6 +585,7 @@ MavlinkMissionManager::handle_mission_request_int(const mavlink_message_t *msg) if (!_int_mode) { _int_mode = true; } + handle_mission_request_both(msg); } @@ -879,7 +883,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) int MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, - struct mission_item_s *mission_item) + struct mission_item_s *mission_item) { if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL || mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) { @@ -890,12 +894,14 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * * alignment, so we can just swap float for int32_t. */ const mavlink_mission_item_int_t *item_int = reinterpret_cast(mavlink_mission_item); - mission_item->lat = ((double)item_int->x)*1e-7; - mission_item->lon = ((double)item_int->y)*1e-7; + mission_item->lat = ((double)item_int->x) * 1e-7; + mission_item->lon = ((double)item_int->y) * 1e-7; + } else { mission_item->lat = (double)mavlink_mission_item->x; mission_item->lon = (double)mavlink_mission_item->y; } + mission_item->altitude = mavlink_mission_item->z; if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL) { @@ -1027,7 +1033,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * int MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, - mavlink_mission_item_t *mavlink_mission_item) + mavlink_mission_item_t *mavlink_mission_item) { mavlink_mission_item->frame = mission_item->frame; mavlink_mission_item->command = mission_item->nav_cmd; @@ -1089,6 +1095,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * mavlink_mission_item->x = (float)mission_item->lat; mavlink_mission_item->y = (float)mission_item->lon; } + mavlink_mission_item->z = mission_item->altitude; if (mission_item->altitude_is_relative) { diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 68f6066cfe..f2aedcccd0 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -171,12 +171,15 @@ MavlinkOrbSubscription::is_published() _last_pub_check = now; #if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) + // Snapdragon has currently no support for orb_exists, therefore // we're not using it. if (_fd < 0) { _fd = orb_subscribe_multi(_topic, _instance); } + #else + // We don't want to subscribe to anything that does not exist // in order to save memory and file descriptors. // However, for some topics like vehicle_command_ack, we want to subscribe @@ -188,6 +191,7 @@ MavlinkOrbSubscription::is_published() if (_fd < 0) { _fd = orb_subscribe_multi(_topic, _instance); } + #endif bool updated; @@ -199,6 +203,7 @@ MavlinkOrbSubscription::is_published() // topic may have been last published before we subscribed uint64_t time_topic = 0; + if (!_published && orb_stat(_fd, &time_topic) == PX4_OK) { if (time_topic != 0) { _published = true; diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 9cbec5b26d..9a3c31d6d1 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -351,7 +351,8 @@ MavlinkParametersManager::send(const hrt_abstime t) // Re-pack the message with the UAVCAN node ID mavlink_message_t mavlink_packet; - mavlink_msg_param_value_encode_chan(mavlink_system.sysid, value.node_id, _mavlink->get_channel(), &mavlink_packet, &msg); + mavlink_msg_param_value_encode_chan(mavlink_system.sysid, value.node_id, _mavlink->get_channel(), &mavlink_packet, + &msg); _mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet); } else if (_send_all_index >= 0 && _mavlink->boot_complete()) { diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index ce1d56363f..b0de654563 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -90,7 +90,7 @@ protected: void send(const hrt_abstime t); - int send_param(param_t param, int component_id=-1); + int send_param(param_t param, int component_id = -1); orb_advert_t _rc_param_map_pub; struct rc_parameter_map_s _rc_param_map; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a2b6ab9a47..7c7cdeead6 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -387,7 +387,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) { ret = set_message_interval((int)(cmd_mavlink.param1 + 0.5f), - cmd_mavlink.param2, cmd_mavlink.param3); + cmd_mavlink.param2, cmd_mavlink.param3); } else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) { get_message_interval((int)cmd_mavlink.param1); @@ -456,14 +456,17 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) if (send_ack) { vehicle_command_ack_s command_ack; command_ack.command = cmd_mavlink.command; + if (ret == PX4_OK) { command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; + } else { command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; } if (_command_ack_pub == nullptr) { - _command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH); + _command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, + vehicle_command_ack_s::ORB_QUEUE_LENGTH); } else { orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack); @@ -556,14 +559,17 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) if (send_ack) { vehicle_command_ack_s command_ack; command_ack.command = cmd_mavlink.command; + if (ret == PX4_OK) { command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; + } else { command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; } if (_command_ack_pub == nullptr) { - _command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH); + _command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, + vehicle_command_ack_s::ORB_QUEUE_LENGTH); } else { orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack); @@ -903,6 +909,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t pos_sp_triplet.current.velocity_frame = set_position_target_local_ned.coordinate_frame; + } else { pos_sp_triplet.current.velocity_valid = false; } @@ -2241,7 +2248,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) ctrl_state.z_acc = hil_state.zacc; static float _acc_hor_filt = 0; - _acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(ctrl_state.x_acc * ctrl_state.x_acc + ctrl_state.y_acc * ctrl_state.y_acc); + _acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(ctrl_state.x_acc * ctrl_state.x_acc + ctrl_state.y_acc * + ctrl_state.y_acc); ctrl_state.horz_acc_mag = _acc_hor_filt; ctrl_state.airspeed_valid = false; diff --git a/src/modules/mavlink/mavlink_ulog.cpp b/src/modules/mavlink/mavlink_ulog.cpp index 0b67f98297..5a0616220b 100644 --- a/src/modules/mavlink/mavlink_ulog.cpp +++ b/src/modules/mavlink/mavlink_ulog.cpp @@ -51,16 +51,17 @@ const float MavlinkULog::_rate_calculation_delta_t = 0.1f; MavlinkULog::MavlinkULog(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component) : _target_system(target_system), _target_component(target_component), - _max_rate_factor(max_rate_factor), - _max_num_messages(math::max(1, (int)ceilf(_rate_calculation_delta_t * _max_rate_factor * datarate / - (MAVLINK_MSG_ID_LOGGING_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)))), - _current_rate_factor(max_rate_factor) + _max_rate_factor(max_rate_factor), + _max_num_messages(math::max(1, (int)ceilf(_rate_calculation_delta_t *_max_rate_factor * datarate / + (MAVLINK_MSG_ID_LOGGING_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)))), + _current_rate_factor(max_rate_factor) { _ulog_stream_sub = orb_subscribe(ORB_ID(ulog_stream)); if (_ulog_stream_sub < 0) { PX4_ERR("orb_subscribe failed (%i)", errno); } + _waiting_for_initial_ack = true; _last_sent_time = hrt_absolute_time(); //(ab)use this timestamp during initialization _next_rate_check = _last_sent_time + _rate_calculation_delta_t * 1.e6f; @@ -71,6 +72,7 @@ MavlinkULog::~MavlinkULog() if (_ulog_stream_ack_pub) { orb_unadvertise(_ulog_stream_ack_pub); } + if (_ulog_stream_sub >= 0) { orb_unsubscribe(_ulog_stream_sub); } @@ -87,28 +89,34 @@ void MavlinkULog::start_ack_received() int MavlinkULog::handle_update(mavlink_channel_t channel) { - static_assert(sizeof(ulog_stream_s::data) == MAVLINK_MSG_LOGGING_DATA_FIELD_DATA_LEN, "Invalid uorb ulog_stream.data length"); - static_assert(sizeof(ulog_stream_s::data) == MAVLINK_MSG_LOGGING_DATA_ACKED_FIELD_DATA_LEN, "Invalid uorb ulog_stream.data length"); + static_assert(sizeof(ulog_stream_s::data) == MAVLINK_MSG_LOGGING_DATA_FIELD_DATA_LEN, + "Invalid uorb ulog_stream.data length"); + static_assert(sizeof(ulog_stream_s::data) == MAVLINK_MSG_LOGGING_DATA_ACKED_FIELD_DATA_LEN, + "Invalid uorb ulog_stream.data length"); if (_waiting_for_initial_ack) { if (hrt_elapsed_time(&_last_sent_time) > 3e5) { PX4_WARN("no ack from logger (is it running?)"); return -1; } + return 0; } // check if we're waiting for an ACK if (_last_sent_time) { bool check_for_updates = false; + if (_ack_received) { _last_sent_time = 0; check_for_updates = true; + } else { if (hrt_elapsed_time(&_last_sent_time) > ulog_stream_ack_s::ACK_TIMEOUT * 1000) { if (++_sent_tries > ulog_stream_ack_s::ACK_MAX_TRIES) { return -ETIMEDOUT; + } else { PX4_DEBUG("re-sending ulog mavlink message (try=%i)", _sent_tries); _last_sent_time = hrt_absolute_time(); @@ -131,8 +139,10 @@ int MavlinkULog::handle_update(mavlink_channel_t channel) bool updated = false; int ret = orb_check(_ulog_stream_sub, &updated); + while (updated && !ret && _current_num_msgs < _max_num_messages) { orb_copy(ORB_ID(ulog_stream), _ulog_stream_sub, &_ulog_data); + if (_ulog_data.flags & ulog_stream_s::FLAGS_NEED_ACK) { _sent_tries = 1; _last_sent_time = hrt_absolute_time(); @@ -160,22 +170,26 @@ int MavlinkULog::handle_update(mavlink_channel_t channel) memcpy(msg.data, _ulog_data.data, sizeof(msg.data)); mavlink_msg_logging_data_send_struct(channel, &msg); } + ++_current_num_msgs; ret = orb_check(_ulog_stream_sub, &updated); } //need to update the rate? hrt_abstime t = hrt_absolute_time(); + if (t > _next_rate_check) { if (_current_num_msgs < _max_num_messages) { _current_rate_factor = _max_rate_factor * (float)_current_num_msgs / _max_num_messages; + } else { _current_rate_factor = _max_rate_factor; } + _current_num_msgs = 0; _next_rate_check = t + _rate_calculation_delta_t * 1.e6f; PX4_DEBUG("current rate=%.3f (max=%i msgs in %.3fs)", (double)_current_rate_factor, _max_num_messages, - (double)_rate_calculation_delta_t); + (double)_rate_calculation_delta_t); } return 0; @@ -186,48 +200,58 @@ void MavlinkULog::initialize() if (_init) { return; } + px4_sem_init(&_lock, 1, 1); _init = true; } -MavlinkULog* MavlinkULog::try_start(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component) +MavlinkULog *MavlinkULog::try_start(int datarate, float max_rate_factor, uint8_t target_system, + uint8_t target_component) { MavlinkULog *ret = nullptr; bool failed = false; lock(); + if (!_instance) { ret = _instance = new MavlinkULog(datarate, max_rate_factor, target_system, target_component); + if (!_instance) { failed = true; } } + unlock(); if (failed) { PX4_ERR("alloc failed"); } + return ret; } void MavlinkULog::stop() { lock(); + if (_instance) { delete _instance; _instance = nullptr; } + unlock(); } void MavlinkULog::handle_ack(mavlink_logging_ack_t ack) { lock(); + if (_instance) { // make sure stop() was not called right before if (_wait_for_ack_sequence == ack.sequence) { _ack_received = true; publish_ack(ack.sequence); } } + unlock(); }