fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 02:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: replace MavlinkOrbSubscription with uORB::Subscription

Browse Source 
* 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
2020-03-14 12:52:46 -04:00
committed by GitHub
parent 4f2d39cb15
commit 5fcd7932e9
19 changed files with 718 additions and 1320 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_high_latency2.cpp
+69 -122
View File
@@ -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);
}