Hello,
I have an issue with sending a custom mavlink message from px4 SITL and receiving it in QGroundControl.
Here is what I did so far
created file test_types.msg in /Firmware/msg
uint64 timestamp
float32[3] t
also added to CMakeLists.txt
created module template in Firmware/src/modules/module
module.cpp
void Module::run()
{
// Example: run the loop synchronized to the sensor_combined topic publication
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
struct test_types_s test;
memset(&test, 0, sizeof(test));
orb_advert_t test_pub = orb_advertise(ORB_ID(test_types), &test);
px4_pollfd_struct_t fds[1];
fds[0].fd = sensor_combined_sub;
fds[0].events = POLLIN;
// initialize parameters
parameters_update(true);
while (!should_exit()) {
// wait for up to 1000ms for data
int pret = px4_poll(fds, (sizeof(fds) / sizeof(fds[0])), 1000);
if (pret == 0) {
// Timeout: let the loop run anyway, don't do `continue` here
} else if (pret < 0) {
// this is undesirable but not much we can do
PX4_ERR("poll error %d, %d", pret, errno);
px4_usleep(50000);
continue;
} else if (fds[0].revents & POLLIN) {
struct sensor_combined_s sensor_combined;
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor_combined);
test.t[0] = sensor_combined.accelerometer_m_s2[0];
test.t[1] = sensor_combined.accelerometer_m_s2[1];
test.t[2] = sensor_combined.accelerometer_m_s2[2];
orb_publish(ORB_ID(test_types), test_pub, &test);
// TODO: do something with the data...
}
parameters_update();
}
orb_unsubscribe(sensor_combined_sub);
}
and added module start /home/niketan/Firmware/ROMFS/px4fmu_common/init.d-posix
created test_types.xml file Firmware/mavlink/include/mavlink/v2.0/message_definitions <?xml version="1.0"?>
common.xml 0 The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right). Timestamp (time since system boot). Roll angle (-pi…+pi) Pitch angle (-pi…+pi) Yaw angle (-pi…+pi)
generated the files with Mavgenerate (GUI) 0utput Firmware/mavlink/include/mavlink/v2.0
i copy mavlink_msg_test_types.h to Firmware/mavlink/include/mavlink/v2.0/common
and include file in common.h
created class in mavlink_messages.cpp
class MavlinkStreamTestTypes : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamTestTypes::get_name_static();
}
static const char *get_name_static()
{
return “TEST_TYPES”;
}
static uint16_t get_id_static()
{
return MAVLINK_MSG_ID_TEST_TYPES;
}
uint16_t get_id()
{
return get_id_static();
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamTestTypes(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_TEST_TYPES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
MavlinkOrbSubscription *_sub;
uint64_t _ca_traj_time;
/* do not allow top copying this class */
MavlinkStreamTestTypes(MavlinkStreamTestTypes &);
MavlinkStreamTestTypes& operator = (const MavlinkStreamTestTypes &);
protected:
explicit MavlinkStreamTestTypes(Mavlink *mavlink) : MavlinkStream(mavlink),
_sub(_mavlink->add_orb_subscription(ORB_ID(test_types))), // make sure you enter the name of your uORB topic here
_ca_traj_time(0)
{}
bool send(const hrt_abstime t)
{
struct test_types_s _test; //make sure ca_traj_struct_s is the definition of your uORB topic
if (_sub->update(&_ca_traj_time, &_test)) {
mavlink_test_types_t _msg_test_types{}; //make sure mavlink_ca_trajectory_t is the definition of your custom MAVLink message
_msg_test_types.time_boot_ms_nik = _test.timestamp;
_msg_test_types.roll_nik = _test.t[0];
_msg_test_types.pitch_nik = _test.t[1];
_msg_test_types.yaw_nik = _test.t[2];
//PX4_INFO("msg!");
mavlink_msg_test_types_send_struct(_mavlink->get_channel(), &_msg_test_types);
}
return true;
}
};
also added
StreamListItem(&MavlinkStreamTestTypes::new_instance, &MavlinkStreamTestTypes::get_name_static, &MavlinkStreamTestTypes::get_id_static)
added in mavlink_main.cpp
MAVLINK_MODE_NORMAL
configure_stream_local(“TEST_TYPES”, 15.0f);
MAVLINK_MODE_ONBOARD
configure_stream_local(“TEST_TYPES”, 100.0f);
MAVLINK_MODE_EXTVISIONMIN
configure_stream_local(“TEST_TYPES”, 20.0f);
MAVLINK_MODE_OSD
configure_stream_local(“TEST_TYPES”, 25.0f);
MAVLINK_MODE_CONFIG
configure_stream_local(“TEST_TYPES”, 50.0f);
MAVLINK_MODE_MINIMAL
configure_stream_local(“TEST_TYPES”, 10.0f);
added module start and mavlink stream -r 50 -s TEST_TYPES -u $udp_gcs_port_local in /home/niketan/Firmware/ROMFS/px4fmu_common/init.d-posix
I have complie and build it with make px4_sitl_default jmavsim with no errors
In Qgroundstation
copied same xml file in qgroundcontrol/libs/mavlink/include/mavlink/v2.0/message_definitions
generated header files in qgroundcontrol/libs/mavlink/include/mavlink/v2.0 using Mavgenerate (GUI)
0utput qgroundcontrol/libs/mavlink/include/mavlink/v2.0
i copy mavlink_msg_test_types.h to qgroundcontrol/libs/mavlink/include/mavlink/v2.0/common
and include file in common.h
now i added following line to vehicle.cc
switch (message.msgid) {
case MAVLINK_MSG_ID_HOME_POSITION:
_handleHomePosition(message);
break;
case MAVLINK_MSG_ID_HEARTBEAT:
_handleHeartbeat(message);
break;
case MAVLINK_MSG_ID_TEST_TYPES:
qDebug()<<"msg ";
break;
after running both i am not Receiving any msg in inspector window or in application output.
is there any additional step i am missing…??
please help me out with it i am stuck from 3 months …
thank you.