forked from Archive/PX4-Autopilot
Add support for mavlink message DEBUG_VECT
This commit is contained in:
parent
eeb966d375
commit
12353f4da7
|
@ -48,6 +48,7 @@ set(msg_file_names
|
|||
cpuload.msg
|
||||
debug_key_value.msg
|
||||
debug_value.msg
|
||||
debug_vect.msg
|
||||
differential_pressure.msg
|
||||
distance_sensor.msg
|
||||
ekf2_innovations.msg
|
||||
|
|
|
@ -0,0 +1,5 @@
|
|||
uint64 timestamp_us # in microseconds since system start
|
||||
int8[10] name # max. 10 characters as key / name
|
||||
float32 x # x value
|
||||
float32 y # y value
|
||||
float32 z # z value
|
|
@ -2046,6 +2046,7 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
configure_stream("HOME_POSITION", 0.5f);
|
||||
configure_stream("NAMED_VALUE_FLOAT", 1.0f);
|
||||
configure_stream("DEBUG", 1.0f);
|
||||
configure_stream("DEBUG_VECT", 1.0f);
|
||||
configure_stream("VFR_HUD", 4.0f);
|
||||
configure_stream("WIND_COV", 1.0f);
|
||||
configure_stream("CAMERA_IMAGE_CAPTURED");
|
||||
|
@ -2075,6 +2076,7 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
configure_stream("HOME_POSITION", 0.5f);
|
||||
configure_stream("NAMED_VALUE_FLOAT", 10.0f);
|
||||
configure_stream("DEBUG", 10.0f);
|
||||
configure_stream("DEBUG_VECT", 10.0f);
|
||||
configure_stream("VFR_HUD", 10.0f);
|
||||
configure_stream("WIND_COV", 10.0f);
|
||||
configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f);
|
||||
|
@ -2135,6 +2137,7 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
configure_stream("HOME_POSITION", 0.5f);
|
||||
configure_stream("NAMED_VALUE_FLOAT", 50.0f);
|
||||
configure_stream("DEBUG", 50.0f);
|
||||
configure_stream("DEBUG_VECT", 50.0f);
|
||||
configure_stream("VFR_HUD", 20.0f);
|
||||
configure_stream("WIND_COV", 10.0f);
|
||||
configure_stream("CAMERA_TRIGGER");
|
||||
|
|
|
@ -66,6 +66,7 @@
|
|||
#include <uORB/topics/cpuload.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_status.h>
|
||||
|
@ -3235,6 +3236,73 @@ protected:
|
|||
}
|
||||
};
|
||||
|
||||
class MavlinkStreamDebugVect : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
const char *get_name() const
|
||||
{
|
||||
return MavlinkStreamDebugVect::get_name_static();
|
||||
}
|
||||
|
||||
static const char *get_name_static()
|
||||
{
|
||||
return "DEBUG_VECT";
|
||||
}
|
||||
|
||||
static uint16_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_DEBUG_VECT;
|
||||
}
|
||||
|
||||
uint16_t get_id()
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamDebugVect(mavlink);
|
||||
}
|
||||
|
||||
unsigned get_size()
|
||||
{
|
||||
return (_debug_time > 0) ? MAVLINK_MSG_ID_DEBUG_VECT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
|
||||
}
|
||||
|
||||
private:
|
||||
MavlinkOrbSubscription *_debug_sub;
|
||||
uint64_t _debug_time;
|
||||
|
||||
/* do not allow top copying this class */
|
||||
MavlinkStreamDebugVect(MavlinkStreamDebugVect &);
|
||||
MavlinkStreamDebugVect &operator = (const MavlinkStreamDebugVect &);
|
||||
|
||||
protected:
|
||||
explicit MavlinkStreamDebugVect(Mavlink *mavlink) : MavlinkStream(mavlink),
|
||||
_debug_sub(_mavlink->add_orb_subscription(ORB_ID(debug_vect))),
|
||||
_debug_time(0)
|
||||
{}
|
||||
|
||||
void send(const hrt_abstime t)
|
||||
{
|
||||
struct debug_vect_s debug = {};
|
||||
|
||||
if (_debug_sub->update(&_debug_time, &debug)) {
|
||||
mavlink_debug_vect_t msg = {};
|
||||
|
||||
msg.time_usec = debug.timestamp_us;
|
||||
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);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
class MavlinkStreamNavControllerOutput : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
|
@ -4233,6 +4301,7 @@ const StreamListItem *streams_list[] = {
|
|||
new StreamListItem(&MavlinkStreamActuatorControlTarget<3>::new_instance, &MavlinkStreamActuatorControlTarget<3>::get_name_static, &MavlinkStreamActuatorControlTarget<3>::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static, &MavlinkStreamNamedValueFloat::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamDebug::new_instance, &MavlinkStreamDebug::get_name_static, &MavlinkStreamDebug::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamDebugVect::new_instance, &MavlinkStreamDebugVect::get_name_static, &MavlinkStreamDebugVect::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamNavControllerOutput::new_instance, &MavlinkStreamNavControllerOutput::get_name_static, &MavlinkStreamNavControllerOutput::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static, &MavlinkStreamCameraCapture::get_id_static),
|
||||
new StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static, &MavlinkStreamCameraTrigger::get_id_static),
|
||||
|
|
|
@ -139,6 +139,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
|||
_control_state_pub(nullptr),
|
||||
_debug_key_value_pub(nullptr),
|
||||
_debug_value_pub(nullptr),
|
||||
_debug_vect_pub(nullptr),
|
||||
_gps_inject_data_pub(nullptr),
|
||||
_command_ack_pub(nullptr),
|
||||
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
|
||||
|
@ -317,6 +318,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
|||
handle_message_debug(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_DEBUG_VECT:
|
||||
handle_message_debug_vect(msg);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
@ -2409,6 +2414,29 @@ void MavlinkReceiver::handle_message_debug(mavlink_message_t *msg)
|
|||
}
|
||||
}
|
||||
|
||||
void MavlinkReceiver::handle_message_debug_vect(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_debug_vect_t debug_msg;
|
||||
debug_vect_s debug_topic = {};
|
||||
|
||||
mavlink_msg_debug_vect_decode(msg, &debug_msg);
|
||||
|
||||
debug_topic.timestamp = hrt_absolute_time();
|
||||
debug_topic.timestamp_us = debug_msg.time_usec;
|
||||
memcpy(debug_topic.name, debug_msg.name, sizeof(debug_topic.name));
|
||||
debug_topic.name[sizeof(debug_topic.name) - 1] = '\0'; // enforce null termination
|
||||
debug_topic.x = debug_msg.x;
|
||||
debug_topic.y = debug_msg.y;
|
||||
debug_topic.z = debug_msg.z;
|
||||
|
||||
if (_debug_vect_pub == nullptr) {
|
||||
_debug_vect_pub = orb_advertise(ORB_ID(debug_vect), &debug_topic);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(debug_vect), _debug_vect_pub, &debug_topic);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Receive data from UART.
|
||||
*/
|
||||
|
|
|
@ -68,6 +68,7 @@
|
|||
#include <uORB/topics/telemetry_status.h>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <uORB/topics/debug_value.h>
|
||||
#include <uORB/topics/debug_vect.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/vehicle_force_setpoint.h>
|
||||
|
@ -154,6 +155,7 @@ private:
|
|||
void handle_message_play_tune(mavlink_message_t *msg);
|
||||
void handle_message_named_value_float(mavlink_message_t *msg);
|
||||
void handle_message_debug(mavlink_message_t *msg);
|
||||
void handle_message_debug_vect(mavlink_message_t *msg);
|
||||
|
||||
void *receive_thread(void *arg);
|
||||
|
||||
|
@ -241,6 +243,7 @@ private:
|
|||
orb_advert_t _control_state_pub;
|
||||
orb_advert_t _debug_key_value_pub;
|
||||
orb_advert_t _debug_value_pub;
|
||||
orb_advert_t _debug_vect_pub;
|
||||
static const int _gps_inject_data_queue_size = 6;
|
||||
orb_advert_t _gps_inject_data_pub;
|
||||
orb_advert_t _command_ack_pub;
|
||||
|
|
Loading…
Reference in New Issue