Sending Command Long message with mavros on C++ ROS1

Hardware Companion Computers
,
1

Hello,
I want to make communication between companion computer (jetson xavier) and my flight controller (orange cube). I can received mavlink message without problem, calculate the checksum… But there is something strange when sending mav message.

Firstly, the payload is an array (or vector) of uint64_t. So all the bytes are inside and we need to decode this to have each useful byte distinctly. Why not just use a uint8_t array instead of ?

Secondly, i can successfully send an heartbeat message thought this code :

image
image881×156 7.91 KB

(i wrote the payload by hand to see each byte and to fully understand)

But when i’m doing the same thing with a command long, it is wrong :

  // Send a Command Long
  float param1 = 27, param2 = 500000, param3 = 0.0, param4 = 0.0, param5 = 0.0, param6 = 0.0, param7 = 0.0;
  uint8_t* array1 = reinterpret_cast<uint8_t*>(&param1);
  uint8_t* array2 = reinterpret_cast<uint8_t*>(&param2);
  uint8_t* array3 = reinterpret_cast<uint8_t*>(&param3);
  uint8_t* array4 = reinterpret_cast<uint8_t*>(&param4);
  uint8_t* array5 = reinterpret_cast<uint8_t*>(&param5);
  uint8_t* array6 = reinterpret_cast<uint8_t*>(&param6);
  uint8_t* array7 = reinterpret_cast<uint8_t*>(&param7);

  uint8_t arrayfinal_cmdlong[] ={array1[0], array1[1], array1[2], array1[3],  array2[0], array2[1], array2[2], array2[3],  array3[0], array3[1], array3[2], array3[3],  array4[0], array4[1], array4[2], array4[3],  array5[0], array5[1], array5[2], array5[3],  array6[0], array6[1], array6[2], array6[3],  array7[0], array7[1], array7[2], array7[3],  511&0xff, (511>>8)&0xff, 1, 1, 0};
  mavros_msgs::Mavlink msg = im.FillMavlinkMess(0xFD, 33, 0, 0, 0, 18, 191, 76, arrayfinal_cmdlong, 152);
  im.m_external_com_publisher.publish(msg);

i tried doing it byte by byte with array of uint64_t, or uint8_t but i don’t have any response from the FC.
I don’t understand why it is not working. If you have any idea thank you for your help.

Gjörn.

@amilcarlucas

2

You are not supposed to build the messages by hand, that is very error prone.

Options:

  1. Use pymavlink and python
  2. used pymavlink to generate C code and then link and call the generated C functions.
  3. Use mavros
  4. use ROS2
3

Thank you for your response @amilcarlucas.

I’m doing an intership and my boss asked me to do like this. I will purpose your idea.
Can you just quickly explain how i’m supposed to do with mavros ? Where to code ? I need to used topics mavlink/from and mavlink/to, but is there already some functions to send and receive mav message using mavros ?

4

https://docs.ros.org/en/hydro/api/mavros/html/srv/CommandLong.html

5

2 problems :

  • on the page you just sent, there is no example for c++ code (and i’m asked to do everything in c++ in nodes, in ROS1 with mavros)
  • Most of the examples are command lines in the terminal or rqt command. But i just want to have one node which will receive and send mavlink message.
6

I updated the title now.