mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-20 11:23:06 +08:00
mavlink receiver move to uORB::Publication
This commit is contained in:
Daniel Agar
File diff suppressed because it is too large
Load Diff
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-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
|
||||
@@ -41,13 +41,20 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_module_params.h>
|
||||
#include "mavlink_ftp.h"
|
||||
#include "mavlink_log_handler.h"
|
||||
#include "mavlink_mission.h"
|
||||
#include "mavlink_parameters.h"
|
||||
#include "mavlink_timesync.h"
|
||||
|
||||
#include <px4_module_params.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/PublicationQueued.hpp>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/collision_report.h>
|
||||
#include <uORB/topics/debug_array.h>
|
||||
@@ -58,6 +65,7 @@
|
||||
#include <uORB/topics/follow_target.h>
|
||||
#include <uORB/topics/gps_inject_data.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/input_rc.h>
|
||||
#include <uORB/topics/landing_target_pose.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/obstacle_distance.h>
|
||||
@@ -65,8 +73,13 @@
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/ping.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/radio_status.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/sensor_accel.h>
|
||||
#include <uORB/topics/sensor_baro.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/sensor_gyro.h>
|
||||
#include <uORB/topics/sensor_mag.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
#include <uORB/topics/transponder_report.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
@@ -74,8 +87,8 @@
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_local_position_setpoint.h>
|
||||
@@ -84,20 +97,11 @@
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_trajectory_waypoint.h>
|
||||
|
||||
#include "mavlink_ftp.h"
|
||||
#include "mavlink_log_handler.h"
|
||||
#include "mavlink_mission.h"
|
||||
#include "mavlink_parameters.h"
|
||||
#include "mavlink_timesync.h"
|
||||
|
||||
class Mavlink;
|
||||
|
||||
class MavlinkReceiver : public ModuleParams
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
MavlinkReceiver(Mavlink *parent);
|
||||
~MavlinkReceiver() = default;
|
||||
|
||||
@@ -120,8 +124,8 @@ private:
|
||||
const vehicle_command_s &vehicle_command);
|
||||
|
||||
void handle_message(mavlink_message_t *msg);
|
||||
|
||||
void handle_message_adsb_vehicle(mavlink_message_t *msg);
|
||||
void handle_message_utm_global_position(mavlink_message_t *msg);
|
||||
void handle_message_att_pos_mocap(mavlink_message_t *msg);
|
||||
void handle_message_battery_status(mavlink_message_t *msg);
|
||||
void handle_message_collision(mavlink_message_t *msg);
|
||||
@@ -157,9 +161,11 @@ private:
|
||||
void handle_message_set_mode(mavlink_message_t *msg);
|
||||
void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
|
||||
void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg);
|
||||
void handle_message_utm_global_position(mavlink_message_t *msg);
|
||||
void handle_message_vision_position_estimate(mavlink_message_t *msg);
|
||||
|
||||
void *receive_thread(void *arg);
|
||||
|
||||
void Run();
|
||||
|
||||
/**
|
||||
* Set the interval at which the given message stream is published.
|
||||
@@ -187,7 +193,6 @@ private:
|
||||
bool evaluate_target_ok(int command, int target_system, int target_component);
|
||||
|
||||
void send_flight_information();
|
||||
|
||||
void send_storage_information(int storage_id);
|
||||
|
||||
/**
|
||||
@@ -195,96 +200,87 @@ private:
|
||||
*/
|
||||
void update_params();
|
||||
|
||||
Mavlink *_mavlink;
|
||||
Mavlink *_mavlink;
|
||||
|
||||
MavlinkFTP _mavlink_ftp;
|
||||
MavlinkLogHandler _mavlink_log_handler;
|
||||
MavlinkTimesync _mavlink_timesync;
|
||||
|
||||
MavlinkMissionManager _mission_manager;
|
||||
MavlinkParametersManager _parameters_manager;
|
||||
MavlinkTimesync _mavlink_timesync;
|
||||
|
||||
mavlink_status_t _status{}; ///< receiver status, used for mavlink_parse_char()
|
||||
mavlink_status_t _status{}; ///< receiver status, used for mavlink_parse_char()
|
||||
|
||||
map_projection_reference_s _hil_local_proj_ref {};
|
||||
offboard_control_mode_s _offboard_control_mode{};
|
||||
// ORB publications
|
||||
uORB::Publication<actuator_controls_s> _actuator_controls_pubs[4] {ORB_ID(actuator_controls_0), ORB_ID(actuator_controls_1), ORB_ID(actuator_controls_2), ORB_ID(actuator_controls_3)};
|
||||
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)};
|
||||
uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
|
||||
uORB::Publication<collision_report_s> _collision_report_pub{ORB_ID(collision_report)};
|
||||
uORB::Publication<debug_array_s> _debug_array_pub{ORB_ID(debug_array)};
|
||||
uORB::Publication<debug_key_value_s> _debug_key_value_pub{ORB_ID(debug_key_value)};
|
||||
uORB::Publication<debug_value_s> _debug_value_pub{ORB_ID(debug_value)};
|
||||
uORB::Publication<debug_vect_s> _debug_vect_pub{ORB_ID(debug_vect)};
|
||||
uORB::Publication<follow_target_s> _follow_target_pub{ORB_ID(follow_target)};
|
||||
uORB::Publication<landing_target_pose_s> _landing_target_pose_pub{ORB_ID(landing_target_pose)};
|
||||
uORB::Publication<obstacle_distance_s> _obstacle_distance_pub{ORB_ID(obstacle_distance)};
|
||||
uORB::Publication<offboard_control_mode_s> _offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
|
||||
uORB::Publication<optical_flow_s> _flow_pub{ORB_ID(optical_flow)};
|
||||
uORB::Publication<position_setpoint_triplet_s> _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)};
|
||||
uORB::Publication<vehicle_attitude_s> _attitude_pub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Publication<vehicle_attitude_setpoint_s> _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
|
||||
uORB::Publication<vehicle_global_position_s> _global_pos_pub{ORB_ID(vehicle_global_position)};
|
||||
uORB::Publication<vehicle_gps_position_s> _gps_pub{ORB_ID(vehicle_gps_position)};
|
||||
uORB::Publication<vehicle_land_detected_s> _land_detector_pub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Publication<vehicle_local_position_s> _local_pos_pub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
|
||||
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
|
||||
uORB::Publication<vehicle_rates_setpoint_s> _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
|
||||
uORB::Publication<vehicle_trajectory_waypoint_s> _trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)};
|
||||
|
||||
vehicle_attitude_s _att {};
|
||||
vehicle_local_position_s _hil_local_pos {};
|
||||
vehicle_land_detected_s _hil_land_detector {};
|
||||
vehicle_control_mode_s _control_mode {};
|
||||
// ORB publications (multi)
|
||||
uORB::Publication<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW};
|
||||
uORB::Publication<distance_sensor_s> _flow_distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW};
|
||||
uORB::Publication<input_rc_s> _rc_pub{ORB_ID(input_rc), ORB_PRIO_LOW};
|
||||
uORB::Publication<manual_control_setpoint_s> _manual_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_LOW};
|
||||
uORB::Publication<ping_s> _ping_pub{ORB_ID(ping), ORB_PRIO_LOW};
|
||||
uORB::Publication<radio_status_s> _radio_status_pub{ORB_ID(radio_status), ORB_PRIO_LOW};
|
||||
uORB::Publication<sensor_accel_s> _accel_pub{ORB_ID(sensor_accel), ORB_PRIO_LOW};
|
||||
uORB::Publication<sensor_baro_s> _baro_pub{ORB_ID(sensor_baro), ORB_PRIO_LOW};
|
||||
uORB::Publication<sensor_gyro_s> _gyro_pub{ORB_ID(sensor_gyro), ORB_PRIO_LOW};
|
||||
uORB::Publication<sensor_mag_s> _mag_pub{ORB_ID(sensor_mag), ORB_PRIO_LOW};
|
||||
|
||||
uORB::PublicationQueued<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
|
||||
uORB::PublicationQueued<transponder_report_s> _transponder_report_pub{ORB_ID(transponder_report)};
|
||||
uORB::PublicationQueued<vehicle_command_s> _cmd_pub{ORB_ID(vehicle_command)};
|
||||
uORB::PublicationQueued<vehicle_command_ack_s> _cmd_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||
// ORB publications (queue length > 1)
|
||||
uORB::PublicationQueued<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
|
||||
uORB::PublicationQueued<transponder_report_s> _transponder_report_pub{ORB_ID(transponder_report)};
|
||||
uORB::PublicationQueued<vehicle_command_ack_s> _cmd_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||
uORB::PublicationQueued<vehicle_command_s> _cmd_pub{ORB_ID(vehicle_command)};
|
||||
|
||||
orb_advert_t _accel_pub{nullptr};
|
||||
orb_advert_t _actuator_controls_pubs[4] {nullptr, nullptr, nullptr, nullptr};
|
||||
orb_advert_t _airspeed_pub{nullptr};
|
||||
orb_advert_t _att_sp_pub{nullptr};
|
||||
orb_advert_t _attitude_pub{nullptr};
|
||||
orb_advert_t _baro_pub{nullptr};
|
||||
orb_advert_t _battery_pub{nullptr};
|
||||
orb_advert_t _collision_report_pub{nullptr};
|
||||
orb_advert_t _debug_array_pub{nullptr};
|
||||
orb_advert_t _debug_key_value_pub{nullptr};
|
||||
orb_advert_t _debug_value_pub{nullptr};
|
||||
orb_advert_t _debug_vect_pub{nullptr};
|
||||
orb_advert_t _distance_sensor_pub{nullptr};
|
||||
orb_advert_t _flow_distance_sensor_pub{nullptr};
|
||||
orb_advert_t _flow_pub{nullptr};
|
||||
orb_advert_t _follow_target_pub{nullptr};
|
||||
orb_advert_t _global_pos_pub{nullptr};
|
||||
orb_advert_t _gps_pub{nullptr};
|
||||
orb_advert_t _gyro_pub{nullptr};
|
||||
orb_advert_t _hil_distance_sensor_pub{nullptr};
|
||||
orb_advert_t _land_detector_pub{nullptr};
|
||||
orb_advert_t _landing_target_pose_pub{nullptr};
|
||||
orb_advert_t _local_pos_pub{nullptr};
|
||||
orb_advert_t _mag_pub{nullptr};
|
||||
orb_advert_t _manual_pub{nullptr};
|
||||
orb_advert_t _mocap_odometry_pub{nullptr};
|
||||
orb_advert_t _obstacle_distance_pub{nullptr};
|
||||
orb_advert_t _offboard_control_mode_pub{nullptr};
|
||||
orb_advert_t _ping_pub{nullptr};
|
||||
orb_advert_t _pos_sp_triplet_pub{nullptr};
|
||||
orb_advert_t _radio_status_pub{nullptr};
|
||||
orb_advert_t _rates_sp_pub{nullptr};
|
||||
orb_advert_t _rc_pub{nullptr};
|
||||
orb_advert_t _trajectory_waypoint_pub{nullptr};
|
||||
orb_advert_t _visual_odometry_pub{nullptr};
|
||||
// ORB subscriptions
|
||||
uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
|
||||
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
uORB::Subscription _param_update_sub{ORB_ID(parameter_update)};
|
||||
uORB::Subscription _vehicle_attitude_sub{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)};
|
||||
|
||||
static constexpr unsigned int MOM_SWITCH_COUNT{8};
|
||||
static constexpr unsigned int MOM_SWITCH_COUNT{8};
|
||||
uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {};
|
||||
uint16_t _mom_switch_state{0};
|
||||
|
||||
int _orb_class_instance{-1};
|
||||
uint64_t _global_ref_timestamp{0};
|
||||
|
||||
uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {};
|
||||
map_projection_reference_s _hil_local_proj_ref{};
|
||||
float _hil_local_alt0{0.0f};
|
||||
bool _hil_local_proj_inited{false};
|
||||
|
||||
uint16_t _mom_switch_state{0};
|
||||
|
||||
uint64_t _global_ref_timestamp{0};
|
||||
|
||||
float _hil_local_alt0{0.0f};
|
||||
|
||||
bool _hil_local_proj_inited{false};
|
||||
|
||||
uORB::Subscription _param_update_sub{ORB_ID(parameter_update)};
|
||||
|
||||
hrt_abstime _last_utm_global_pos_com{0};
|
||||
hrt_abstime _last_utm_global_pos_com{0};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
|
||||
(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr,
|
||||
(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
|
||||
(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_com_flight_uuid,
|
||||
(ParamFloat<px4::params::SENS_FLOW_MAXHGT>) _param_sens_flow_maxhgt,
|
||||
(ParamFloat<px4::params::SENS_FLOW_MAXR>) _param_sens_flow_maxr,
|
||||
(ParamFloat<px4::params::SENS_FLOW_MINHGT>) _param_sens_flow_minhgt,
|
||||
(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_com_flight_uuid,
|
||||
(ParamInt<px4::params::SENS_FLOW_ROT>) _param_sens_flow_rot
|
||||
);
|
||||
|
||||
|
||||
@@ -55,10 +55,8 @@ public:
|
||||
/**
|
||||
* Constructor
|
||||
*
|
||||
* @param meta The uORB metadata (usually from
|
||||
* the ORB_ID() macro) for the topic.
|
||||
* @param priority The priority for multi pub/sub, 0-based, -1 means
|
||||
* don't publish as multi
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
|
||||
* @param priority The priority for multi pub/sub, 0 means don't publish as multi
|
||||
*/
|
||||
Publication(const orb_metadata *meta, uint8_t priority = 0) : _meta(meta), _priority(priority) {}
|
||||
|
||||
@@ -120,7 +118,7 @@ public:
|
||||
* the ORB_ID() macro) for the topic.
|
||||
* @param priority The priority for multi pub, 0-based.
|
||||
*/
|
||||
PublicationData(const orb_metadata *meta, int priority = -1) : Publication<T>(meta, priority) {}
|
||||
PublicationData(const orb_metadata *meta, uint8_t priority = 0) : Publication<T>(meta, priority) {}
|
||||
~PublicationData() = default;
|
||||
|
||||
T &get() { return _data; }
|
||||
|
||||
Reference in New Issue
Block a user