dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink debug messages disable on CONSTRAINED_FLASH boards 

- mavlink messages DEBUG/DEBUG_FLOAT_ARRAY/DEBUG_VECT/NAMED_VALUE_FLOAT move to separate stream headers and don't include if CONSTRAINED_FLASH
 - mavlink receiver DEBUG/DEBUG_FLOAT_ARRAY/DEBUG_VECT/NAMED_VALUE_FLOAT handling excluded if CONSTRAINED_FLASH
 - msg: skip debug_array.msg, debug_key_value.msg, debug_value.msg, debug_vect.msg if CONSTRAINED_FLASH
This commit is contained in:
Daniel Agar 2020-10-19 12:35:48 -04:00
parent d9b3db1082
commit 16119f0e8c
9 changed files with 423 additions and 316 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
msg/CMakeLists.txt
View File

@ -49,10 +49,6 @@ set(msg_files
collision_report.msg
commander_state.msg
cpuload.msg
debug_array.msg
debug_key_value.msg
debug_value.msg
debug_vect.msg
differential_pressure.msg
distance_sensor.msg
ekf2_timestamps.msg
@ -173,6 +169,17 @@ set(msg_files
yaw_estimator_status.msg
)
if(NOT px4_constrained_flash_build)
list(APPEND msg_files
debug_array.msg
debug_key_value.msg
debug_value.msg
debug_vect.msg
)
endif()
list(SORT msg_files)
set(deprecated_msgs
ekf2_innovations.msg # 2019-11-22, Updated estimator interface and logging; replaced by 'estimator_innovations'.
)

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

@ -1571,9 +1571,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("BATTERY_STATUS", 0.5f);
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
configure_stream_local("COLLISION", unlimited_rate);
configure_stream_local("DEBUG", 1.0f);
configure_stream_local("DEBUG_FLOAT_ARRAY", 1.0f);
configure_stream_local("DEBUG_VECT", 1.0f);
configure_stream_local("DISTANCE_SENSOR", 0.5f);
configure_stream_local("ESC_INFO", 1.0f);
configure_stream_local("ESC_STATUS", 1.0f);
@ -1585,7 +1582,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("GPS_STATUS", 1.0f);
configure_stream_local("HOME_POSITION", 0.5f);
configure_stream_local("LOCAL_POSITION_NED", 1.0f);
configure_stream_local("NAMED_VALUE_FLOAT", 1.0f);
configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.0f);
configure_stream_local("OBSTACLE_DISTANCE", 1.0f);
configure_stream_local("ORBIT_EXECUTION_STATUS", 2.0f);
@ -1600,6 +1596,14 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("VFR_HUD", 4.0f);
configure_stream_local("VIBRATION", 0.1f);
configure_stream_local("WIND_COV", 0.5f);
#if !defined(CONSTRAINED_FLASH)
configure_stream_local("DEBUG", 1.0f);
configure_stream_local("DEBUG_FLOAT_ARRAY", 1.0f);
configure_stream_local("DEBUG_VECT", 1.0f);
configure_stream_local("NAMED_VALUE_FLOAT", 1.0f);
#endif // !CONSTRAINED_FLASH
break;
case MAVLINK_MODE_ONBOARD:
@ -1625,9 +1629,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("CAMERA_CAPTURE", 2.0f);
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
configure_stream_local("COLLISION", unlimited_rate);
configure_stream_local("DEBUG", 10.0f);
configure_stream_local("DEBUG_FLOAT_ARRAY", 10.0f);
configure_stream_local("DEBUG_VECT", 10.0f);
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
configure_stream_local("EXTENDED_SYS_STATE", 5.0f);
configure_stream_local("GLOBAL_POSITION_INT", 50.0f);
@ -1635,7 +1636,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("GPS_RAW_INT", unlimited_rate);
configure_stream_local("GPS_STATUS", 1.0f);
configure_stream_local("HOME_POSITION", 0.5f);
configure_stream_local("NAMED_VALUE_FLOAT", 10.0f);
configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f);
configure_stream_local("OPTICAL_FLOW_RAD", 10.0f);
configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f);
@ -1652,6 +1652,14 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("VFR_HUD", 10.0f);
configure_stream_local("VIBRATION", 0.5f);
configure_stream_local("WIND_COV", 10.0f);
#if !defined(CONSTRAINED_FLASH)
configure_stream_local("DEBUG", 10.0f);
configure_stream_local("DEBUG_FLOAT_ARRAY", 10.0f);
configure_stream_local("DEBUG_VECT", 10.0f);
configure_stream_local("NAMED_VALUE_FLOAT", 10.0f);
#endif // !CONSTRAINED_FLASH
break;
case MAVLINK_MODE_EXTVISION:
@ -1675,16 +1683,12 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("BATTERY_STATUS", 0.5f);
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
configure_stream_local("COLLISION", unlimited_rate);
configure_stream_local("DEBUG", 1.0f);
configure_stream_local("DEBUG_FLOAT_ARRAY", 1.0f);
configure_stream_local("DEBUG_VECT", 1.0f);
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
configure_stream_local("GPS2_RAW", 1.0f);
configure_stream_local("GPS_RAW_INT", 1.0f);
configure_stream_local("HOME_POSITION", 0.5f);
configure_stream_local("NAMED_VALUE_FLOAT", 1.0f);
configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.5f);
configure_stream_local("OPTICAL_FLOW_RAD", 1.0f);
configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f);
@ -1699,8 +1703,15 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("VFR_HUD", 4.0f);
configure_stream_local("VIBRATION", 0.5f);
configure_stream_local("WIND_COV", 1.0f);
break;
#if !defined(CONSTRAINED_FLASH)
configure_stream_local("DEBUG", 1.0f);
configure_stream_local("DEBUG_FLOAT_ARRAY", 1.0f);
configure_stream_local("DEBUG_VECT", 1.0f);
configure_stream_local("NAMED_VALUE_FLOAT", 1.0f);
#endif // !CONSTRAINED_FLASH
break;
case MAVLINK_MODE_OSD:
configure_stream_local("ALTITUDE", 10.0f);
@ -1746,9 +1757,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("BATTERY_STATUS", 0.5f);
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
configure_stream_local("COLLISION", unlimited_rate);
configure_stream_local("DEBUG", 50.0f);
configure_stream_local("DEBUG_FLOAT_ARRAY", 50.0f);
configure_stream_local("DEBUG_VECT", 50.0f);
configure_stream_local("ESC_INFO", 10.0f);
configure_stream_local("ESC_STATUS", 10.0f);
configure_stream_local("ESTIMATOR_STATUS", 5.0f);
@ -1760,7 +1768,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("HIGHRES_IMU", 50.0f);
configure_stream_local("HOME_POSITION", 0.5f);
configure_stream_local("MANUAL_CONTROL", 5.0f);
configure_stream_local("NAMED_VALUE_FLOAT", 50.0f);
configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f);
configure_stream_local("OPTICAL_FLOW_RAD", 10.0f);
configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f);
@ -1779,6 +1786,14 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("VFR_HUD", 20.0f);
configure_stream_local("VIBRATION", 2.5f);
configure_stream_local("WIND_COV", 10.0f);
#if !defined(CONSTRAINED_FLASH)
configure_stream_local("DEBUG", 50.0f);
configure_stream_local("DEBUG_FLOAT_ARRAY", 50.0f);
configure_stream_local("DEBUG_VECT", 50.0f);
configure_stream_local("NAMED_VALUE_FLOAT", 50.0f);
#endif // !CONSTRAINED_FLASH
break;
case MAVLINK_MODE_IRIDIUM:

