yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

mavlink move to new lightweight uORB::Subscription

Browse Source
This commit is contained in:
Daniel Agar
2019-05-18 14:04:35 -04:00
parent 2c63e335e9
commit 579cbbb42c
11 changed files with 97 additions and 330 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
src/modules/mavlink/mavlink_main.cpp
View File

@@ -613,35 +613,26 @@ Mavlink::mavlink_open_uart(int baud, const char *uart_name, bool force_flow_cont
/* if this is a config link, stay here and wait for it to open */
if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) {
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
struct actuator_armed_s armed;
uORB::SubscriptionData<actuator_armed_s> armed_sub{ORB_ID(actuator_armed)};
/* get the system arming state and abort on arming */
while (_uart_fd < 0) {
/* abort if an arming topic is published and system is armed */
bool updated = false;
orb_check(armed_sub, &updated);
armed_sub.update();
if (updated) {
/* the system is now providing arming status feedback.
* instead of timing out, we resort to abort bringing
* up the terminal.
*/
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
if (armed.armed) {
/* this is not an error, but we are done */
orb_unsubscribe(armed_sub);
return -1;
}
/* the system is now providing arming status feedback.
* instead of timing out, we resort to abort bringing
* up the terminal.
*/
if (armed_sub.get().armed) {
/* this is not an error, but we are done */
return -1;
}
px4_usleep(100000);
_uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
}
orb_unsubscribe(armed_sub);
}
if (_uart_fd < 0) {
@@ -2136,7 +2127,7 @@ Mavlink::task_main(int argc, char *argv[])
MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log));
struct vehicle_status_s status;
vehicle_status_s status{};
status_sub->update(&status_time, &status);
/* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/
@@ -2205,7 +2196,9 @@ Mavlink::task_main(int argc, char *argv[])
update_rate_mult();
if (param_sub->update(&param_time, nullptr)) {
parameter_update_s param_update;
if (param_sub->update(&param_time, &param_update)) {
mavlink_update_parameters();
#if defined(CONFIG_NET)
@@ -2242,7 +2235,7 @@ Mavlink::task_main(int argc, char *argv[])
}
}
struct vehicle_command_s vehicle_cmd;
vehicle_command_s vehicle_cmd{};
if (cmd_sub->update_if_changed(&vehicle_cmd)) {
if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) &&
@@ -2287,7 +2280,7 @@ Mavlink::task_main(int argc, char *argv[])
/* send command ACK */
uint16_t current_command_ack = 0;
struct vehicle_command_ack_s command_ack;
vehicle_command_ack_s command_ack{};
if (ack_sub->update_if_changed(&command_ack)) {
if (!command_ack.from_external) {
@@ -2308,7 +2301,7 @@ Mavlink::task_main(int argc, char *argv[])
}
}
struct mavlink_log_s mavlink_log;
mavlink_log_s mavlink_log{};
if (mavlink_log_sub->update_if_changed(&mavlink_log)) {
_logbuffer.put(&mavlink_log);

11
src/modules/mavlink/mavlink_mission.cpp
View File

@@ -73,15 +73,11 @@ uint16_t MavlinkMissionManager::_safepoint_update_counter = 0;
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
_mavlink(mavlink)
{
_offboard_mission_sub = orb_subscribe(ORB_ID(mission));
_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
init_offboard_mission();
}
MavlinkMissionManager::~MavlinkMissionManager()
{
orb_unsubscribe(_mission_result_sub);
orb_unadvertise(_offboard_mission_pub);
}
@@ -489,12 +485,9 @@ MavlinkMissionManager::send(const hrt_abstime now)
return;
}
bool updated = false;
orb_check(_mission_result_sub, &updated);
mission_result_s mission_result{};
if (updated) {
mission_result_s mission_result;
orb_copy(ORB_ID(mission_result), _mission_result_sub, &mission_result);
if (_mission_result_sub.update(&mission_result)) {
if (_current_seq != mission_result.seq_current) {
_current_seq = mission_result.seq_current;

6
src/modules/mavlink/mavlink_mission.h
View File

@@ -46,7 +46,8 @@
#pragma once
#include <dataman/dataman.h>
#include <uORB/uORB.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/mission_result.h>
#include "mavlink_bridge_header.h"
#include "mavlink_rate_limiter.h"
@@ -124,8 +125,7 @@ private:
static bool _transfer_in_progress; ///< Global variable checking for current transmission
int _offboard_mission_sub{-1};
int _mission_result_sub{-1};
uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
orb_advert_t _offboard_mission_pub{nullptr};

139
src/modules/mavlink/mavlink_orb_subscription.cpp
View File

@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2014-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,144 +35,23 @@
* @file mavlink_orb_subscription.cpp
* uORB subscription implementation.
*
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Lorenz Meier <lorenz@px4.io>
*/
#include "mavlink_orb_subscription.h"
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <px4_defines.h>
#include <uORB/uORB.h>
MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, int instance) :
_topic(topic),
_instance(instance)
{
}
MavlinkOrbSubscription::~MavlinkOrbSubscription()
{
if (_fd >= 0) {
orb_unsubscribe(_fd);
}
}
orb_id_t
MavlinkOrbSubscription::get_topic() const
{
return _topic;
}
int
MavlinkOrbSubscription::get_instance() const
{
return _instance;
}
bool
MavlinkOrbSubscription::update(uint64_t *time, void *data)
MavlinkOrbSubscription::is_published()
{
// TODO this is NOT atomic operation, we can get data newer than time
// if topic was published between orb_stat and orb_copy calls.
const bool published = _sub.published();
if (!is_published()) {
return false;
}
if (published) {
return true;
uint64_t time_topic;
if (orb_stat(_fd, &time_topic)) {
/* error getting last topic publication time */
time_topic = 0;
}
if (time_topic == 0 || (time_topic != *time)) {
if (orb_copy(_topic, _fd, data) == PX4_OK) {
/* data copied successfully */
*time = time_topic;
return true;
}
} else if (!published && _subscribe_from_beginning) {
// For some topics like vehicle_command_ack, we want to subscribe
// from the beginning in order not to miss or delay the first publish respective advertise.
return _sub.forceInit();
}
return false;
}
bool
MavlinkOrbSubscription::update(void *data)
{
if (!is_published()) {
return false;
}
if (orb_copy(_topic, _fd, data) != PX4_OK) {
return false;
}
return true;
}
bool
MavlinkOrbSubscription::update_if_changed(void *data)
{
if (!is_published()) {
return false;
}
bool updated;
if (orb_check(_fd, &updated) || !updated) {
return false;
}
return update(data);
}
bool
MavlinkOrbSubscription::is_published()
{
// If we marked it as published no need to check again
if (_published) {
return true;
}
hrt_abstime now = hrt_absolute_time();
if (now - _last_pub_check < 300000) {
return false;
}
// We are checking now
_last_pub_check = now;
// 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
// from the beginning in order not to miss or delay the first publish respective advertise.
if (!_subscribe_from_beginning && orb_exists(_topic, _instance)) {
return false;
}
if (_fd < 0) {
_fd = orb_subscribe_multi(_topic, _instance);
}
bool updated;
orb_check(_fd, &updated);
if (updated) {
_published = true;
}
return _published;
}
void
MavlinkOrbSubscription::subscribe_from_beginning(bool from_beginning)
{
_subscribe_from_beginning = from_beginning;
}

32
src/modules/mavlink/mavlink_orb_subscription.h
View File

@@ -43,14 +43,14 @@
#include <drivers/drv_hrt.h>
#include <containers/List.hpp>
#include "uORB/uORB.h" // orb_id_t
#include <uORB/Subscription.hpp>
class MavlinkOrbSubscription : public ListNode<MavlinkOrbSubscription *>
{
public:
MavlinkOrbSubscription(const orb_id_t topic, int instance);
~MavlinkOrbSubscription();
MavlinkOrbSubscription(const orb_id_t topic, int instance) : _sub(topic, instance) {}
~MavlinkOrbSubscription() = default;
/**
* Check if subscription updated based on timestamp.
@@ -61,14 +61,14 @@ public:
* still copy the data.
* If no data available data buffer will be filled with zeros.
*/
bool update(uint64_t *time, void *data);
bool update(uint64_t *time, void *data) { return _sub.update(time, data); }
/**
* Copy topic data to given buffer.
*
* @return true only if topic data copied successfully.
*/
bool update(void *data);
bool update(void *data) { return _sub.copy(data); }
/**
* Check if the subscription has been updated.
@@ -76,7 +76,7 @@ public:
* @return true if there has been an update which has been
* copied successfully.
*/
bool update_if_changed(void *data);
bool update_if_changed(void *data) { return _sub.update(data); }
/**
* Check if the topic has been published.
@@ -87,28 +87,16 @@ public:
*/
bool is_published();
void subscribe_from_beginning(bool from_beginning);
void subscribe_from_beginning(bool from_beginning) { _subscribe_from_beginning = from_beginning; }
orb_id_t get_topic() const;
int get_instance() const;
int get_fd() { return _fd; }
orb_id_t get_topic() const { return _sub.get_topic(); }
int get_instance() const { return _sub.get_instance(); }
private:
const orb_id_t _topic; ///< topic metadata
const uint8_t _instance; ///< get topic instance
int _fd{-1}; ///< subscription handle
bool _published{false}; ///< topic was ever published
uORB::Subscription _sub;
bool _subscribe_from_beginning{false}; ///< we need to subscribe from the beginning, e.g. for vehicle_command_acks
hrt_abstime _last_pub_check{0}; ///< when we checked last
/* do not allow copying this class */
MavlinkOrbSubscription(const MavlinkOrbSubscription &);
MavlinkOrbSubscription operator=(const MavlinkOrbSubscription &);
};

46
src/modules/mavlink/mavlink_parameters.cpp
View File

@@ -42,33 +42,16 @@
#include <stdio.h>
#include <uORB/topics/uavcan_parameter_request.h>
#include <uORB/topics/uavcan_parameter_value.h>
#include "mavlink_parameters.h"
#include "mavlink_main.h"
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) :
_send_all_index(-1),
_uavcan_open_request_list(nullptr),
_uavcan_waiting_for_request_response(false),
_uavcan_queued_request_items(0),
_rc_param_map_pub(nullptr),
_rc_param_map(),
_uavcan_parameter_request_pub(nullptr),
_uavcan_parameter_value_sub(-1),
_mavlink_parameter_sub(-1),
_param_update_time(0),
_param_update_index(0),
_mavlink(mavlink)
{
}
MavlinkParametersManager::~MavlinkParametersManager()
{
if (_uavcan_parameter_value_sub >= 0) {
orb_unsubscribe(_uavcan_parameter_value_sub);
}
if (_uavcan_parameter_request_pub) {
orb_unadvertise(_uavcan_parameter_request_pub);
}
@@ -349,18 +332,10 @@ MavlinkParametersManager::send_untransmitted()
{
bool sent_one = false;
// Check for untransmitted system parameters
if (_mavlink_parameter_sub < 0) {
_mavlink_parameter_sub = orb_subscribe(ORB_ID(parameter_update));
}
bool param_ready;
orb_check(_mavlink_parameter_sub, &param_ready);
if (param_ready) {
if (_mavlink_parameter_sub.updated()) {
// Clear the ready flag
struct parameter_update_s value;
orb_copy(ORB_ID(parameter_update), _mavlink_parameter_sub, &value);
parameter_update_s value;
_mavlink_parameter_sub.update(&value);
// Schedule an update if not already the case
if (_param_update_time == 0) {
@@ -408,16 +383,9 @@ bool
MavlinkParametersManager::send_uavcan()
{
/* Send parameter values received from the UAVCAN topic */
if (_uavcan_parameter_value_sub < 0) {
_uavcan_parameter_value_sub = orb_subscribe(ORB_ID(uavcan_parameter_value));
}
bool param_value_ready;
orb_check(_uavcan_parameter_value_sub, &param_value_ready);
if (param_value_ready) {
struct uavcan_parameter_value_s value;
orb_copy(ORB_ID(uavcan_parameter_value), _uavcan_parameter_value_sub, &value);
if (_uavcan_parameter_value_sub.updated()) {
uavcan_parameter_value_s value;
_uavcan_parameter_value_sub.update(&value);
// Check if we received a matching parameter, drop it from the list and request the next
if (_uavcan_open_request_list != nullptr

25
src/modules/mavlink/mavlink_parameters.h
View File

@@ -46,8 +46,10 @@
#include "mavlink_bridge_header.h"
#include <uORB/uORB.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/rc_parameter_map.h>
#include <uORB/topics/uavcan_parameter_request.h>
#include <uORB/topics/uavcan_parameter_value.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
@@ -70,7 +72,7 @@ public:
void handle_message(const mavlink_message_t *msg);
private:
int _send_all_index;
int _send_all_index{-1};
/* do not allow top copying this class */
MavlinkParametersManager(MavlinkParametersManager &);
@@ -119,18 +121,19 @@ protected:
*/
void dequeue_uavcan_request();
_uavcan_open_request_list_item *_uavcan_open_request_list; ///< Pointer to the first item in the linked list
bool _uavcan_waiting_for_request_response; ///< We have reqested a parameter and wait for the response
uint16_t _uavcan_queued_request_items; ///< Number of stored parameter requests currently in the list
_uavcan_open_request_list_item *_uavcan_open_request_list{nullptr}; ///< Pointer to the first item in the linked list
bool _uavcan_waiting_for_request_response{false}; ///< We have reqested a parameter and wait for the response
uint16_t _uavcan_queued_request_items{0}; ///< Number of stored parameter requests currently in the list
orb_advert_t _rc_param_map_pub;
struct rc_parameter_map_s _rc_param_map;
orb_advert_t _rc_param_map_pub{nullptr};
rc_parameter_map_s _rc_param_map{};
orb_advert_t _uavcan_parameter_request_pub;
int _uavcan_parameter_value_sub;
int _mavlink_parameter_sub;
hrt_abstime _param_update_time;
int _param_update_index;
orb_advert_t _uavcan_parameter_request_pub{nullptr};
uORB::Subscription _uavcan_parameter_value_sub{ORB_ID(uavcan_parameter_value)};
uORB::Subscription _mavlink_parameter_sub{ORB_ID(parameter_update)};
hrt_abstime _param_update_time{0};
int _param_update_index{0};
Mavlink *_mavlink;
};

48
src/modules/mavlink/mavlink_receiver.cpp
View File

@@ -101,13 +101,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_att.q[0] = 1.0f;
}
MavlinkReceiver::~MavlinkReceiver()
{
orb_unsubscribe(_control_mode_sub);
orb_unsubscribe(_actuator_armed_sub);
orb_unsubscribe(_vehicle_attitude_sub);
}
void
MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result)
{
@@ -350,10 +343,10 @@ MavlinkReceiver::send_flight_information()
flight_info.flight_uuid = (uint64_t)flight_uuid;
}
actuator_armed_s actuator_armed;
int ret = orb_copy(ORB_ID(actuator_armed), _actuator_armed_sub, &actuator_armed);
actuator_armed_s actuator_armed{};
bool ret = _actuator_armed_sub.copy(&actuator_armed);
if (ret == 0 && actuator_armed.timestamp != 0) {
if (ret && actuator_armed.timestamp != 0) {
flight_info.arming_time_utc = flight_info.takeoff_time_utc = actuator_armed.armed_time_ms;
}
@@ -853,12 +846,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
/* If we are in offboard control mode and offboard control loop through is enabled
* also publish the setpoint topic which is read by the controller */
if (_mavlink->get_forward_externalsp()) {
bool updated;
orb_check(_control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
}
_control_mode_sub.update(&_control_mode);
if (_control_mode.flag_control_offboard_enabled) {
if (is_force_sp && offboard_control_mode.ignore_position &&
@@ -1019,14 +1008,8 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode);
}
/* If we are in offboard control mode, publish the actuator controls */
bool updated;
orb_check(_control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
}
_control_mode_sub.update(&_control_mode);
if (_control_mode.flag_control_offboard_enabled) {
@@ -1184,9 +1167,6 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
odometry.pose_covariance[i] = odom.pose_covariance[i];
}
bool updated;
orb_check(_vehicle_attitude_sub, &updated);
if (odom.child_frame_id == MAV_FRAME_BODY_FRD) { /* WRT to estimated vehicle body-fixed frame */
/* get quaternion from the msg quaternion itself and build DCM matrix from it */
Rbl = matrix::Dcmf(matrix::Quatf(odometry.q)).I();
@@ -1207,8 +1187,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
}
} else if (odom.child_frame_id == MAV_FRAME_BODY_NED) { /* WRT to vehicle body-NED frame */
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att);
if (_vehicle_attitude_sub.update(&_att)) {
/* get quaternion from vehicle_attitude quaternion and build DCM matrix from it */
Rbl = matrix::Dcmf(matrix::Quatf(_att.q)).I();
@@ -1233,8 +1212,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
} else if (odom.child_frame_id == MAV_FRAME_VISION_NED || /* WRT to vehicle local NED frame */
odom.child_frame_id == MAV_FRAME_MOCAP_NED) {
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att);
if (_vehicle_attitude_sub.update(&_att)) {
/* get quaternion from vehicle_attitude quaternion and build DCM matrix from it */
matrix::Dcmf Rlb = matrix::Quatf(_att.q);
@@ -1340,12 +1318,8 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
/* If we are in offboard control mode and offboard control loop through is enabled
* also publish the setpoint topic which is read by the controller */
if (_mavlink->get_forward_externalsp()) {
bool updated;
orb_check(_control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
}
_control_mode_sub.update(&_control_mode);
if (_control_mode.flag_control_offboard_enabled) {
@@ -2706,12 +2680,6 @@ MavlinkReceiver::receive_thread(void *arg)
return nullptr;
}
void
MavlinkReceiver::print_status()
{
}
void *
MavlinkReceiver::start_helper(void *context)
{

17
src/modules/mavlink/mavlink_receiver.h
View File

@@ -98,16 +98,7 @@ public:
* Constructor
*/
MavlinkReceiver(Mavlink *parent);
/**
* Destructor, also kills the mavlinks task.
*/
~MavlinkReceiver();
/**
* Display the mavlink status.
*/
void print_status();
~MavlinkReceiver() = default;
/**
* Start the receiver thread
@@ -257,9 +248,9 @@ private:
static constexpr int _gps_inject_data_queue_size{6};
int _actuator_armed_sub{orb_subscribe(ORB_ID(actuator_armed))};
int _control_mode_sub{orb_subscribe(ORB_ID(vehicle_control_mode))};
int _vehicle_attitude_sub{orb_subscribe(ORB_ID(vehicle_attitude))};
uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
int _orb_class_instance{-1};

59
src/modules/mavlink/mavlink_ulog.cpp
View File

@@ -56,19 +56,9 @@ MavlinkULog::MavlinkULog(int datarate, float max_rate_factor, uint8_t target_sys
(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));
// make sure we won't read any old messages
while (_ulog_stream_sub.update()) {
if (_ulog_stream_sub < 0) {
PX4_ERR("orb_subscribe failed (%i)", errno);
} else {
// make sure we won't read any old messages
struct ulog_stream_s stream_msg;
bool update;
while (orb_check(_ulog_stream_sub, &update) == 0 && update) {
orb_copy(ORB_ID(ulog_stream), _ulog_stream_sub, &stream_msg);
}
}
_waiting_for_initial_ack = true;
@@ -81,10 +71,6 @@ MavlinkULog::~MavlinkULog()
if (_ulog_stream_ack_pub) {
orb_unadvertise(_ulog_stream_ack_pub);
}
if (_ulog_stream_sub >= 0) {
orb_unsubscribe(_ulog_stream_sub);
}
}
void MavlinkULog::start_ack_received()
@@ -129,13 +115,16 @@ int MavlinkULog::handle_update(mavlink_channel_t channel)
} else {
PX4_DEBUG("re-sending ulog mavlink message (try=%i)", _sent_tries);
_last_sent_time = hrt_absolute_time();
const ulog_stream_s &ulog_data = _ulog_stream_sub.get();
mavlink_logging_data_acked_t msg;
msg.sequence = _ulog_data.sequence;
msg.length = _ulog_data.length;
msg.first_message_offset = _ulog_data.first_message_offset;
msg.sequence = ulog_data.sequence;
msg.length = ulog_data.length;
msg.first_message_offset = ulog_data.first_message_offset;
msg.target_system = _target_system;
msg.target_component = _target_component;
memcpy(msg.data, _ulog_data.data, sizeof(msg.data));
memcpy(msg.data, ulog_data.data, sizeof(msg.data));
mavlink_msg_logging_data_acked_send_struct(channel, &msg);
}
}
@@ -146,44 +135,40 @@ int MavlinkULog::handle_update(mavlink_channel_t channel)
}
}
bool updated = false;
int ret = orb_check(_ulog_stream_sub, &updated);
while ((_current_num_msgs < _max_num_messages) && _ulog_stream_sub.update()) {
const ulog_stream_s &ulog_data = _ulog_stream_sub.get();
while (updated && !ret && _current_num_msgs < _max_num_messages) {
orb_copy(ORB_ID(ulog_stream), _ulog_stream_sub, &_ulog_data);
if (_ulog_data.timestamp > 0) {
if (_ulog_data.flags & ulog_stream_s::FLAGS_NEED_ACK) {
if (ulog_data.timestamp > 0) {
if (ulog_data.flags & ulog_stream_s::FLAGS_NEED_ACK) {
_sent_tries = 1;
_last_sent_time = hrt_absolute_time();
lock();
_wait_for_ack_sequence = _ulog_data.sequence;
_wait_for_ack_sequence = ulog_data.sequence;
_ack_received = false;
unlock();
mavlink_logging_data_acked_t msg;
msg.sequence = _ulog_data.sequence;
msg.length = _ulog_data.length;
msg.first_message_offset = _ulog_data.first_message_offset;
msg.sequence = ulog_data.sequence;
msg.length = ulog_data.length;
msg.first_message_offset = ulog_data.first_message_offset;
msg.target_system = _target_system;
msg.target_component = _target_component;
memcpy(msg.data, _ulog_data.data, sizeof(msg.data));
memcpy(msg.data, ulog_data.data, sizeof(msg.data));
mavlink_msg_logging_data_acked_send_struct(channel, &msg);
} else {
mavlink_logging_data_t msg;
msg.sequence = _ulog_data.sequence;
msg.length = _ulog_data.length;
msg.first_message_offset = _ulog_data.first_message_offset;
msg.sequence = ulog_data.sequence;
msg.length = ulog_data.length;
msg.first_message_offset = ulog_data.first_message_offset;
msg.target_system = _target_system;
msg.target_component = _target_component;
memcpy(msg.data, _ulog_data.data, sizeof(msg.data));
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?

5
src/modules/mavlink/mavlink_ulog.h
View File

@@ -46,6 +46,7 @@
#include <px4_sem.h>
#include <drivers/drv_hrt.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/ulog_stream.h>
#include <uORB/topics/ulog_stream_ack.h>
@@ -96,7 +97,6 @@ public:
float current_data_rate() const { return _current_rate_factor; }
float maximum_data_rate() const { return _max_rate_factor; }
int get_ulog_stream_fd() const { return _ulog_stream_sub; }
private:
MavlinkULog(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component);
@@ -120,13 +120,12 @@ private:
static MavlinkULog *_instance;
static const float _rate_calculation_delta_t; ///< rate update interval
int _ulog_stream_sub = -1;
uORB::SubscriptionData<ulog_stream_s> _ulog_stream_sub{ORB_ID(ulog_stream)};
orb_advert_t _ulog_stream_ack_pub = nullptr;
uint16_t _wait_for_ack_sequence;
uint8_t _sent_tries = 0;
volatile bool _ack_received = false; ///< set to true if a matching ack received
hrt_abstime _last_sent_time = 0; ///< last time we sent a message that requires an ack
ulog_stream_s _ulog_data;
bool _waiting_for_initial_ack = false;
const uint8_t _target_system;
const uint8_t _target_component;