mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
mavlink: replace MavlinkOrbSubscription with uORB::Subscription
* uORB orb_stat() and update(uint64_t *time, void *dst) are now obsolete and have been deleted * mavlink messages add more advertised checks in streams get_size() check to improve data rate calculation across different scenarios
This commit is contained in:
Daniel Agar
GitHub
parent
4f2d39cb15
commit
5fcd7932e9
@ -221,7 +221,6 @@ class Graph(object):
|
||||
# to this, so that we can ignore it)
|
||||
special_cases_sub = [
|
||||
('sensors', r'voted_sensors_update\.cpp$', r'\binitSensorClass\b\(([^,)]+)', r'^meta$'),
|
||||
('mavlink', r'.*', r'\badd_orb_subscription\b\(([^,)]+)', r'^_topic$'),
|
||||
('listener', r'.*', None, r'^(id)$'),
|
||||
('logger', r'.*', None, r'^(topic|sub\.metadata|_polling_topic_meta)$'),
|
||||
|
||||
|
||||
@ -52,9 +52,6 @@
|
||||
* IOCTLs for individual topics.
|
||||
*/
|
||||
|
||||
/** Fetch the time at which the topic was last updated into *(uint64_t *)arg */
|
||||
#define ORBIOCLASTUPDATE _ORBIOC(10)
|
||||
|
||||
/** Check whether the topic has been updated since it was last read, sets *(bool *)arg */
|
||||
#define ORBIOCUPDATED _ORBIOC(11)
|
||||
|
||||
|
||||
@ -50,7 +50,6 @@ px4_add_module(
|
||||
mavlink_main.cpp
|
||||
mavlink_messages.cpp
|
||||
mavlink_mission.cpp
|
||||
mavlink_orb_subscription.cpp
|
||||
mavlink_parameters.cpp
|
||||
mavlink_rate_limiter.cpp
|
||||
mavlink_receiver.cpp
|
||||
|
||||
@ -43,56 +43,11 @@
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/geofence_result.h>
|
||||
#include <uORB/topics/position_controller_status.h>
|
||||
#include <uORB/topics/tecs_status.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_status_flags.h>
|
||||
#include <uORB/topics/vtol_vehicle_status.h>
|
||||
#include <uORB/topics/wind_estimate.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
using matrix::wrap_2pi;
|
||||
|
||||
MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : MavlinkStream(mavlink),
|
||||
_actuator_sub_0(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
|
||||
_actuator_time_0(0),
|
||||
_actuator_sub_1(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_1))),
|
||||
_actuator_time_1(0),
|
||||
_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
|
||||
_airspeed_time(0),
|
||||
_attitude_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))),
|
||||
_attitude_sp_time(0),
|
||||
_estimator_status_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))),
|
||||
_estimator_status_time(0),
|
||||
_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(position_controller_status))),
|
||||
_pos_ctrl_status_time(0),
|
||||
_geofence_sub(_mavlink->add_orb_subscription(ORB_ID(geofence_result))),
|
||||
_geofence_time(0),
|
||||
_local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))),
|
||||
_local_pos_time(0),
|
||||
_global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
|
||||
_global_pos_time(0),
|
||||
_gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))),
|
||||
_gps_time(0),
|
||||
_mission_result_sub(_mavlink->add_orb_subscription(ORB_ID(mission_result))),
|
||||
_mission_result_time(0),
|
||||
_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
|
||||
_status_time(0),
|
||||
_status_flags_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status_flags))),
|
||||
_status_flags_time(0),
|
||||
_tecs_status_sub(_mavlink->add_orb_subscription(ORB_ID(tecs_status))),
|
||||
_tecs_time(0),
|
||||
_wind_sub(_mavlink->add_orb_subscription(ORB_ID(wind_estimate))),
|
||||
_wind_time(0),
|
||||
MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) :
|
||||
MavlinkStream(mavlink),
|
||||
_airspeed(SimpleAnalyzer::AVERAGE),
|
||||
_airspeed_sp(SimpleAnalyzer::AVERAGE),
|
||||
_climb_rate(SimpleAnalyzer::MAX),
|
||||
@ -103,11 +58,6 @@ MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : Mavlink
|
||||
_throttle(SimpleAnalyzer::AVERAGE),
|
||||
_windspeed(SimpleAnalyzer::AVERAGE)
|
||||
{
|
||||
|
||||
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
|
||||
_batteries[i].subscription = _mavlink->add_orb_subscription(ORB_ID(battery_status), i);
|
||||
}
|
||||
|
||||
reset_last_sent();
|
||||
}
|
||||
|
||||
@ -116,7 +66,7 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t)
|
||||
// only send the struct if transmitting is allowed
|
||||
// this assures that the stream timer is only reset when actually a message is transmitted
|
||||
if (_mavlink->should_transmit()) {
|
||||
mavlink_high_latency2_t msg = {};
|
||||
mavlink_high_latency2_t msg{};
|
||||
set_default_values(msg);
|
||||
|
||||
bool updated = _airspeed.valid();
|
||||
@ -218,7 +168,7 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t)
|
||||
|
||||
} else {
|
||||
// reset the analysers every 60 seconds if nothing should be transmitted
|
||||
if ((t - _last_reset_time) > 60000000) {
|
||||
if ((t - _last_reset_time) > 60_s) {
|
||||
reset_analysers(t);
|
||||
PX4_DEBUG("Resetting HIGH_LATENCY2 analysers");
|
||||
}
|
||||
@ -250,30 +200,29 @@ void MavlinkStreamHighLatency2::reset_analysers(const hrt_abstime t)
|
||||
|
||||
bool MavlinkStreamHighLatency2::write_airspeed(mavlink_high_latency2_t *msg)
|
||||
{
|
||||
struct airspeed_s airspeed;
|
||||
airspeed_s airspeed;
|
||||
|
||||
const bool updated = _airspeed_sub->update(&_airspeed_time, &airspeed);
|
||||
|
||||
if (_airspeed_time > 0) {
|
||||
if (_airspeed_sub.update(&airspeed)) {
|
||||
if (airspeed.confidence < 0.95f) { // the same threshold as for the commander
|
||||
msg->failure_flags |= HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return updated;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool MavlinkStreamHighLatency2::write_attitude_sp(mavlink_high_latency2_t *msg)
|
||||
{
|
||||
struct vehicle_attitude_setpoint_s attitude_sp;
|
||||
vehicle_attitude_setpoint_s attitude_sp;
|
||||
|
||||
const bool updated = _attitude_sp_sub->update(&_attitude_sp_time, &attitude_sp);
|
||||
|
||||
if (_attitude_sp_time > 0) {
|
||||
if (_attitude_sp_sub.update(&attitude_sp)) {
|
||||
msg->target_heading = static_cast<uint8_t>(math::degrees(wrap_2pi(attitude_sp.yaw_body)) * 0.5f);
|
||||
return true;
|
||||
}
|
||||
|
||||
return updated;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool MavlinkStreamHighLatency2::write_battery_status(mavlink_high_latency2_t *msg)
|
||||
@ -282,7 +231,7 @@ bool MavlinkStreamHighLatency2::write_battery_status(mavlink_high_latency2_t *ms
|
||||
bool updated = false;
|
||||
|
||||
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
|
||||
if (_batteries[i].subscription->update(&_batteries[i].timestamp, &battery)) {
|
||||
if (_batteries[i].subscription.update(&battery)) {
|
||||
updated = true;
|
||||
_batteries[i].connected = battery.connected;
|
||||
|
||||
@ -297,58 +246,56 @@ bool MavlinkStreamHighLatency2::write_battery_status(mavlink_high_latency2_t *ms
|
||||
|
||||
bool MavlinkStreamHighLatency2::write_estimator_status(mavlink_high_latency2_t *msg)
|
||||
{
|
||||
struct estimator_status_s estimator_status;
|
||||
estimator_status_s estimator_status;
|
||||
|
||||
const bool updated = _estimator_status_sub->update(&_estimator_status_time, &estimator_status);
|
||||
|
||||
if (_estimator_status_time > 0) {
|
||||
if (_estimator_status_sub.update(&estimator_status)) {
|
||||
if (estimator_status.gps_check_fail_flags > 0 ||
|
||||
estimator_status.filter_fault_flags > 0 ||
|
||||
estimator_status.innovation_check_flags > 0) {
|
||||
|
||||
msg->failure_flags |= HL_FAILURE_FLAG_ESTIMATOR;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return updated;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool MavlinkStreamHighLatency2::write_fw_ctrl_status(mavlink_high_latency2_t *msg)
|
||||
{
|
||||
position_controller_status_s pos_ctrl_status = {};
|
||||
position_controller_status_s pos_ctrl_status;
|
||||
|
||||
const bool updated = _pos_ctrl_status_sub->update(&_pos_ctrl_status_time, &pos_ctrl_status);
|
||||
|
||||
if (_pos_ctrl_status_time > 0) {
|
||||
if (_pos_ctrl_status_sub.update(&pos_ctrl_status)) {
|
||||
uint16_t target_distance;
|
||||
convert_limit_safe(pos_ctrl_status.wp_dist * 0.1f, target_distance);
|
||||
msg->target_distance = target_distance;
|
||||
return true;
|
||||
}
|
||||
|
||||
return updated;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool MavlinkStreamHighLatency2::write_geofence_result(mavlink_high_latency2_t *msg)
|
||||
{
|
||||
struct geofence_result_s geofence;
|
||||
geofence_result_s geofence;
|
||||
|
||||
const bool updated = _geofence_sub->update(&_geofence_time, &geofence);
|
||||
|
||||
if (_geofence_time > 0) {
|
||||
if (_geofence_sub.update(&geofence)) {
|
||||
if (geofence.geofence_violated) {
|
||||
msg->failure_flags |= HL_FAILURE_FLAG_GEOFENCE;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return updated;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool MavlinkStreamHighLatency2::write_global_position(mavlink_high_latency2_t *msg)
|
||||
{
|
||||
struct vehicle_global_position_s global_pos;
|
||||
vehicle_global_position_s global_pos;
|
||||
|
||||
const bool updated = _global_pos_sub->update(&_global_pos_time, &global_pos);
|
||||
|
||||
if (_global_pos_time > 0) {
|
||||
if (_global_pos_sub.update(&global_pos)) {
|
||||
msg->latitude = global_pos.lat * 1e7;
|
||||
msg->longitude = global_pos.lon * 1e7;
|
||||
|
||||
@ -364,46 +311,45 @@ bool MavlinkStreamHighLatency2::write_global_position(mavlink_high_latency2_t *m
|
||||
msg->altitude = altitude;
|
||||
|
||||
msg->heading = static_cast<uint8_t>(math::degrees(wrap_2pi(global_pos.yaw)) * 0.5f);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return updated;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool MavlinkStreamHighLatency2::write_mission_result(mavlink_high_latency2_t *msg)
|
||||
{
|
||||
struct mission_result_s mission_result;
|
||||
mission_result_s mission_result;
|
||||
|
||||
const bool updated = _mission_result_sub->update(&_mission_result_time, &mission_result);
|
||||
|
||||
if (_mission_result_time > 0) {
|
||||
if (_mission_result_sub.update(&mission_result)) {
|
||||
msg->wp_num = mission_result.seq_current;
|
||||
return true;
|
||||
}
|
||||
|
||||
return updated;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool MavlinkStreamHighLatency2::write_tecs_status(mavlink_high_latency2_t *msg)
|
||||
{
|
||||
struct tecs_status_s tecs_status;
|
||||
tecs_status_s tecs_status;
|
||||
|
||||
const bool updated = _tecs_status_sub->update(&_tecs_time, &tecs_status);
|
||||
|
||||
if (_tecs_time > 0) {
|
||||
if (_tecs_status_sub.update(&tecs_status)) {
|
||||
int16_t target_altitude;
|
||||
convert_limit_safe(tecs_status.altitude_sp, target_altitude);
|
||||
msg->target_altitude = target_altitude;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return updated;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool MavlinkStreamHighLatency2::write_vehicle_status(mavlink_high_latency2_t *msg)
|
||||
{
|
||||
struct vehicle_status_s status;
|
||||
vehicle_status_s status;
|
||||
|
||||
const bool updated = _status_sub->update(&_status_time, &status);
|
||||
|
||||
if (_status_time > 0) {
|
||||
if (_status_sub.update(&status)) {
|
||||
if ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE)
|
||||
&& !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE)) {
|
||||
msg->failure_flags |= HL_FAILURE_FLAG_ABSOLUTE_PRESSURE;
|
||||
@ -452,18 +398,18 @@ bool MavlinkStreamHighLatency2::write_vehicle_status(mavlink_high_latency2_t *ms
|
||||
uint8_t mavlink_base_mode;
|
||||
get_mavlink_navigation_mode(&status, &mavlink_base_mode, &custom_mode);
|
||||
msg->custom_mode = custom_mode.custom_mode_hl;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return updated;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool MavlinkStreamHighLatency2::write_vehicle_status_flags(mavlink_high_latency2_t *msg)
|
||||
{
|
||||
struct vehicle_status_flags_s status_flags;
|
||||
vehicle_status_flags_s status_flags;
|
||||
|
||||
const bool updated = _status_flags_sub->update(&_status_flags_time, &status_flags);
|
||||
|
||||
if (_status_flags_time > 0) {
|
||||
if (_status_flags_sub.update(&status_flags)) {
|
||||
if (!status_flags.condition_global_position_valid) { //TODO check if there is a better way to get only GPS failure
|
||||
msg->failure_flags |= HL_FAILURE_FLAG_GPS;
|
||||
}
|
||||
@ -471,23 +417,24 @@ bool MavlinkStreamHighLatency2::write_vehicle_status_flags(mavlink_high_latency2
|
||||
if (status_flags.offboard_control_signal_lost && status_flags.offboard_control_signal_found_once) {
|
||||
msg->failure_flags |= HL_FAILURE_FLAG_OFFBOARD_LINK;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return updated;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool MavlinkStreamHighLatency2::write_wind_estimate(mavlink_high_latency2_t *msg)
|
||||
{
|
||||
struct wind_estimate_s wind;
|
||||
wind_estimate_s wind;
|
||||
|
||||
const bool updated = _wind_sub->update(&_wind_time, &wind);
|
||||
|
||||
if (_wind_time > 0) {
|
||||
msg->wind_heading = static_cast<uint8_t>(
|
||||
math::degrees(wrap_2pi(atan2f(wind.windspeed_east, wind.windspeed_north))) * 0.5f);
|
||||
if (_wind_sub.update(&wind)) {
|
||||
msg->wind_heading = static_cast<uint8_t>(math::degrees(wrap_2pi(atan2f(wind.windspeed_east,
|
||||
wind.windspeed_north))) * 0.5f);
|
||||
return true;
|
||||
}
|
||||
|
||||
return updated;
|
||||
return false;
|
||||
}
|
||||
|
||||
void MavlinkStreamHighLatency2::update_data()
|
||||
@ -519,7 +466,7 @@ void MavlinkStreamHighLatency2::update_airspeed()
|
||||
{
|
||||
airspeed_s airspeed;
|
||||
|
||||
if (_airspeed_sub->update(&airspeed)) {
|
||||
if (_airspeed_sub.update(&airspeed)) {
|
||||
_airspeed.add_value(airspeed.indicated_airspeed_m_s, _update_rate_filtered);
|
||||
_temperature.add_value(airspeed.air_temperature_celsius, _update_rate_filtered);
|
||||
}
|
||||
@ -529,7 +476,7 @@ void MavlinkStreamHighLatency2::update_tecs_status()
|
||||
{
|
||||
tecs_status_s tecs_status;
|
||||
|
||||
if (_tecs_status_sub->update(&tecs_status)) {
|
||||
if (_tecs_status_sub.update(&tecs_status)) {
|
||||
_airspeed_sp.add_value(tecs_status.airspeed_sp, _update_rate_filtered);
|
||||
}
|
||||
}
|
||||
@ -539,7 +486,7 @@ void MavlinkStreamHighLatency2::update_battery_status()
|
||||
battery_status_s battery;
|
||||
|
||||
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
|
||||
if (_batteries[i].subscription->update(&battery)) {
|
||||
if (_batteries[i].subscription.update(&battery)) {
|
||||
_batteries[i].connected = battery.connected;
|
||||
_batteries[i].analyzer.add_value(battery.remaining, _update_rate_filtered);
|
||||
}
|
||||
@ -550,7 +497,7 @@ void MavlinkStreamHighLatency2::update_local_position()
|
||||
{
|
||||
vehicle_local_position_s local_pos;
|
||||
|
||||
if (_local_pos_sub->update(&local_pos)) {
|
||||
if (_local_pos_sub.update(&local_pos)) {
|
||||
_climb_rate.add_value(fabsf(local_pos.vz), _update_rate_filtered);
|
||||
_groundspeed.add_value(sqrtf(local_pos.vx * local_pos.vx + local_pos.vy * local_pos.vy), _update_rate_filtered);
|
||||
}
|
||||
@ -560,7 +507,7 @@ void MavlinkStreamHighLatency2::update_gps()
|
||||
{
|
||||
vehicle_gps_position_s gps;
|
||||
|
||||
if (_gps_sub->update(&gps)) {
|
||||
if (_gps_sub.update(&gps)) {
|
||||
_eph.add_value(gps.eph, _update_rate_filtered);
|
||||
_epv.add_value(gps.epv, _update_rate_filtered);
|
||||
}
|
||||
@ -570,17 +517,17 @@ void MavlinkStreamHighLatency2::update_vehicle_status()
|
||||
{
|
||||
vehicle_status_s status;
|
||||
|
||||
if (_status_sub->update(&status)) {
|
||||
if (_status_sub.update(&status)) {
|
||||
if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
|
||||
struct actuator_controls_s actuator = {};
|
||||
actuator_controls_s actuator{};
|
||||
|
||||
if (status.is_vtol && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
|
||||
if (_actuator_sub_1->update(&actuator)) {
|
||||
if (_actuator_1_sub.copy(&actuator)) {
|
||||
_throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], _update_rate_filtered);
|
||||
}
|
||||
|
||||
} else {
|
||||
if (_actuator_sub_0->update(&actuator)) {
|
||||
if (_actuator_0_sub.copy(&actuator)) {
|
||||
_throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], _update_rate_filtered);
|
||||
}
|
||||
}
|
||||
@ -595,7 +542,7 @@ void MavlinkStreamHighLatency2::update_wind_estimate()
|
||||
{
|
||||
wind_estimate_s wind;
|
||||
|
||||
if (_wind_sub->update(&wind)) {
|
||||
if (_wind_sub.update(&wind)) {
|
||||
_windspeed.add_value(sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east),
|
||||
_update_rate_filtered);
|
||||
}
|
||||
|
||||
@ -44,6 +44,22 @@
|
||||
#include "mavlink_simple_analyzer.h"
|
||||
#include "mavlink_stream.h"
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/geofence_result.h>
|
||||
#include <uORB/topics/position_controller_status.h>
|
||||
#include <uORB/topics/tecs_status.h>
|
||||
#include <uORB/topics/wind_estimate.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_status_flags.h>
|
||||
|
||||
class MavlinkStreamHighLatency2 : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
@ -85,57 +101,28 @@ public:
|
||||
private:
|
||||
|
||||
struct PerBatteryData {
|
||||
PerBatteryData() {}
|
||||
MavlinkOrbSubscription *subscription{nullptr};
|
||||
PerBatteryData(uint8_t instance) : subscription(ORB_ID(battery_status), instance) {}
|
||||
uORB::Subscription subscription;
|
||||
SimpleAnalyzer analyzer{SimpleAnalyzer::AVERAGE};
|
||||
uint64_t timestamp{0};
|
||||
bool connected{false};
|
||||
};
|
||||
|
||||
MavlinkOrbSubscription *_actuator_sub_0;
|
||||
uint64_t _actuator_time_0;
|
||||
|
||||
MavlinkOrbSubscription *_actuator_sub_1;
|
||||
uint64_t _actuator_time_1;
|
||||
|
||||
MavlinkOrbSubscription *_airspeed_sub;
|
||||
uint64_t _airspeed_time;
|
||||
|
||||
MavlinkOrbSubscription *_attitude_sp_sub;
|
||||
uint64_t _attitude_sp_time;
|
||||
|
||||
MavlinkOrbSubscription *_estimator_status_sub;
|
||||
uint64_t _estimator_status_time;
|
||||
|
||||
MavlinkOrbSubscription *_pos_ctrl_status_sub;
|
||||
uint64_t _pos_ctrl_status_time;
|
||||
|
||||
MavlinkOrbSubscription *_geofence_sub;
|
||||
uint64_t _geofence_time;
|
||||
|
||||
MavlinkOrbSubscription *_local_pos_sub;
|
||||
uint64_t _local_pos_time;
|
||||
|
||||
MavlinkOrbSubscription *_global_pos_sub;
|
||||
uint64_t _global_pos_time;
|
||||
|
||||
MavlinkOrbSubscription *_gps_sub;
|
||||
uint64_t _gps_time;
|
||||
|
||||
MavlinkOrbSubscription *_mission_result_sub;
|
||||
uint64_t _mission_result_time;
|
||||
|
||||
MavlinkOrbSubscription *_status_sub;
|
||||
uint64_t _status_time;
|
||||
|
||||
MavlinkOrbSubscription *_status_flags_sub;
|
||||
uint64_t _status_flags_time;
|
||||
|
||||
MavlinkOrbSubscription *_tecs_status_sub;
|
||||
uint64_t _tecs_time;
|
||||
|
||||
MavlinkOrbSubscription *_wind_sub;
|
||||
uint64_t _wind_time;
|
||||
uORB::Subscription _actuator_0_sub{ORB_ID(actuator_controls_0)};
|
||||
uORB::Subscription _actuator_1_sub{ORB_ID(actuator_controls_1)};
|
||||
uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
|
||||
uORB::Subscription _attitude_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
|
||||
uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
|
||||
uORB::Subscription _pos_ctrl_status_sub{ORB_ID(position_controller_status)};
|
||||
uORB::Subscription _geofence_sub{ORB_ID(geofence_result)};
|
||||
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
|
||||
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _gps_sub{ORB_ID(vehicle_gps_position)};
|
||||
uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
|
||||
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _status_flags_sub{ORB_ID(vehicle_status_flags)};
|
||||
uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)};
|
||||
uORB::Subscription _wind_sub{ORB_ID(wind_estimate)};
|
||||
|
||||
SimpleAnalyzer _airspeed;
|
||||
SimpleAnalyzer _airspeed_sp;
|
||||
@ -151,7 +138,7 @@ private:
|
||||
hrt_abstime _last_update_time = 0;
|
||||
float _update_rate_filtered = 0.0f;
|
||||
|
||||
PerBatteryData _batteries[ORB_MULTI_MAX_INSTANCES];
|
||||
PerBatteryData _batteries[ORB_MULTI_MAX_INSTANCES] {0, 1, 2, 3};
|
||||
|
||||
/* do not allow top copying this class */
|
||||
MavlinkStreamHighLatency2(MavlinkStreamHighLatency2 &);
|
||||
|
||||
@ -1155,12 +1155,11 @@ Mavlink::send_statustext_emergency(const char *string)
|
||||
void
|
||||
Mavlink::send_autopilot_capabilites()
|
||||
{
|
||||
struct vehicle_status_s status;
|
||||
uORB::Subscription status_sub{ORB_ID(vehicle_status)};
|
||||
vehicle_status_s status;
|
||||
|
||||
MavlinkOrbSubscription *status_sub = this->add_orb_subscription(ORB_ID(vehicle_status));
|
||||
|
||||
if (status_sub->update(&status)) {
|
||||
mavlink_autopilot_version_t msg = {};
|
||||
if (status_sub.copy(&status)) {
|
||||
mavlink_autopilot_version_t msg{};
|
||||
|
||||
msg.capabilities = MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT;
|
||||
msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_INT;
|
||||
@ -1242,27 +1241,6 @@ Mavlink::send_protocol_version()
|
||||
set_proto_version(curr_proto_ver);
|
||||
}
|
||||
|
||||
MavlinkOrbSubscription *
|
||||
Mavlink::add_orb_subscription(const orb_id_t topic, int instance, bool disable_sharing)
|
||||
{
|
||||
if (!disable_sharing) {
|
||||
/* check if already subscribed to this topic */
|
||||
for (MavlinkOrbSubscription *sub : _subscriptions) {
|
||||
if (sub->get_topic() == topic && sub->get_instance() == instance) {
|
||||
/* already subscribed */
|
||||
return sub;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* add new subscription */
|
||||
MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, instance);
|
||||
|
||||
_subscriptions.add(sub_new);
|
||||
|
||||
return sub_new;
|
||||
}
|
||||
|
||||
int
|
||||
Mavlink::configure_stream(const char *stream_name, const float rate)
|
||||
{
|
||||
@ -2138,22 +2116,17 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
|
||||
uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)};
|
||||
|
||||
MavlinkOrbSubscription *cmd_sub = add_orb_subscription(ORB_ID(vehicle_command), 0, true);
|
||||
MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
|
||||
uint64_t status_time = 0;
|
||||
MavlinkOrbSubscription *ack_sub = add_orb_subscription(ORB_ID(vehicle_command_ack), 0, true);
|
||||
/* We don't want to miss the first advertise of an ACK, so we subscribe from the
|
||||
* beginning and not just when the topic exists. */
|
||||
ack_sub->subscribe_from_beginning(true);
|
||||
cmd_sub->subscribe_from_beginning(true);
|
||||
uORB::Subscription cmd_sub{ORB_ID(vehicle_command)};
|
||||
uORB::Subscription status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription ack_sub{ORB_ID(vehicle_command_ack)};
|
||||
|
||||
/* command ack */
|
||||
uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||
|
||||
MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log));
|
||||
uORB::Subscription mavlink_log_sub{ORB_ID(mavlink_log)};
|
||||
|
||||
vehicle_status_s status{};
|
||||
status_sub->update(&status_time, &status);
|
||||
status_sub.copy(&status);
|
||||
|
||||
/* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/
|
||||
_transmitting_enabled = true;
|
||||
@ -2268,7 +2241,7 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
|
||||
configure_sik_radio();
|
||||
|
||||
if (status_sub->update(&status_time, &status)) {
|
||||
if (status_sub.update(&status)) {
|
||||
/* switch HIL mode if required */
|
||||
set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON);
|
||||
|
||||
@ -2291,9 +2264,9 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
}
|
||||
}
|
||||
|
||||
vehicle_command_s vehicle_cmd{};
|
||||
vehicle_command_s vehicle_cmd;
|
||||
|
||||
if (cmd_sub->update_if_changed(&vehicle_cmd)) {
|
||||
if (cmd_sub.update(&vehicle_cmd)) {
|
||||
if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) &&
|
||||
(_mode == MAVLINK_MODE_IRIDIUM)) {
|
||||
if (vehicle_cmd.param1 > 0.5f) {
|
||||
@ -2330,9 +2303,9 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
|
||||
/* send command ACK */
|
||||
uint16_t current_command_ack = 0;
|
||||
vehicle_command_ack_s command_ack{};
|
||||
vehicle_command_ack_s command_ack;
|
||||
|
||||
if (ack_sub->update_if_changed(&command_ack)) {
|
||||
if (ack_sub.update(&command_ack)) {
|
||||
if (!command_ack.from_external) {
|
||||
mavlink_command_ack_t msg;
|
||||
msg.result = command_ack.result;
|
||||
@ -2351,9 +2324,9 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
}
|
||||
}
|
||||
|
||||
mavlink_log_s mavlink_log{};
|
||||
mavlink_log_s mavlink_log;
|
||||
|
||||
if (mavlink_log_sub->update_if_changed(&mavlink_log)) {
|
||||
if (mavlink_log_sub.update(&mavlink_log)) {
|
||||
_logbuffer.put(&mavlink_log);
|
||||
}
|
||||
|
||||
@ -2497,9 +2470,6 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
/* delete streams */
|
||||
_streams.clear();
|
||||
|
||||
/* delete subscriptions */
|
||||
_subscriptions.clear();
|
||||
|
||||
if (_uart_fd >= 0 && !_is_usb_uart) {
|
||||
/* close UART */
|
||||
::close(_uart_fd);
|
||||
|
||||
@ -79,7 +79,6 @@
|
||||
|
||||
#include "mavlink_command_sender.h"
|
||||
#include "mavlink_messages.h"
|
||||
#include "mavlink_orb_subscription.h"
|
||||
#include "mavlink_shell.h"
|
||||
#include "mavlink_ulog.h"
|
||||
|
||||
@ -342,15 +341,6 @@ public:
|
||||
|
||||
void handle_message(const mavlink_message_t *msg);
|
||||
|
||||
/**
|
||||
* Add a mavlink orb topic subscription while ensuring that only a single object exists
|
||||
* for a given topic id and instance.
|
||||
* @param topic orb topic id
|
||||
* @param instance topic instance
|
||||
* @param disable_sharing if true, force creating a new instance
|
||||
*/
|
||||
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance = 0, bool disable_sharing = false);
|
||||
|
||||
int get_instance_id() const { return _instance_id; }
|
||||
|
||||
/**
|
||||
@ -565,7 +555,6 @@ private:
|
||||
|
||||
unsigned _main_loop_delay{1000}; /**< mainloop delay, depends on data rate */
|
||||
|
||||
List<MavlinkOrbSubscription *> _subscriptions;
|
||||
List<MavlinkStream *> _streams;
|
||||
|
||||
MavlinkShell *_mavlink_shell{nullptr};
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -1,57 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mavlink_orb_subscription.cpp
|
||||
* uORB subscription implementation.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "mavlink_orb_subscription.h"
|
||||
|
||||
bool
|
||||
MavlinkOrbSubscription::is_published()
|
||||
{
|
||||
const bool published = _sub.advertised();
|
||||
|
||||
if (published) {
|
||||
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.subscribe();
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
@ -1,103 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mavlink_orb_subscription.h
|
||||
* uORB subscription definition.
|
||||
*
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#ifndef MAVLINK_ORB_SUBSCRIPTION_H_
|
||||
#define MAVLINK_ORB_SUBSCRIPTION_H_
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <containers/List.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
class MavlinkOrbSubscription : public ListNode<MavlinkOrbSubscription *>
|
||||
{
|
||||
public:
|
||||
|
||||
MavlinkOrbSubscription(const orb_id_t topic, int instance) : _sub(topic, instance) {}
|
||||
~MavlinkOrbSubscription() = default;
|
||||
|
||||
/**
|
||||
* Check if subscription updated based on timestamp.
|
||||
*
|
||||
* @return true only if topic was updated based on a timestamp and
|
||||
* copied to buffer successfully.
|
||||
* If topic was not updated since last check it will return false but
|
||||
* still copy the data.
|
||||
* If no data available data buffer will be filled with zeros.
|
||||
*/
|
||||
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) { return _sub.copy(data); }
|
||||
|
||||
/**
|
||||
* Check if the subscription has been updated.
|
||||
*
|
||||
* @return true if there has been an update which has been
|
||||
* copied successfully.
|
||||
*/
|
||||
bool update_if_changed(void *data) { return _sub.update(data); }
|
||||
|
||||
/**
|
||||
* Check if the topic has been published.
|
||||
*
|
||||
* This call will return true if the topic was ever published.
|
||||
* @return true if the topic has been published at least once.
|
||||
* If no data is available the buffer will be filled with zeros.
|
||||
*/
|
||||
bool is_published();
|
||||
|
||||
void subscribe_from_beginning(bool from_beginning) { _subscribe_from_beginning = from_beginning; }
|
||||
|
||||
orb_id_t get_topic() const { return _sub.get_topic(); }
|
||||
int get_instance() const { return _sub.get_instance(); }
|
||||
|
||||
private:
|
||||
|
||||
uORB::Subscription _sub;
|
||||
|
||||
bool _subscribe_from_beginning{false}; ///< we need to subscribe from the beginning, e.g. for vehicle_command_acks
|
||||
};
|
||||
|
||||
|
||||
#endif /* MAVLINK_ORB_SUBSCRIPTION_H_ */
|
||||
@ -94,19 +94,4 @@ void Subscription::unsubscribe()
|
||||
_last_generation = 0;
|
||||
}
|
||||
|
||||
bool Subscription::update(uint64_t *time, void *dst)
|
||||
{
|
||||
if ((time != nullptr) && (dst != nullptr) && advertised()) {
|
||||
// always copy data to dst regardless of update
|
||||
const uint64_t t = _node->copy_and_get_timestamp(dst, _last_generation);
|
||||
|
||||
if (*time == 0 || *time != t) {
|
||||
*time = t;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace uORB
|
||||
|
||||
@ -118,17 +118,6 @@ public:
|
||||
*/
|
||||
bool update(void *dst) { return updated() ? copy(dst) : false; }
|
||||
|
||||
/**
|
||||
* Check if subscription updated based on timestamp.
|
||||
*
|
||||
* @return true only if topic was updated based on a timestamp and
|
||||
* copied to buffer successfully.
|
||||
* If topic was not updated since last check it will return false but
|
||||
* still copy the data.
|
||||
* If no data available data buffer will be filled with zeros.
|
||||
*/
|
||||
bool update(uint64_t *time, void *dst);
|
||||
|
||||
/**
|
||||
* Copy the struct
|
||||
* @param data The uORB message struct we are updating.
|
||||
|
||||
@ -97,11 +97,6 @@ int orb_check(int handle, bool *updated)
|
||||
return uORB::Manager::get_instance()->orb_check(handle, updated);
|
||||
}
|
||||
|
||||
int orb_stat(int handle, uint64_t *time)
|
||||
{
|
||||
return uORB::Manager::get_instance()->orb_stat(handle, time);
|
||||
}
|
||||
|
||||
int orb_exists(const struct orb_metadata *meta, int instance)
|
||||
{
|
||||
return uORB::Manager::get_instance()->orb_exists(meta, instance);
|
||||
|
||||
@ -221,11 +221,6 @@ extern int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) _
|
||||
*/
|
||||
extern int orb_check(int handle, bool *updated) __EXPORT;
|
||||
|
||||
/**
|
||||
* @see uORB::Manager::orb_stat()
|
||||
*/
|
||||
extern int orb_stat(int handle, uint64_t *time) __EXPORT;
|
||||
|
||||
/**
|
||||
* @see uORB::Manager::orb_exists()
|
||||
*/
|
||||
|
||||
@ -183,19 +183,6 @@ uORB::DeviceNode::copy(void *dst, unsigned &generation)
|
||||
return updated;
|
||||
}
|
||||
|
||||
uint64_t
|
||||
uORB::DeviceNode::copy_and_get_timestamp(void *dst, unsigned &generation)
|
||||
{
|
||||
ATOMIC_ENTER;
|
||||
|
||||
const hrt_abstime update_time = _last_update;
|
||||
copy_locked(dst, generation);
|
||||
|
||||
ATOMIC_LEAVE;
|
||||
|
||||
return update_time;
|
||||
}
|
||||
|
||||
ssize_t
|
||||
uORB::DeviceNode::read(cdev::file_t *filp, char *buffer, size_t buflen)
|
||||
{
|
||||
@ -216,13 +203,13 @@ uORB::DeviceNode::read(cdev::file_t *filp, char *buffer, size_t buflen)
|
||||
*/
|
||||
ATOMIC_ENTER;
|
||||
|
||||
copy_locked(buffer, sd->generation);
|
||||
|
||||
// if subscriber has an interval track the last update time
|
||||
if (sd->update_interval) {
|
||||
sd->update_interval->last_update = _last_update;
|
||||
sd->update_interval->last_update = hrt_absolute_time();
|
||||
}
|
||||
|
||||
copy_locked(buffer, sd->generation);
|
||||
|
||||
ATOMIC_LEAVE;
|
||||
|
||||
return _meta->o_size;
|
||||
@ -279,10 +266,6 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)
|
||||
|
||||
memcpy(_data + (_meta->o_size * (generation % _queue_size)), buffer, _meta->o_size);
|
||||
|
||||
/* update the timestamp and generation count */
|
||||
_last_update = hrt_absolute_time();
|
||||
|
||||
|
||||
// callbacks
|
||||
for (auto item : _callbacks) {
|
||||
item->call();
|
||||
@ -302,13 +285,6 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
|
||||
SubscriberData *sd = filp_to_sd(filp);
|
||||
|
||||
switch (cmd) {
|
||||
case ORBIOCLASTUPDATE: {
|
||||
ATOMIC_ENTER;
|
||||
*(hrt_abstime *)arg = _last_update;
|
||||
ATOMIC_LEAVE;
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
case ORBIOCUPDATED: {
|
||||
ATOMIC_ENTER;
|
||||
*(bool *)arg = appears_updated(sd);
|
||||
|
||||
@ -214,21 +214,6 @@ public:
|
||||
*/
|
||||
bool copy(void *dst, unsigned &generation);
|
||||
|
||||
/**
|
||||
* Copies data and the corresponding generation
|
||||
* from a node to the buffer provided.
|
||||
*
|
||||
* @param dst
|
||||
* The buffer into which the data is copied.
|
||||
* If topic was not updated since last check it will return false but
|
||||
* still copy the data.
|
||||
* @param generation
|
||||
* The generation that was copied.
|
||||
* @return uint64_t
|
||||
* Returns the timestamp of the copied data.
|
||||
*/
|
||||
uint64_t copy_and_get_timestamp(void *dst, unsigned &generation);
|
||||
|
||||
// add item to list of work items to schedule on node update
|
||||
bool register_callback(SubscriptionCallback *callback_sub);
|
||||
|
||||
@ -269,20 +254,21 @@ private:
|
||||
};
|
||||
|
||||
const orb_metadata *_meta; /**< object metadata information */
|
||||
const uint8_t _instance; /**< orb multi instance identifier */
|
||||
|
||||
uint8_t *_data{nullptr}; /**< allocated object buffer */
|
||||
hrt_abstime _last_update{0}; /**< time the object was last updated */
|
||||
px4::atomic<unsigned> _generation{0}; /**< object generation count */
|
||||
List<uORB::SubscriptionCallback *> _callbacks;
|
||||
uint8_t _priority; /**< priority of the topic */
|
||||
bool _advertised{false}; /**< has ever been advertised (not necessarily published data yet) */
|
||||
uint8_t _queue_size; /**< maximum number of elements in the queue */
|
||||
int8_t _subscriber_count{0};
|
||||
|
||||
// statistics
|
||||
uint32_t _lost_messages = 0; /**< nr of lost messages for all subscribers. If two subscribers lose the same
|
||||
message, it is counted as two. */
|
||||
|
||||
const uint8_t _instance; /**< orb multi instance identifier */
|
||||
uint8_t _priority; /**< priority of the topic */
|
||||
bool _advertised{false}; /**< has ever been advertised (not necessarily published data yet) */
|
||||
uint8_t _queue_size; /**< maximum number of elements in the queue */
|
||||
int8_t _subscriber_count{0};
|
||||
|
||||
inline static SubscriberData *filp_to_sd(cdev::file_t *filp);
|
||||
|
||||
/**
|
||||
|
||||
@ -305,11 +305,6 @@ int uORB::Manager::orb_check(int handle, bool *updated)
|
||||
return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_stat(int handle, uint64_t *time)
|
||||
{
|
||||
return px4_ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_priority(int handle, int32_t *priority)
|
||||
{
|
||||
return px4_ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority);
|
||||
|
||||
@ -177,7 +177,7 @@ public:
|
||||
*
|
||||
* The data is atomically published to the topic and any waiting subscribers
|
||||
* will be notified. Subscribers that are not waiting can check the topic
|
||||
* for updates using orb_check and/or orb_stat.
|
||||
* for updates using orb_check.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
@ -192,7 +192,7 @@ public:
|
||||
*
|
||||
* The returned value is a file descriptor that can be passed to poll()
|
||||
* in order to wait for updates to a topic, as well as topic_read,
|
||||
* orb_check and orb_stat.
|
||||
* orb_check.
|
||||
*
|
||||
* If there were any publications of the topic prior to the subscription,
|
||||
* an orb_check right after orb_subscribe will return true.
|
||||
@ -222,7 +222,7 @@ public:
|
||||
*
|
||||
* The returned value is a file descriptor that can be passed to poll()
|
||||
* in order to wait for updates to a topic, as well as topic_read,
|
||||
* orb_check and orb_stat.
|
||||
* orb_check.
|
||||
*
|
||||
* If there were any publications of the topic prior to the subscription,
|
||||
* an orb_check right after orb_subscribe_multi will return true.
|
||||
@ -289,9 +289,7 @@ public:
|
||||
* topic is likely to have updated.
|
||||
*
|
||||
* Updates are tracked on a per-handle basis; this call will continue to
|
||||
* return true until orb_copy is called using the same handle. This interface
|
||||
* should be preferred over calling orb_stat due to the race window between
|
||||
* stat and copy that can lead to missed updates.
|
||||
* return true until orb_copy is called using the same handle.
|
||||
*
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @param updated Set to true if the topic has been updated since the
|
||||
@ -301,17 +299,6 @@ public:
|
||||
*/
|
||||
int orb_check(int handle, bool *updated);
|
||||
|
||||
/**
|
||||
* Return the last time that the topic was updated. If a queue is used, it returns
|
||||
* the timestamp of the latest element in the queue.
|
||||
*
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @param time Returns the absolute time that the topic was updated, or zero if it has
|
||||
* never been updated. Time is measured in microseconds.
|
||||
* @return OK on success, PX4_ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_stat(int handle, uint64_t *time);
|
||||
|
||||
/**
|
||||
* Check if a topic has already been created and published (advertised)
|
||||
*
|
||||
|
||||
@ -150,19 +150,16 @@ bool MicroBenchORB::time_px4_uorb()
|
||||
uint64_t time = 0;
|
||||
|
||||
PERF("orb_check vehicle_status", ret = orb_check(fd_status, &updated), 1000);
|
||||
PERF("orb_stat vehicle_status", ret = orb_stat(fd_status, &time), 1000);
|
||||
PERF("orb_copy vehicle_status", ret = orb_copy(ORB_ID(vehicle_status), fd_status, &status), 1000);
|
||||
|
||||
printf("\n");
|
||||
|
||||
PERF("orb_check vehicle_local_position", ret = orb_check(fd_lpos, &updated), 1000);
|
||||
PERF("orb_stat vehicle_local_position", ret = orb_stat(fd_lpos, &time), 1000);
|
||||
PERF("orb_copy vehicle_local_position", ret = orb_copy(ORB_ID(vehicle_local_position), fd_lpos, &lpos), 1000);
|
||||
|
||||
printf("\n");
|
||||
|
||||
PERF("orb_check sensor_gyro", ret = orb_check(fd_gyro, &updated), 1000);
|
||||
PERF("orb_stat sensor_gyro", ret = orb_stat(fd_gyro, &time), 1000);
|
||||
PERF("orb_copy sensor_gyro", ret = orb_copy(ORB_ID(sensor_gyro), fd_gyro, &gyro), 1000);
|
||||
|
||||
printf("\n");
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user