286
src/modules/mavlink/mavlink_messages.cpp
View File

@ -65,10 +65,6 @@
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/collision_report.h>
#include <uORB/topics/cpuload.h>
#include <uORB/topics/debug_array.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/debug_value.h>
#include <uORB/topics/debug_vect.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/estimator_sensor_bias.h>
@ -128,6 +124,13 @@ using matrix::wrap_2pi;
#include "streams/RAW_RPM.hpp"
#include "streams/STORAGE_INFORMATION.hpp"
#if !defined(CONSTRAINED_FLASH)
# include "streams/DEBUG.hpp"
# include "streams/DEBUG_FLOAT_ARRAY.hpp"
# include "streams/DEBUG_VECT.hpp"
# include "streams/NAMED_VALUE_FLOAT.hpp"
#endif // !CONSTRAINED_FLASH
// ensure PX4 rotation enum and MAV_SENSOR_ROTATION align
static_assert(MAV_SENSOR_ROTATION_NONE == static_cast<MAV_SENSOR_ORIENTATION>(ROTATION_NONE),
"Roll: 0, Pitch: 0, Yaw: 0");
@ -4165,273 +4168,6 @@ protected:
}
};
class MavlinkStreamNamedValueFloat : public MavlinkStream
{
public:
const char *get_name() const override
{
return MavlinkStreamNamedValueFloat::get_name_static();
}
static constexpr const char *get_name_static()
{
return "NAMED_VALUE_FLOAT";
}
static constexpr uint16_t get_id_static()
{
return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
}
uint16_t get_id() override
{
return get_id_static();
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamNamedValueFloat(mavlink);
}
unsigned get_size() override
{
return _debug_sub.advertised() ? MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
uORB::Subscription _debug_sub{ORB_ID(debug_key_value)};
/* do not allow top copying this class */
MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &) = delete;
MavlinkStreamNamedValueFloat &operator = (const MavlinkStreamNamedValueFloat &) = delete;
protected:
explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink)
{}
bool send() override
{
debug_key_value_s debug;
if (_debug_sub.update(&debug)) {
mavlink_named_value_float_t msg{};
msg.time_boot_ms = debug.timestamp / 1000ULL;
memcpy(msg.name, debug.key, sizeof(msg.name));
/* enforce null termination */
msg.name[sizeof(msg.name) - 1] = '\0';
msg.value = debug.value;
mavlink_msg_named_value_float_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
class MavlinkStreamDebug : public MavlinkStream
{
public:
const char *get_name() const override
{
return MavlinkStreamDebug::get_name_static();
}
static constexpr const char *get_name_static()
{
return "DEBUG";
}
static constexpr uint16_t get_id_static()
{
return MAVLINK_MSG_ID_DEBUG;
}
uint16_t get_id() override
{
return get_id_static();
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamDebug(mavlink);
}
unsigned get_size() override
{
return _debug_sub.advertised() ? MAVLINK_MSG_ID_DEBUG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
uORB::Subscription _debug_sub{ORB_ID(debug_value)};
/* do not allow top copying this class */
MavlinkStreamDebug(MavlinkStreamDebug &) = delete;
MavlinkStreamDebug &operator = (const MavlinkStreamDebug &) = delete;
protected:
explicit MavlinkStreamDebug(Mavlink *mavlink) : MavlinkStream(mavlink)
{}
bool send() override
{
debug_value_s debug;
if (_debug_sub.update(&debug)) {
mavlink_debug_t msg{};
msg.time_boot_ms = debug.timestamp / 1000ULL;
msg.ind = debug.ind;
msg.value = debug.value;
mavlink_msg_debug_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
class MavlinkStreamDebugVect : public MavlinkStream
{
public:
const char *get_name() const override
{
return MavlinkStreamDebugVect::get_name_static();
}
static constexpr const char *get_name_static()
{
return "DEBUG_VECT";
}
static constexpr uint16_t get_id_static()
{
return MAVLINK_MSG_ID_DEBUG_VECT;
}
uint16_t get_id() override
{
return get_id_static();
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamDebugVect(mavlink);
}
unsigned get_size() override
{
return _debug_sub.advertised() ? MAVLINK_MSG_ID_DEBUG_VECT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
uORB::Subscription _debug_sub{ORB_ID(debug_vect)};
/* do not allow top copying this class */
MavlinkStreamDebugVect(MavlinkStreamDebugVect &) = delete;
MavlinkStreamDebugVect &operator = (const MavlinkStreamDebugVect &) = delete;
protected:
explicit MavlinkStreamDebugVect(Mavlink *mavlink) : MavlinkStream(mavlink)
{}
bool send() override
{
debug_vect_s debug;
if (_debug_sub.update(&debug)) {
mavlink_debug_vect_t msg{};
msg.time_usec = debug.timestamp;
memcpy(msg.name, debug.name, sizeof(msg.name));
/* enforce null termination */
msg.name[sizeof(msg.name) - 1] = '\0';
msg.x = debug.x;
msg.y = debug.y;
msg.z = debug.z;
mavlink_msg_debug_vect_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
class MavlinkStreamDebugFloatArray : public MavlinkStream
{
public:
const char *get_name() const override
{
return MavlinkStreamDebugFloatArray::get_name_static();
}
static constexpr const char *get_name_static()
{
return "DEBUG_FLOAT_ARRAY";
}
static constexpr uint16_t get_id_static()
{
return MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY;
}
uint16_t get_id() override
{
return get_id_static();
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamDebugFloatArray(mavlink);
}
unsigned get_size() override
{
return _debug_array_sub.advertised() ? MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
uORB::Subscription _debug_array_sub{ORB_ID(debug_array)};
/* do not allow top copying this class */
MavlinkStreamDebugFloatArray(MavlinkStreamDebugFloatArray &);
MavlinkStreamDebugFloatArray &operator = (const MavlinkStreamDebugFloatArray &);
protected:
explicit MavlinkStreamDebugFloatArray(Mavlink *mavlink) : MavlinkStream(mavlink)
{}
bool send() override
{
debug_array_s debug;
if (_debug_array_sub.update(&debug)) {
mavlink_debug_float_array_t msg{};
msg.time_usec = debug.timestamp;
msg.array_id = debug.id;
memcpy(msg.name, debug.name, sizeof(msg.name));
/* enforce null termination */
msg.name[sizeof(msg.name) - 1] = '\0';
for (size_t i = 0; i < debug_array_s::ARRAY_SIZE; i++) {
msg.data[i] = debug.data[i];
}
mavlink_msg_debug_float_array_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
class MavlinkStreamNavControllerOutput : public MavlinkStream
{
public:
@ -4974,10 +4710,18 @@ static const StreamListItem streams_list[] = {
create_stream_list_item<MavlinkStreamOpticalFlowRad>(),
create_stream_list_item<MavlinkStreamActuatorControlTarget<0> >(),
create_stream_list_item<MavlinkStreamActuatorControlTarget<1> >(),
#if defined(NAMED_VALUE_FLOAT_HPP)
create_stream_list_item<MavlinkStreamNamedValueFloat>(),
#endif // NAMED_VALUE_FLOAT_HPP
#if defined(DEBUG_HPP)
create_stream_list_item<MavlinkStreamDebug>(),
#endif // DEBUG_HPP
#if defined(DEBUG_VECT_HPP)
create_stream_list_item<MavlinkStreamDebugVect>(),
#endif // DEBUG_VECT_HPP
#if defined(DEBUG_FLOAT_ARRAY_HPP)
create_stream_list_item<MavlinkStreamDebugFloatArray>(),
#endif // DEBUG_FLOAT_ARRAY_HPP
create_stream_list_item<MavlinkStreamNavControllerOutput>(),
create_stream_list_item<MavlinkStreamCameraCapture>(),
create_stream_list_item<MavlinkStreamCameraTrigger>(),

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

@ -246,6 +246,20 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_trajectory_representation_waypoints(msg);
break;
case MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS:
handle_message_onboard_computer_status(msg);
break;
case MAVLINK_MSG_ID_GENERATOR_STATUS:
handle_message_generator_status(msg);
break;
case MAVLINK_MSG_ID_STATUSTEXT:
handle_message_statustext(msg);
break;
#if !defined(CONSTRAINED_FLASH)
case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
handle_message_named_value_float(msg);
break;
@ -261,18 +275,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
case MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY:
handle_message_debug_float_array(msg);
break;
case MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS:
handle_message_onboard_computer_status(msg);
break;
case MAVLINK_MSG_ID_GENERATOR_STATUS:
handle_message_generator_status(msg);
break;
case MAVLINK_MSG_ID_STATUSTEXT:
handle_message_statustext(msg);
break;
#endif // !CONSTRAINED_FLASH
default:
break;
@ -2704,6 +2707,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
}
}
#if !defined(CONSTRAINED_FLASH)
void
MavlinkReceiver::handle_message_named_value_float(mavlink_message_t *msg)
{
@ -2772,6 +2776,7 @@ MavlinkReceiver::handle_message_debug_float_array(mavlink_message_t *msg)
_debug_array_pub.publish(debug_topic);
}
#endif // !CONSTRAINED_FLASH
void
MavlinkReceiver::handle_message_onboard_computer_status(mavlink_message_t *msg)

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

@ -62,10 +62,6 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/cellular_status.h>
#include <uORB/topics/collision_report.h>
#include <uORB/topics/debug_array.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/debug_value.h>
#include <uORB/topics/debug_vect.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/follow_target.h>
@ -104,6 +100,13 @@
#include <uORB/topics/vehicle_trajectory_bezier.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#if !defined(CONSTRAINED_FLASH)
# include <uORB/topics/debug_array.h>
# include <uORB/topics/debug_key_value.h>
# include <uORB/topics/debug_value.h>
# include <uORB/topics/debug_vect.h>
#endif // !CONSTRAINED_FLASH
class Mavlink;
class MavlinkReceiver : public ModuleParams
@ -144,9 +147,6 @@ private:
void handle_message_command_ack(mavlink_message_t *msg);
void handle_message_command_int(mavlink_message_t *msg);
void handle_message_command_long(mavlink_message_t *msg);
void handle_message_debug(mavlink_message_t *msg);
void handle_message_debug_float_array(mavlink_message_t *msg);
void handle_message_debug_vect(mavlink_message_t *msg);
void handle_message_distance_sensor(mavlink_message_t *msg);
void handle_message_follow_target(mavlink_message_t *msg);
void handle_message_generator_status(mavlink_message_t *msg);
@ -160,7 +160,6 @@ private:
void handle_message_landing_target(mavlink_message_t *msg);
void handle_message_logging_ack(mavlink_message_t *msg);
void handle_message_manual_control(mavlink_message_t *msg);
void handle_message_named_value_float(mavlink_message_t *msg);
void handle_message_obstacle_distance(mavlink_message_t *msg);
void handle_message_odometry(mavlink_message_t *msg);
void handle_message_onboard_computer_status(mavlink_message_t *msg);
@ -182,6 +181,13 @@ private:
void handle_message_utm_global_position(mavlink_message_t *msg);
void handle_message_vision_position_estimate(mavlink_message_t *msg);
#if !defined(CONSTRAINED_FLASH)
void handle_message_debug(mavlink_message_t *msg);
void handle_message_debug_float_array(mavlink_message_t *msg);
void handle_message_debug_vect(mavlink_message_t *msg);
void handle_message_named_value_float(mavlink_message_t *msg);
#endif // !CONSTRAINED_FLASH
void CheckHeartbeats(const hrt_abstime &t, bool force = false);
void Run();
@ -236,10 +242,6 @@ private:
uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
uORB::Publication<cellular_status_s> _cellular_status_pub{ORB_ID(cellular_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<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::Publication<follow_target_s> _follow_target_pub{ORB_ID(follow_target)};
uORB::Publication<irlock_report_s> _irlock_report_pub{ORB_ID(irlock_report)};
@ -265,6 +267,13 @@ private:
uORB::Publication<vehicle_trajectory_bezier_s> _trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)};
uORB::Publication<vehicle_trajectory_waypoint_s> _trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)};
#if !defined(CONSTRAINED_FLASH)
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)};
#endif // !CONSTRAINED_FLASH
// ORB publications (multi)
uORB::PublicationMulti<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::PublicationMulti<distance_sensor_s> _flow_distance_sensor_pub{ORB_ID(distance_sensor)};

79
src/modules/mavlink/streams/DEBUG.hpp Normal file
View File

@ -0,0 +1,79 @@
/****************************************************************************
*
* Copyright (c) 2020 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.
*
****************************************************************************/
#ifndef DEBUG_HPP
#define DEBUG_HPP
#include <uORB/topics/debug_value.h>
class MavlinkStreamDebug : public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamDebug(mavlink); }
static constexpr const char *get_name_static() { return "DEBUG"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_DEBUG; }
const char *get_name() const override { return get_name_static(); }
uint16_t get_id() override { return get_id_static(); }
unsigned get_size() override
{
return _debug_value_sub.advertised() ? MAVLINK_MSG_ID_DEBUG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
explicit MavlinkStreamDebug(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _debug_value_sub{ORB_ID(debug_value)};
bool send() override
{
debug_value_s debug;
if (_debug_value_sub.update(&debug)) {
mavlink_debug_t msg{};
msg.time_boot_ms = debug.timestamp / 1000ULL;
msg.ind = debug.ind;
msg.value = debug.value;
mavlink_msg_debug_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
#endif // DEBUG_HPP

85
src/modules/mavlink/streams/DEBUG_FLOAT_ARRAY.hpp Normal file
View File

@ -0,0 +1,85 @@
/****************************************************************************
*
* Copyright (c) 2020 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.
*
****************************************************************************/
#ifndef DEBUG_FLOAT_ARRAY_HPP
#define DEBUG_FLOAT_ARRAY_HPP
#include <uORB/topics/debug_array.h>
class MavlinkStreamDebugFloatArray : public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamDebugFloatArray(mavlink); }
static constexpr const char *get_name_static() { return "DEBUG_FLOAT_ARRAY"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY; }
const char *get_name() const override { return get_name_static(); }
uint16_t get_id() override { return get_id_static(); }
unsigned get_size() override
{
return _debug_array_sub.advertised() ? MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
explicit MavlinkStreamDebugFloatArray(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _debug_array_sub{ORB_ID(debug_array)};
bool send() override
{
debug_array_s debug;
if (_debug_array_sub.update(&debug)) {
mavlink_debug_float_array_t msg{};
msg.time_usec = debug.timestamp;
msg.array_id = debug.id;
memcpy(msg.name, debug.name, sizeof(msg.name));
msg.name[sizeof(msg.name) - 1] = '\0'; // enforce null termination
for (size_t i = 0; i < debug_array_s::ARRAY_SIZE; i++) {
msg.data[i] = debug.data[i];
}
mavlink_msg_debug_float_array_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
#endif // DEBUG_FLOAT_ARRAY_HPP

82
src/modules/mavlink/streams/DEBUG_VECT.hpp Normal file
View File

@ -0,0 +1,82 @@
/****************************************************************************
*
* Copyright (c) 2020 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.
*
****************************************************************************/
#ifndef DEBUG_VECT_HPP
#define DEBUG_VECT_HPP
#include <uORB/topics/debug_vect.h>
class MavlinkStreamDebugVect : public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamDebugVect(mavlink); }
static constexpr const char *get_name_static() { return "DEBUG_VECT"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_DEBUG_VECT; }
const char *get_name() const override { return get_name_static(); }
uint16_t get_id() override { return get_id_static(); }
unsigned get_size() override
{
return _debug_sub.advertised() ? MAVLINK_MSG_ID_DEBUG_VECT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
explicit MavlinkStreamDebugVect(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _debug_sub{ORB_ID(debug_vect)};
bool send() override
{
debug_vect_s debug;
if (_debug_sub.update(&debug)) {
mavlink_debug_vect_t msg{};
msg.time_usec = debug.timestamp;
memcpy(msg.name, debug.name, sizeof(msg.name));
msg.name[sizeof(msg.name) - 1] = '\0'; // enforce null termination
msg.x = debug.x;
msg.y = debug.y;
msg.z = debug.z;
mavlink_msg_debug_vect_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
#endif // DEBUG_VECT_HPP

81
src/modules/mavlink/streams/NAMED_VALUE_FLOAT.hpp Normal file
View File

@ -0,0 +1,81 @@
/****************************************************************************
*
* Copyright (c) 2020 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.
*
****************************************************************************/
#ifndef NAMED_VALUE_FLOAT_HPP
#define NAMED_VALUE_FLOAT_HPP
#include <uORB/topics/debug_key_value.h>
class MavlinkStreamNamedValueFloat : public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamNamedValueFloat(mavlink); }
static constexpr const char *get_name_static() { return "NAMED_VALUE_FLOAT"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; }
const char *get_name() const override { return get_name_static(); }
uint16_t get_id() override { return get_id_static(); }
unsigned get_size() override
{
return _debug_key_value_sub.advertised() ? MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _debug_key_value_sub{ORB_ID(debug_key_value)};
bool send() override
{
debug_key_value_s debug;
if (_debug_key_value_sub.update(&debug)) {
mavlink_named_value_float_t msg{};
msg.time_boot_ms = debug.timestamp / 1000ULL;
memcpy(msg.name, debug.key, sizeof(msg.name));
msg.name[sizeof(msg.name) - 1] = '\0'; // enforce null termination
msg.value = debug.value;
mavlink_msg_named_value_float_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
#endif // NAMED_VALUE_FLOAT_HPP