Custom MAVLink Message

PX4 Autopilot
1

I am trying to use a custom mavlink message with PX4 and MAVSDK.
Here is my current status:

  1. I created a separate message definition file which has common.xml included in it and generated the headers for it.
  2. I compiled MAVSDK with the new headers and made new functions to use this message.
  3. I used the new functions to access and send this new MAVLink message.
  4. I then recompiled PX4 with the new MAVlink headers.
  5. I debugged and found out that my new MAVlink message is being sent from MAVSDK properly.
  6. I added a new uORB topic to be used with this message.
  7. I included the new MAVlink header in mavlink_receiver.h

After this, through some debug I found out that the message is being received by the PX4 but is not being parsed.
After some more debug I found out that the parse_char helper function from MAVlink was giving a bad crc error.

I can properly the received message id and everything if I remove the check for bad crc in the parse_char function.

Can anyone help me with how can I fix this bad crc?

2

It sounds like the MAVLink headers are not exactly in sync. Have you made sure they are the same?

Also, could you paste the code where you send the custom message? I would want to check that.

3

Sure.

  • This is my new message

      <message id="182" name="SET_ATTITUDE_Z_VELOCITY">
              <description>Sets a desired vehicle attitude and z velocity. Used by an external controller to command the vehicle (manual controller or other system).</description>
              <field type="uint32_t" name="time_boot_ms" units="ms">Timestamp (time since system boot).</field>
              <field type="uint8_t" name="target_system">System ID</field>
              <field type="uint8_t" name="target_component">Component ID</field>
              <field type="uint8_t" name="type_mask">Mappings: If any of these bits are set, the corresponding input should be ignored: bit 7: z_velocity, bit 8: attitude</field>
              <field type="float[4]" name="q">Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0)</field>
              <field type="float" name="z_velocity" units="m/s">Velocity in z-direction</field>
          </message>

  • Code from MAVSDK which sends the custom message

      void OffboardImpl::set_attitude_z_velocity(Offboard::AttitudeZVelocity attitude_z_velocity)
  {
      _mutex.lock();
      _attitude_z_velocity = attitude_z_velocity;

      if (_mode != Mode::ATTITUDE_Z_VELOCITY) {
          if (_call_every_cookie) {
              // If we're already sending other setpoints, stop that now.
              _parent->remove_call_every(_call_every_cookie);
              _call_every_cookie = nullptr;
          }
          // We automatically send body setpoints from now on.
          _parent->add_call_every(
              [this]() { send_attitude_z_velocity(); }, SEND_INTERVAL_S, &_call_every_cookie);

          _mode = Mode::ATTITUDE_Z_VELOCITY;
      } else {
          // We're already sending these kind of setpoints. Since the setpoint change, let's
          // reschedule the next call, so we don't send setpoints too often.
          _parent->reset_call_every(_call_every_cookie);
      }
      _mutex.unlock();

      // also send it right now to reduce latency
      send_attitude_z_velocity();
  }

  void OffboardImpl::send_attitude_z_velocity()
  {
      // const static uint8_t IGNORE_BODY_ROLL_RATE = (1 << 0);
      // const static uint8_t IGNORE_BODY_PITCH_RATE = (1 << 1);
      // const static uint8_t IGNORE_BODY_YAW_RATE = (1 << 2);
      // const static uint8_t IGNORE_4 = (1 << 3);
      // const static uint8_t IGNORE_5 = (1 << 4);
      // const static uint8_t IGNORE_6 = (1 << 5);
      // const static uint8_t IGNORE_THRUST = (1 << 6);
      // const static uint8_t IGNORE_ATTITUDE = (1 << 7);

      _mutex.lock();
      const float down_speed = _attitude_z_velocity.down_speed_m_s;
      const float roll = to_rad_from_deg(_attitude_z_velocity.roll_deg);
      const float pitch = to_rad_from_deg(_attitude_z_velocity.pitch_deg);
      const float yaw = to_rad_from_deg(_attitude_z_velocity.yaw_deg);
      _mutex.unlock();

      const double cos_phi_2 = cos(double(roll) / 2.0);
      const double sin_phi_2 = sin(double(roll) / 2.0);
      const double cos_theta_2 = cos(double(pitch) / 2.0);
      const double sin_theta_2 = sin(double(pitch) / 2.0);
      const double cos_psi_2 = cos(double(yaw) / 2.0);
      const double sin_psi_2 = sin(double(yaw) / 2.0);

      float q[4];

      q[0] = float(cos_phi_2 * cos_theta_2 * cos_psi_2 + sin_phi_2 * sin_theta_2 * sin_psi_2);
      q[1] = float(sin_phi_2 * cos_theta_2 * cos_psi_2 - cos_phi_2 * sin_theta_2 * sin_psi_2);
      q[2] = float(cos_phi_2 * sin_theta_2 * cos_psi_2 + sin_phi_2 * cos_theta_2 * sin_psi_2);
      q[3] = float(cos_phi_2 * cos_theta_2 * sin_psi_2 - sin_phi_2 * sin_theta_2 * cos_psi_2);

      mavlink_message_t message;
      mavlink_msg_set_attitude_z_velocity_pack(
          _parent->get_own_system_id(),
          _parent->get_own_component_id(),
          &message,
          static_cast<uint32_t>(_parent->get_time().elapsed_s() * 1e3),
          _parent->get_system_id(),
          _parent->get_autopilot_id(),
          0,
          q,
          down_speed);
      LogDebug() << "Sending message with id: " << message.msgid;
      bool result = _parent->send_message(message);
      LogDebug() << "Sent: " << (bool)result;
  }
4

Are you sure that you replaced all the header files that are used in PX4? Could it be that somehow the existing header files still get included somehow. The CRC table is here:

5

I have replaced the existing header files with my new headers. Let me just tell you how I made the new message so that maybe you can tell me where I went wrong.

  1. I have made a new .xml file which has common.xml included in it and has the custom message in it.
  2. I used the python generator to generate files for this xml.
  3. I then just copied the newly created folders for my custom message, common and standard in PX4.

Does this sound correct?

I am wondering if the CRC for the custom message are not appended to the table in common.h. Do I have to append it manually?

6

Yes that should be correct. And no you should not have to add anything manually.